18 #include "simpletools.h" 22 static void pulse_outCtrAux(
int pin,
int time);
23 static int servoAux_start(
void);
24 static void servoAux(
void *par);
27 static volatile unsigned int servoAuxCog = 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 servoAux_angle(
int pin,
int degreeTenths)
47 return servoAux_set(pin, degreeTenths + 500);
50 int servoAux_speed(
int pin,
int speed)
52 return servoAux_set(pin, speed + 1500);
55 int servoAux_set(
int pin,
int time)
59 int result = servoAux_start();
60 if(result == 0)
return -1;
61 if(result == -1)
return -2;
63 int s =
sizeof(p)/
sizeof(
int);
65 while(lockset(lockID));
66 for(i = 0; i < s; i++)
98 int servoAux_setRamp(
int pin,
int stepSize)
100 int s =
sizeof(p)/
sizeof(
int);
102 while(lockset(lockID));
103 for(i = 0; i < s; i++)
117 int servoAux_get(
int pin)
119 int s =
sizeof(p)/
sizeof(
int);
121 for(i = 0; i < s; i++)
132 static void servoAux(
void *par)
134 int dtpw = (CLKFREQ/1000000)*2500;
137 int dtTw = (CLKFREQ/1000000)*20000;
141 int s =
sizeof(p)/
sizeof(
int);
146 while(lockset(lockID));
147 for(i = 0; i < s; i++)
160 int diff = tPulse - tp[i];
169 tPulse = tp[i] + step;
171 pulse_outCtrAux(p[i], tPulse);
176 while((CNT - pw) <= dtpw);
181 while((CNT - pw) <= dtpw);
187 void servoAux_stop(
void)
191 cogstop(servoAuxCog-1);
208 static int servoAux_start(
void)
211 if(lockID == -1)
return -1;
212 else lockclr(lockID);
214 servoAuxCog = cogstart(servoAux, NULL, stack,
220 static int ta = 0, tb = 0, dta = 0, dtb = 0;
222 static void pulse_outCtrAux(
int pin,
int time)
231 signed long phsVal = -time * st_iodt;
235 int state = get_output(pin);
242 if(ta == 0 || (dta && (CNT - ta > dta)))
253 else if(tb == 0 || (dtb && (CNT - tb > dtb)))