5 volatile int abd_speedLimit[4];
6 volatile int abd_intTabSetup;
7 volatile int abd_dist[2];
8 volatile int abd_ticks[2];
9 volatile int abd_ticksf[2];
10 volatile int abd_ticksi[2];
11 volatile int abd_gotoFlag[2];
12 volatile int abd_dvFlag[2];
13 volatile int abd_rampStep[3];
14 volatile int abd_gotoRampStep[3];
15 volatile int abd_speedLimit[4];
16 volatile int abd_gotoSpeedLimit[4];
17 volatile int abd_ditherA[2];
18 volatile int abd_ditherAa[2];
19 volatile int abd_ditherAd[2];
20 volatile int abd_ditherAp[2];
21 volatile int abd_blockGoto;
22 volatile int abd_ditherV[2];
23 volatile int abd_ditherVa[2];
24 volatile int abd_ditherVd[2];
25 volatile int abd_ditherVp[2];
26 volatile int abd_stopCtr[2];
29 volatile char abd_str[128];
33 void interpolation_table_setup();
34 void set_drive_speed(
int left,
int right);
35 int abd_abs(
int value);
43 if( (abd_dvFlag[ABD_L] == 1) || (abd_dvFlag[ABD_R] == 1))
49 int rampStepSave = abd_rampStep[ABD_B];
50 abd_rampStep[ABD_B] = abd_gotoRampStep[ABD_B];
52 int topSpeedSave = abd_speedLimit[ABD_B];
53 abd_speedLimit[ABD_B] = abd_gotoSpeedLimit[ABD_B];
58 interpolation_table_setup();
59 set_drive_speed(0, 0);
62 abd_dist[ABD_L] = left;
63 abd_dist[ABD_R] = right;
65 for(
int lr = 0; lr <=1; lr++)
68 if(abd_abs(abd_ticksf[lr] - abd_ticks[lr]) < 6)
70 abd_ticksi[lr] = abd_ticksf[lr];
74 abd_ticksi[lr] = abd_ticks[lr];
77 abd_ticksf[lr] = abd_ticksi[lr] + abd_dist[lr];
79 abd_speedLimit[lr] = abd_speedLimit[ABD_B];
80 abd_rampStep[lr] = abd_rampStep[ABD_B];
97 if(lr == 0) {Rp = 1; Lp = 0;}
else {Rp = 0; Lp = 1;}
99 if(abd_abs(abd_dist[Rp]) > abd_abs(abd_dist[Lp]))
101 abd_speedLimit[Lp] = abd_speedLimit[ABD_B] * abd_abs(abd_dist[Lp]) / abd_abs(abd_dist[Rp]);
102 abd_rampStep[Lp] = abd_rampStep[ABD_B] * abd_abs(abd_dist[Lp]) / abd_abs(abd_dist[Rp]);
104 abd_ditherA[Lp] = (abd_rampStep[ABD_B] * abd_dist[Lp]) % abd_abs(abd_dist[Rp]);
105 abd_ditherA[Lp] *= 50;
106 abd_ditherA[Lp] /= abd_abs(abd_dist[Rp]);
107 abd_ditherA[Lp] = abd_abs(abd_ditherA[Lp]);
109 abd_ditherV[Lp] = (abd_speedLimit[ABD_B] * abd_dist[Lp]) % abd_abs(abd_dist[Rp]);
110 abd_ditherV[Lp] *= 50;
111 abd_ditherV[Lp] /= abd_abs(abd_dist[Rp]);
112 abd_ditherV[Lp] = abd_abs(abd_ditherV[Lp]);
117 if(abd_blockGoto == 1)
119 sprint(abd_str,
"\r\rddL%d, spdLimL=%d, rmpStpL=%d dthrAL=%d, dthrVL=%d | "\
120 "ddR%d, spdLimL=%d, rmpStpL=%d dthrAL=%d, dthrVL=%d\r\r",
121 left, abd_speedLimit[ABD_L], abd_rampStep[ABD_L], abd_ditherA[ABD_L], abd_ditherV[ABD_L],
122 right, abd_speedLimit[ABD_R], abd_rampStep[ABD_R], abd_ditherA[ABD_R], abd_ditherV[ABD_R]
127 if(abd_blockGoto == 1)
130 while((abd_gotoFlag[ABD_L] != 0) || (abd_gotoFlag[ABD_R] != 0) || (abd_stopCtr[ABD_L] > 0) || (abd_stopCtr[ABD_R] > 0));
132 while((abd_gotoFlag[ABD_L] != 0) || (abd_gotoFlag[ABD_R] != 0) || (abd_stopCtr[ABD_L] > 0) || (abd_stopCtr[ABD_R] > 0));
135 sprint(abd_str,
"\r\rddL%d, spdLimL=%d, rmpStpL=%d dthrAL=%d, dthrVL=%d | "\
136 "ddR%d, spdLimR=%d, rmpStpR=%d dthrAR=%d, dthrVR=%d\r\r",
137 left, abd_speedLimit[ABD_L], abd_rampStep[ABD_L], abd_ditherA[ABD_L], abd_ditherV[ABD_L],
138 right, abd_speedLimit[ABD_R], abd_rampStep[ABD_R], abd_ditherA[ABD_R], abd_ditherV[ABD_R]
142 abd_rampStep[ABD_B] = rampStepSave;
143 abd_speedLimit[ABD_B] = topSpeedSave;