3 volatile int abd_ticksL;
4 volatile int abd_ticksR;
5 volatile int abd_speedL;
6 volatile int abd_speedR;
9 volatile int abd_zeroDelay;
12 volatile int abd_dlca;
13 volatile int abd_drca;
23 volatile unsigned int _servoPulseReps;
28 int ticksLstart = abd_ticksL;
29 int ticksRstart = abd_ticksR;
30 int ticksLtarget = ticksLstart + left;
31 int ticksRtarget = ticksRstart + right;
32 int distRampL = 0, distRampR = 0, rampClampL = 0, rampClampR = 0, distCruiseL = 0;
42 #ifdef interactive_development_mode 45 xbee = fdserial_open(11, 10, 0, 9600);
49 dprint(xbee,
"---\n");
57 for(
int i = abd_speedL; i > 0; i -= abd_rampStep) distRampL += (i * dt);
59 #ifdef interactive_development_mode 60 dprint(xbee,
"F Before ramp distRampL = %d\n", distRampL);
62 for(
int i = abd_speedL; i <= abd_speedLimit; i += abd_rampStep)
64 if(i > abd_speedLimit) i = abd_speedLimit;
65 distRampL += (i * dt * 2);
73 #ifdef interactive_development_mode 74 dprint(xbee,
"F After ramp distRampL = %d\n", distRampL);
81 for(
int i = abd_speedR; i > 0; i -= abd_rampStep) distRampR += (i * dt);
83 #ifdef interactive_development_mode 84 dprint(xbee,
"F Before ramp distRampR = %d\n", distRampR);
87 for(
int i = abd_speedR; i <= abd_speedLimit; i += abd_rampStep)
89 if(i > abd_speedLimit) i = abd_speedLimit;
90 distRampR += (i * dt * 2);
92 if(right <= distRampR)
98 #ifdef interactive_development_mode 99 dprint(xbee,
"F After ramp distRampR = %d\n", distRampR);
119 for(
int i = abd_speedL; i < 0; i += abd_rampStep) distRampL -= (i * dt);
121 #ifdef interactive_development_mode 122 dprint(xbee,
"B Before ramp distRampL = %d\n", distRampL);
124 for(
int i = abd_speedL; i >= -abd_speedLimit; i -= abd_rampStep)
126 if(i < -abd_speedLimit) i = -abd_speedLimit;
127 distRampL += (i * dt * 2);
129 if(left >= distRampL)
135 #ifdef interactive_development_mode 136 dprint(xbee,
"B After ramp distRampL = %d\n", distRampL);
145 for(
int i = abd_speedR; i < 0; i += abd_rampStep) distRampR -= (i * dt);
147 #ifdef interactive_development_mode 148 dprint(xbee,
"B Before ramp distRampR = %d\n", distRampR);
151 for(
int i = abd_speedR; i >= -abd_speedLimit; i -= abd_rampStep)
153 if(i < -abd_speedLimit) i = -abd_speedLimit;
154 distRampR += (i * dt * 2);
156 if(right >= distRampR)
162 #ifdef interactive_development_mode 163 dprint(xbee,
"B After ramp distRampR = %d\n", distRampR);
174 distCruiseL = left - distRampL;
178 int tCruiseL = distCruiseL / rampClampL;
182 #ifdef interactive_development_mode 183 dprint(xbee,
"distRampL = %d\n", distRampL);
184 dprint(xbee,
"distCruiseL = %d\n", distCruiseL);
185 dprint(xbee,
"distCruiseL + distRampL = %d\n", distCruiseL + distRampL);
186 dprint(xbee,
"rampClampL = %d\n\n", rampClampL);
188 dprint(xbee,
"distRampR = %d\n", distRampR);
189 dprint(xbee,
"distCruiseR = %d\n", distCruiseR);
190 dprint(xbee,
"distCruiseR + distRampR = %d\n", distCruiseR + distRampR);
191 dprint(xbee,
"rampClampR = %d\n", rampClampR);
199 if(rampClampL < 0 && tCruiseL > 0) pause(tCruiseL);
200 if(rampClampL > 0 && tCruiseL > 0) pause(tCruiseL);
210 #ifdef interactive_development_mode 211 dprint(xbee,
"tCruiseL = %d\n\n", tCruiseL);
221 abd_dlc = abd_ticksL;
222 abd_dlca = abd_dlc * abd_dsr;
226 abd_drc = abd_ticksR;
227 abd_drca = abd_drc * abd_dsr;
232 #ifdef interactive_development_mode 233 dprint(xbee,
"ticksL, ticksR before correction: %d, %d\n", ticksL, ticksR);
237 int tempL = 0, tempR = 0;
238 int tempTicks = _servoPulseReps;
241 while(tempTicks >= _servoPulseReps);
243 if(abd_ticksL > ticksLtarget)
246 if(abd_iL > 0) abd_iL = 0;
248 else if (abd_ticksL < ticksLtarget)
251 if(abd_iL < 0) abd_iL = 0;
259 if(abd_ticksR > ticksRtarget)
262 if(abd_iR > 0) abd_iR = 0;
264 else if (abd_ticksR < ticksRtarget)
267 if(abd_iR < 0) abd_iR = 0;
276 if((abd_ticksL == ticksLtarget) && (abd_ticksR == ticksRtarget))
284 if(reps == 10)
break;
287 #ifdef interactive_development_mode 288 dprint(xbee,
"ticksL, ticksR after correction: %d, %d\n", ticksL, ticksR);
291 abd_dlc = abd_ticksL;
292 abd_dlca = abd_dlc * abd_dsr;
296 abd_drc = abd_ticksR;
297 abd_drca = abd_drc * abd_dsr;
This library takes care of encoder monitoring and servo signalling, and provides a simple set of func...
void drive_ramp(int left, int right)
Ramp up to the specified wheel speeds. It works almost the same as drive_speed, except that it steps ...
void drive_goto(int distLeft, int distRight)
Make each wheel go a particular distance. Recommended for straight forward, backward, turns and pivots. Not recommended for curves. This function ramps up to full speed if the distance is long enough. It holds that speed until it needs to ramp down. After ramping down it applies compensation. This function is primarily a convenience for dead reckoning, and does not return until the maneuver has completed.
#define ON
ON can be used in place of a nonzero value to enabled parameters in functions like drive_feedback and...
void drive_speed(int left, int right)
Set wheel speeds in encoder ticks per second. An encoder tick is 1/64th of a revolution, and makes causes the wheel to roll 3.25 mm.
#define OFF
OFF can be used in place of zero to enabled parameters in functions like drive_feedback and drive_tri...