16 void cal_drive_pins(
int servoPinLeft,
int servoPinRight,
int encPinLeft,
int encPinright);
17 void cal_drive_speeds(
int left,
int right);
18 void cal_drive_setramp(
int left,
int right);
19 void cal_drive_sleep();
20 void cal_drive_stop();
21 void cal_drive_display(
void);
24 static int ticksRprev;
40 static volatile int sPinL = 12, sPinR = 13, rampLeft, rampRight;
41 static volatile int ePinL = 14, ePinR = 15;
47 #ifdef xbee_abcalibrate_debug
56 cal_drive_pins(sPinL, sPinR, ePinL, ePinR);
77 int ticksLprev = ticksL;
83 #ifdef xbee_abcalibrate_debug
84 dprint(xbee,
"a = %d, speed = %d, ",
88 cal_drive_speeds(speed, 0);
93 for(
int i = 0; i < 10; i++)
95 while(ticksL <= ticksLprev + 1)
103 if((tcL == 0)&&(i>0))
break;
109 if((tcL < 20)&&(step == 5))
115 if(tcL > 20) step = 5;
118 if(tcLav > 210 || tcLav < 0)
goto TryAgainLeft;
121 #ifdef xbee_abcalibrate_debug
122 dprint(xbee,
"ticksL = %d, tcL = %d, \n",
126 }
while(speed < 200);
142 cal_drive_speeds(0, 0);
156 #ifdef xbee_abcalibrate_debug
158 for(r = 0; r < a; r++)
160 dprint(xbee,
"r = %d, spdr = %d, spdm = %d, \n", r, spdr[r], spdm[r]);
165 for(r = 1; r < a-1; r++)
167 if((spdm[r] > spdm[r-1] + 30) && (spdm[r] > spdm[r+1] + 30)) spdm[r] = (spdm[r+1] + spdm[r-1])/2;
172 for(r = 1; r < a-1; r++)
174 if((spdm[r] == 0) && (spdm[r+1] != 0) && (spdm[r-1] != 0)) spdm[r] = (spdm[r+1] + spdm[r-1])/2;
177 #ifdef xbee_abcalibrate_debug
179 for(r = 0; r < a; r++)
181 dprint(xbee,
"r = %d, spdr = %d, spdm = %d, \n", r, spdr[r], spdm[r]);
189 #ifdef xbee_abcalibrate_debug
190 dprint(xbee,
"r = %d\n", r);
197 for(r = 0; r < a; r++)
199 if((spdm[r]==0)&&(zstart==0)) zstart = r;
200 if((zstart!=0)&&(spdm[r]!=0)&&(zend==0)) zend = r;
203 #ifdef xbee_abcalibrate_debug
204 dprint(xbee,
"zstart = %d, zend = %d\n", zstart, zend);
210 for(r = 1; r < lcnt-1; r++)
212 if((spdm[r] < 0)||(spdm[r] > 200)) spdm[r] = (spdm[r+1] + spdm[r-1])/2;
215 spdr[zstart] = (spdr[zstart] + spdr[zend - 1])/2;
217 int subval = zend - zstart - 1;
219 for(r = zend; r < lcnt; r++)
221 spdr[r - subval] = spdr[r];
222 spdm[r - subval] = spdm[r];
227 #ifdef xbee_abcalibrate_debug
228 for(r = 0; r < a; r++)
230 dprint(xbee,
"r = %d, spdr = %d, spdm = %d, \n", r, spdr[r], spdm[r]);
236 #ifdef xbee_abcalibrate_debug
237 dprint(xbee,
"lcnt = %d\n", lcnt);
238 dprint(xbee,
"zstart = %d\n", zstart);
243 #ifdef xbee_abcalibrate_debug
244 print(
"left eeAddr = %d\n", eeAddr);
251 for(r = 0; r < lcnt; r++)
300 #ifdef xbee_abcalibrate_debug
301 print(
"left eeAddr = %d\n", eeAddr);
324 for(r = 0; r < lcnt; r++)
333 #ifdef xbee_abcalibrate_debug
334 for(r = 0; r < lcnt; r++)
336 dprint(xbee,
"r = %d, spdr = %d, spdm = %d, \n", r, spdr[r], spdm[r]);
338 dprint(xbee,
"\n\nDONE!!!\n\n");
365 #ifdef xbee_abcalibrate_debug
366 dprint(xbee,
"a = %d, speed = %d, ",
370 cal_drive_speeds(0, speed);
375 for(
int i = 0; i < 10; i++)
377 while(ticksR <= ticksRprev + 1)
385 if((tcR == 0)&&(i>0))
break;
391 if((tcR < 20)&&(step == 5))
397 if(tcR > 20) step = 5;
400 if(tcRav > 210 || tcRav < 0)
goto TryAgainRight;
403 #ifdef xbee_abcalibrate_debug
404 dprint(xbee,
"ticksR = %d, tcR = %d, \n",
409 }
while(speed < 200);
430 #ifdef xbee_abcalibrate_debug
432 for(r = 0; r < a; r++)
434 dprint(xbee,
"r = %d, spdr = %d, spdm = %d, \n", r, spdr[r], spdm[r]);
439 for(r = 1; r < a-1; r++)
441 if((spdm[r] > spdm[r-1] + 30) && (spdm[r] > spdm[r+1] + 30)) spdm[r] = (spdm[r+1] + spdm[r-1])/2;
446 for(r = 1; r < a-1; r++)
448 if((spdm[r] == 0) && (spdm[r+1] != 0) && (spdm[r-1] != 0)) spdm[r] = (spdm[r+1] + spdm[r-1])/2;
451 #ifdef xbee_abcalibrate_debug
453 for(r = 0; r < a; r++)
455 dprint(xbee,
"r = %d, spdr = %d, spdm = %d, \n", r, spdr[r], spdm[r]);
468 #ifdef xbee_abcalibrate_debug
469 dprint(xbee,
"r = %d\n", r);
476 for(r = 0; r < a; r++)
478 if((spdm[r]==0)&&(zstart==0)) zstart = r;
479 if((zstart!=0)&&(spdm[r]!=0)&&(zend==0)) zend = r;
482 #ifdef xbee_abcalibrate_debug
483 dprint(xbee,
"zstart = %d, zend = %d\n", zstart, zend);
488 for(r = 1; r < rcnt-1; r++)
490 if((spdm[r] < 0)||(spdm[r] > 200)) spdm[r] = (spdm[r+1] + spdm[r-1])/2;
496 spdr[zstart] = (spdr[zstart] + spdr[zend - 1])/2;
498 subval = zend - zstart - 1;
500 for(r = zend; r < rcnt; r++)
502 spdr[r - subval] = spdr[r];
503 spdm[r - subval] = spdm[r];
508 #ifdef xbee_abcalibrate_debug
509 for(r = 0; r < a; r++)
511 dprint(xbee,
"r = %d, spdr = %d, spdm = %d, \n", r, spdr[r], spdm[r]);
517 #ifdef xbee_abcalibrate_debug
518 dprint(xbee,
"rcnt = %d\n", rcnt);
519 dprint(xbee,
"zstart = %d\n", zstart);
527 for(r = 0; r < rcnt; r++)
563 #ifdef xbee_abcalibrate_debug
564 print(
"right eeAddr = %d\n", eeAddr);
574 for(r = 0; r < rcnt; r++)
595 for(r = 0; r < 120; r++)
604 for(r = 0; r < rcnt; r++)
616 #ifdef xbee_abcalibrate_debug
617 for(r = 0; r < rcnt; r++)
619 dprint(xbee,
"r = %d, spdr = %d, spdm = %d, \n", r, spdr[r], spdm[r]);
623 dprint(xbee,
"\n\nDONE!!!\n\n");
635 static int stack[(160 + (400 * 4)) / 4];
639 static volatile int tiL;
640 static volatile int* sspAddrL;
641 static volatile int* sspAddrR;
642 static volatile int* ssrAddrL;
643 static volatile int* ssrAddrR;
644 volatile int ticksL = 0;
645 volatile int ticksR = 0;
646 static volatile int speedL;
647 static volatile int speedR;
648 static volatile int tfL;
649 static volatile int cL;
650 static volatile int tR;
651 static volatile int tiR;
652 static volatile int tfR;
653 static volatile int cR;
654 static volatile int stateL;
655 static volatile int stateR;
656 static volatile int dirL;
657 static volatile int ssReqL;
659 void cal_encoders(
void *par);
661 void cal_drive_pins(
int servoPinLeft,
int servoPinRight,
int encPinLeft,
int encPinRight)
663 sPinL = servoPinLeft;
664 sPinR = servoPinRight;
669 void cal_drive_speeds(
int left,
int right)
688 cog = 1 +
cogstart(cal_encoders, NULL, stack,
sizeof(stack));
693 void cal_drive_display(
void)
695 static int ticksLprev;
696 static int ticksRprev;
703 void cal_drive_setramp(
int left,
int right)
709 void cal_drive_sleep()
715 void cal_drive_stop()
721 void cal_encoders(
void *par)
723 stateL = (
INA >> ePinL) & 1;
724 stateR = (
INA >> ePinR) & 1;
736 if(((
INA >> ePinL) & 1) != stateL)
738 stateL = (~stateL) & 1;
751 if(((
INA >> ePinR) & 1) != stateR)
753 stateR = (~stateR) & 1;
775 sPinL = servoPinLeft;
776 sPinR = servoPinRight;
779 unsigned char pinInfo[8] = {
's',
'p',
'L', 12,
' ',
'R', 13,
' '};
780 pinInfo[3] = (
char) servoPinLeft;
781 pinInfo[6] = (
char) servoPinRight;
793 unsigned char pinInfo[8] = {
'e',
'p',
'L', 14,
' ',
'R', 15,
' '};
794 pinInfo[3] = (
char) encPinLeft;
795 pinInfo[6] = (
char) encPinRight;