5 volatile int abd_speedd[2];
6 volatile int abd_speedi[2];
7 volatile int abd_speedT[2];
8 volatile int abd_speed[2];
9 volatile int abd_speedLimit[4];
11 volatile int abd_rampStep[3];
12 volatile int abd_stopCtr[2];
13 volatile unsigned int _servoPulseReps;
14 volatile int speedPrev[2];
18 volatile int abd_dvFlag[2];
21 volatile char abd_str[128];
26 unsigned int abd_stack[44 + 128];
28 volatile int abd_ditherA[2];
29 volatile int abd_ditherAa[2];
30 volatile int abd_ditherAd[2];
31 volatile int abd_ditherAp[2];
33 volatile int abd_ditherV[2];
34 volatile int abd_ditherVa[2];
35 volatile int abd_ditherVd[2];
36 volatile int abd_ditherVp[2];
38 volatile int abd_gotoFlag[2];
39 volatile int abd_intTabSetup;
42 void abd_encoders(
void *par);
43 void interpolation_table_setup(
void);
44 int abd_abs(
int value);
47 void set_drive_speed(
int left,
int right)
61 abd_speedT[ABD_L] = left;
62 abd_speedT[ABD_R] = right;
64 abd_speedi[ABD_L] = abd_speed[ABD_L];
65 abd_speedi[ABD_R] = abd_speed[ABD_R];
66 abd_speedd[ABD_L] = abd_speedT[ABD_L] - abd_speedi[ABD_L];
67 abd_speedd[ABD_R] = abd_speedT[ABD_R] - abd_speedi[ABD_R];
69 for(
int lr = ABD_L; lr <= ABD_R; lr++)
85 abd_speedLimit[lr] = abd_speedLimit[ABD_B];
86 abd_rampStep[lr] = abd_rampStep[ABD_B];
90 if(lr == 0) {Rp = 1; Lp = 0;}
else {Rp = 0; Lp = 1;}
93 if(abd_abs(abd_speedd[Rp]) > abd_abs(abd_speedd[Lp]))
98 abd_rampStep[Lp] = abd_rampStep[ABD_B] * abd_abs(abd_speedd[Lp]) / abd_abs(abd_speedd[Rp]);
100 abd_ditherA[Lp] = (abd_rampStep[ABD_B] * abd_speedd[Lp]) % abd_abs(abd_speedd[Rp]);
101 abd_ditherA[Lp] *= 50;
102 abd_ditherA[Lp] /= abd_abs(abd_speedd[Rp]);
103 abd_ditherA[Lp] = abd_abs(abd_ditherA[Lp]);
109 abd_str,
"\rvL%d, rspdLimL=%d, rmpStpL=%d dthrAL=%d, dthrVL=%d | "\
110 "vR%d, spdLimR=%d, rmpStpR=%d dthrAR=%d, dthrVR=%d\r\r",
111 abd_speedT[ABD_L], abd_speedLimit[ABD_L], abd_rampStep[ABD_L], abd_ditherA[ABD_L], abd_ditherV[ABD_L],
112 abd_speedT[ABD_R], abd_speedLimit[ABD_R], abd_rampStep[ABD_R], abd_ditherA[ABD_R], abd_ditherV[ABD_R]
119 abd_cog = 1 +
cogstart(abd_encoders, NULL, abd_stack,
sizeof(abd_stack)-1);
123 if(abd_stopCtr[ABD_L] || abd_stopCtr[ABD_R])
125 while(abd_stopCtr[ABD_L] || abd_stopCtr[ABD_R]);
128 int n = _servoPulseReps + 6;
129 while(_servoPulseReps != n);
130 if(abd_stopCtr[ABD_L] || abd_stopCtr[ABD_R])
132 while(abd_stopCtr[ABD_L] || abd_stopCtr[ABD_R]);
142 interpolation_table_setup();
143 set_drive_speed(0, 0);
146 if((left != speedPrev[ABD_L]) || (right != speedPrev[ABD_R]))
149 set_drive_speed(left, right);
153 speedPrev[ABD_L] = abd_speedT[ABD_L];
154 speedPrev[ABD_R] = abd_speedT[ABD_R];