18 #include "simpletools.h" 22 static void pulse_outCtr(
int pin,
int time);
23 static int servo_start(
void);
24 static void servo(
void *par);
27 static volatile unsigned int servoCog = 0;
28 static volatile unsigned int lockID;
29 static unsigned int stack[44 + 24];
31 static volatile int p[14] = {-1, -1, -1, -1,
32 -1, -1, -1, -1, -1, -1,
34 static volatile int t[14] = {-1, -1, -1, -1,
35 -1, -1, -1, -1, -1, -1,
37 static volatile int tp[14] = {-1, -1, -1, -1,
38 -1, -1, -1, -1, -1, -1,
40 static volatile int r[14] = {2000, 2000, 2000, 2000,
41 2000, 2000, 2000, 2000, 2000,
42 2000, 2000, 2000, 2000, 2000};
45 int servo_angle(
int pin,
int degreeTenths)
47 return servo_set(pin, degreeTenths + 500);
51 int servo_speed(
int pin,
int speed)
53 return servo_set(pin, speed + 1500);
57 int servo_set(
int pin,
int time)
61 int result = servo_start();
62 if(result == 0)
return -1;
63 if(result == -1)
return -2;
65 int s =
sizeof(p)/
sizeof(
int);
67 while(lockset(lockID));
68 for(i = 0; i < s; i++)
100 int servo_setramp(
int pin,
int stepSize)
102 int s =
sizeof(p)/
sizeof(
int);
104 while(lockset(lockID));
105 for(i = 0; i < s; i++)
119 int servo_get(
int pin)
121 int s =
sizeof(p)/
sizeof(
int);
123 for(i = 0; i < s; i++)
134 int servo_disable(
int pin)
138 int result = servo_set(pin, 0);
143 static void servo(
void *par)
145 int dtpw = (CLKFREQ/1000000)*2500;
148 int dtTw = (CLKFREQ/1000000)*20000;
152 int s =
sizeof(p)/
sizeof(
int);
156 while(lockset(lockID));
157 for(i = 0; i < s; i++)
170 int diff = tPulse - tp[i];
179 tPulse = tp[i] + step;
181 pulse_outCtr(p[i], tPulse);
186 while((CNT - pw) <= dtpw);
191 while((CNT - pw) <= dtpw);
197 void servo_stop(
void)
219 static int servo_start(
void)
222 if(lockID == -1)
return -1;
223 else lockclr(lockID);
225 servoCog = cogstart(servo, NULL, stack,
231 static int ta = 0, tb = 0, dta = 0, dtb = 0;
233 static void pulse_outCtr(
int pin,
int time)
242 signed long phsVal = -time * st_iodt;
246 int state = get_output(pin);
253 if(ta == 0 || (dta && (CNT - ta > dta)))
264 else if(tb == 0 || (dtb && (CNT - tb > dtb)))