7 #include "simpletools.h" 8 #include "simpletext.h" 19 void drive_com(
int arrayLcnt,
int arrayRcnt,
20 int centerL,
int centerR,
21 int* pwAddrL,
int* pwAddrR,
22 int* spdAddrL,
int* spdAddrR);
23 void drive_set(
int left,
int right);
24 void encoders(
void *par);
25 void interpolate(
int* ltmp,
int* rtmp);
26 void interpolation_table_setup();
27 void servos_diffDrive(
void);
28 void drive_record(
int startStop);
29 void drive_displayControlSystem(
int start,
int end);
30 void set_drive_speed(
int left,
int right);
34 volatile int abd_trimFL, abd_trimFR, abd_trimBL, abd_trimBR, abd_trimticksF, abd_trimticksB;
35 volatile int abd_trimticksF = 0;
36 volatile int abd_trimticksB = 0;
40 volatile int abd_ticksL = 0;
41 volatile int abd_ticksR = 0;
42 volatile int abd_speedL;
43 volatile int abd_speedR;
45 int abd_speedLimit = 128;
46 volatile int abd_zeroDelay =
ON;
49 volatile int abd_dlca = 0;
50 volatile int abd_drca = 0;
51 volatile int abd_dsr = 400;
58 volatile int abd_eaL = 0;
59 volatile int abd_eaR = 0;
60 volatile unsigned int _servoPulseReps;
63 volatile int abd_sPinL = 12, abd_sPinR = 13;
64 volatile int abd_ePinL = 14, abd_ePinR = 15;
66 int abd_intTabSetup = 0;
69 volatile int abd_record = 0;
72 volatile int abd_elCntL;
73 volatile int abd_elCntR;
74 volatile int abd_cntrLidx;
75 volatile int abd_cntrRidx;
81 static volatile int cntrLval;
82 static volatile int cntrRval;
86 static unsigned int stack[44 + 52];
96 static volatile int trimctr = 0;
97 static volatile int dca, trimticks;
99 static volatile int leftPrev, rightPrev;
101 static volatile int kp[6];
103 static volatile int tcL;
104 static volatile int tcR;
105 static volatile int tiL;
108 static volatile int ridx = 0;
111 static volatile int tiR;
113 static volatile int stateL;
114 static volatile int stateR;
116 static volatile int* pwL;
117 static volatile int* pwR;
118 static volatile int* spdL;
119 static volatile int* spdR;
121 static volatile int etpsR;
122 static volatile int etpsL;
124 static volatile int pcount;
125 static volatile unsigned int _sprOld;
126 static volatile unsigned int _sprNext;
129 static volatile int ssiL;
130 static volatile int ssiR;
132 static volatile int driveL;
133 static volatile int driveR;
135 static volatile int phsL;
136 static volatile int phsR;
137 static volatile int phsLr;
138 static volatile int phsRr;
140 static int trimFunction = 1;
141 static int encoderFeedback = 1;
143 static int speedLprev = 0, speedRprev = 0;
152 interpolation_table_setup();
153 set_drive_speed(0, 0);
169 abd_speedLimit = maxTicksPerSec;
175 abd_rampStep = stepsize;
181 encoderFeedback = enabled;
187 trimFunction = enabled;
191 void drive_com(
int arrayLcnt,
int arrayRcnt,
192 int centerL,
int centerR,
193 int* pwAddrL,
int* pwAddrR,
194 int* spdAddrL,
int* spdAddrR)
196 abd_elCntL = arrayLcnt;
197 abd_elCntR = arrayRcnt;
198 abd_cntrLidx = centerL;
199 abd_cntrRidx = centerR;
204 cntrLval = pwAddrL[abd_cntrLidx];
205 cntrRval = pwAddrR[abd_cntrRidx];
209 void interpolation_table_setup()
211 if(!abd_us) abd_us = CLKFREQ/1000000;
216 unsigned char str[12];
231 int cntL = ee_getInt(abd_eeAddr);
233 int zstartL = ee_getInt(abd_eeAddr);
235 for(r = 0; r < cntL; r++)
237 abd_spdrL[r] = ee_getInt(abd_eeAddr);
239 abd_spdmL[r] = ee_getInt(abd_eeAddr);
242 abd_spdmL[cntL - 1] = 1000;
247 int cntR = ee_getInt(abd_eeAddr);
249 int zstartR = ee_getInt(abd_eeAddr);
251 for(r = 0; r < cntR; r++)
253 abd_spdrR[r] = ee_getInt(abd_eeAddr);
255 abd_spdmR[r] = ee_getInt(abd_eeAddr);
258 abd_spdmR[cntR - 1] = 1000;
261 drive_com(cntL, cntR, zstartL, zstartR, abd_spdrL, abd_spdrR, abd_spdmL, abd_spdmR);
265 abd_trimFL = ee_getInt(abd_eeAddr + 0);
266 abd_trimFR = ee_getInt(abd_eeAddr + 4);
267 abd_trimBL = ee_getInt(abd_eeAddr + 8);
268 abd_trimBR = ee_getInt(abd_eeAddr + 12);
269 abd_trimticksF = ee_getInt(abd_eeAddr + 16);
270 abd_trimticksB = ee_getInt(abd_eeAddr + 20);
273 unsigned char pinInfo[16];
275 for(
int i = 0; i < 16; i++)
276 pinInfo[i] = ee_getByte(eeAddr + i);
278 if(pinInfo[0] ==
's' && pinInfo[1] ==
'p' && pinInfo[2] ==
'L' && pinInfo[5] ==
'R')
280 abd_sPinL = (int) pinInfo[3];
281 abd_sPinR = (int) pinInfo[6];
284 if(pinInfo[8] ==
'e' && pinInfo[9] ==
'p' && pinInfo[10] ==
'L' && pinInfo[13] ==
'R')
286 abd_ePinL = (int) pinInfo[11];
287 abd_ePinR = (int) pinInfo[14];
297 void interpolate(
int *ltmp,
int *rtmp)
322 int rprev = abd_cntrLidx;
324 for(
int r = abd_cntrLidx; r != limit; r+=listep)
326 if(spdL[r] == lookupval)
331 if((spdL[rprev] < lookupval) && (spdL[r] > lookupval))
333 int x = ((pwL[r]-pwL[rprev])*(lookupval-spdL[rprev]))/(spdL[r]-spdL[rprev]);
334 left = pwL[rprev] + x;
357 rprev = abd_cntrRidx;
359 for(
int r = abd_cntrRidx; r != limit; r+=listep)
361 if(spdR[r] == lookupval)
366 if((spdR[rprev] < lookupval) && (spdR[r] > lookupval))
368 int x = ((pwR[r]-pwR[rprev])*(lookupval-spdR[rprev]))/(spdR[r]-spdR[rprev]);
369 right = pwR[rprev] + x;
384 void set_drive_speed(
int left,
int right)
389 if(left > abd_speedLimit) left = abd_speedLimit;
390 if(left < -abd_speedLimit) left = -abd_speedLimit;
391 if(right > abd_speedLimit) right = abd_speedLimit;
392 if(right < -abd_speedLimit) right = -abd_speedLimit;
396 int rightTemp = right;
398 interpolate(&leftTemp, &rightTemp);
412 cog = 1 + cogstart(encoders, NULL, stack,
sizeof(stack)-1);
421 interpolation_table_setup();
422 set_drive_speed(0, 0);
427 if(abd_zeroDelay ==
ON)
429 if((speedLprev > 0 && left <= 0) || (speedLprev < 0 && left >= 0) || (speedRprev > 0 && right <= 0) || (speedRprev < 0 && right >= 0))
431 int tempLeftZ = left;
432 int tempRightZ = right;
433 if((speedLprev > 0 && left <= 0) || (speedLprev < 0 && left >= 0))
437 if((speedRprev > 0 && right <= 0) || (speedRprev < 0 && right >= 0))
441 set_drive_speed(tempLeftZ, tempRightZ);
442 speedLprev = tempLeftZ;
443 speedRprev = tempRightZ;
451 set_drive_speed(left, right);
453 speedLprev = abd_speedL;
454 speedRprev = abd_speedR;
458 void encoders(
void *par)
464 OUTA &= ~(1 << abd_sPinL);
465 OUTA &= ~(1 << abd_sPinR);
466 DIRA |= 1 << abd_sPinL;
467 DIRA |= 1 << abd_sPinR;
474 interpolate(&tempL, &tempR);
480 CTRA = abd_sPinL | (4 << 26);
481 CTRB = abd_sPinR | (4 << 26);
483 phsL = (1500 + tempL);
484 phsR = (1500 - tempR);
489 int dt1 = 13*(CLKFREQ/1000);
490 int dt2 = 7*(CLKFREQ/1000);
520 stateL = (INA >> abd_ePinL) & 1;
521 stateR = (INA >> abd_ePinR) & 1;
523 while(!_servoPulseReps);
527 int tdst = CLKFREQ/abd_dsr;
530 _sprOld = _servoPulseReps;
547 if(((INA >> abd_ePinL) & 1) != stateL)
549 stateL = (~stateL) & 1;
552 if((CNT - tiL) > (CLKFREQ/400))
557 if(phsL > cntrLval + 1500)
562 else if(phsL < cntrLval + 1500)
574 if(((INA >> abd_ePinR) & 1) != stateR)
576 stateR = (~stateR) & 1;
579 if((CNT - tiR) > (CLKFREQ/400))
584 if(phsR < 1500 - cntrRval)
589 else if(phsR > 1500 - cntrRval)
601 if((td - CNT) > tdst)
611 abd_dlc = abd_dlca/abd_dsr;
612 abd_drc = abd_drca/abd_dsr;
618 if((((abd_speedL > 0)&&abd_trimFL))||((abd_speedR > 0)&&(abd_trimFR)))
620 trimticks = abd_trimticksF;
621 dca = abd_dlca*abd_trimFL + abd_drca*abd_trimFR;
623 else if((((abd_speedL < 0)&&abd_trimBL))||((abd_speedR < 0)&&(abd_trimBR)))
625 trimticks = abd_trimticksB;
626 dca = abd_dlca*abd_trimBL + abd_drca*abd_trimBR;
628 if((((abd_speedL > 0)&&abd_trimFL))||((abd_speedR > 0)&&(abd_trimFR)))
632 diff = dca - trimAccum;
633 inc = diff/trimticks;
636 trimAccum += (inc*trimticks);
638 abd_dlca += (abd_trimFL*inc);
639 abd_drca += (abd_trimFR*inc);
641 else if((((abd_speedL < 0)&&abd_trimBL))||((abd_speedR < 0)&&(abd_trimBR)))
645 diff = dca - trimAccum;
646 inc = diff/trimticks;
649 trimAccum += (inc*trimticks);
651 abd_dlca += (abd_trimBL*inc);
652 abd_drca += (abd_trimBR*inc);
671 if((CNT - t) >= (dt1 + dt2))
676 _sprOld = _servoPulseReps;
683 abd_edL = abd_dlc - abd_ticksL;
690 abd_pL = abd_edL * (3+(abd_speedL/10));
691 if(abd_edL>0)abd_iL+=1;
else if(abd_edL<0) abd_iL-=1;
693 else if(abd_speedL < 0)
695 abd_pL = abd_edL * (-3+(abd_speedL/10));
696 if(abd_edL>0)abd_iL-=1;
else if(abd_edL<0) abd_iL+=1;
699 if(maxIL < 0) maxIL = -maxIL;
700 if(abd_iL > maxIL) abd_iL = maxIL;
701 if(abd_iL < -maxIL) abd_iL = -maxIL;
704 driveL = abd_iL + abd_pL + ssiL + 1500;
707 driveL = -abd_iL - abd_pL + ssiL + 1500;
712 driveL = ssiL + 1500;
715 abd_edR = abd_drc - abd_ticksR;
722 abd_pR = abd_edR * (3+(abd_speedR/10));
723 if(abd_edR>0)abd_iR+=1;
else if(abd_edR<0) abd_iR-=1;
725 else if(abd_speedR < 0)
727 abd_pR = abd_edR * (-3+(abd_speedR/10));
728 if(abd_edR>0)abd_iR-=1;
else if(abd_edR<0) abd_iR+=1;
731 if(maxIR < 0) maxIR = - maxIR;
732 if(abd_iR > maxIR) abd_iR = maxIR;
733 if(abd_iR < -maxIR) abd_iR = -maxIR;
736 driveR = -abd_iR - abd_pR + ssiR + 1500;
739 driveR = abd_iR + abd_pR + ssiR + 1500;
744 driveR = ssiR + 1500;
770 #ifdef interactive_development_mode 789 #endif // interactive_development_mode #define _ActivityBot_EE_Start_
ActivityBot EEPROM calibration data start address.
This library takes care of encoder monitoring and servo signalling, and provides a simple set of func...
void drive_close(void)
Stop the servo/encoder system. This is useful for reclaiming a processor for other tasks...
#define ON
ON can be used in place of a nonzero value to enabled parameters in functions like drive_feedback and...
void drive_setMaxSpeed(int speed)
Modifies the default maxiumum top speed for use with encoders. The default is 128 ticks/second = 2 re...
void drive_feedback(int enabled)
Enables or disables encoder feedback for speed control.
int drive_open()
Start or restart the servo/encoder system.
void drive_setRampStep(int stepsize)
Overrides the default 4 ticks/second per 50th of a second for ramping.
void drive_trim(int enabled)
Enables or disables drive trim which can be used to compensate for mechanical wheel alignment errors...
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.