#include <htc.h>
#include "delay.h"
#include "lcd.h"
#define Servo0 RD0 // master servo
#define Servo1 RD1 // joint11
#define Servo2 RD2 // joint2
#define Servo3 RD3 // joint3
#define Servo4 RD4 // tong
#define Servo5 RD5
int i;
int time=0,division=0,difference=0;
int delayTime=0,delayDivision=0, delayDifference=0;
void initialPos(void)
{
PORTD=0xFF;
DelayUs(250);DelayUs(250);DelayUs(250);DelayUs(250);
DelayUs(250);DelayUs(250);
PORTD=0x00;
DelayMs(18);
DelayUs(250);DelayUs(250);
}
void servoExecute(char servo, char direction, char angle)
{
time=angle*5;
division=time/250;
difference=time-division*250;
delayTime=500-time;
delayDivision=delayTime/250;
delayDifference=delayTime%250;
Servo0=1;
DelayUs(250);DelayUs(250);DelayUs(250);DelayUs(250);
if(direction){
DelayUs(250);DelayUs(250);
DelayUs(division*250);DelayUs(difference);
Servo0=0;
DelayMs(18);
DelayUs(delayDivision*250);DelayUs(delayDifference);
}else{
Servo0=0;
DelayUs(division*250);DelayUs(difference);
DelayMs(18);DelayUs(250);DelayUs(250);
DelayUs(delayDivision*250);DelayUs(delayDifference);
}
}
void main(void)
{
TRISB=0x00;
TRISD=0x00;
PORTD=0x00;
ADCON1=0x07;
TRISC=0x00;
DelayUs(250);DelayUs(250);DelayUs(250);DelayUs(250);
initialPos();
DelayUs(250);DelayUs(250);DelayUs(250);DelayUs(250);
lcd_init();
while(1)
{
servoExecute(0,1,30);
lcd_gotoxy(1,1);
veri_yolla(division%10+48);
lcd_gotoxy(2,1);
/* veri_yolla(difference/100+48);
veri_yolla((difference%100)/10+48);
veri_yolla(difference%10+48);*/
veri_yolla(time/100+48);
veri_yolla((time%100)/10+48);
veri_yolla(time%10+48);
}
}