3 #include "simpletools.h" 4 #include "simpletext.h" 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 48 fdserial *xbee = fdserial_open(11, 10, 0, 9600);
55 freqout(4, 2000, 3000);
56 cal_drive_pins(sPinL, sPinR, ePinL, ePinR);
59 ee_putInt(0, eeAddr + 0);
60 ee_putInt(0, eeAddr + 4);
61 ee_putInt(0, eeAddr + 8);
62 ee_putInt(0, eeAddr + 12);
63 ee_putInt(0, eeAddr + 16);
64 ee_putInt(0, eeAddr + 20);
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 157 dprint(xbee,
"Before\n");
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 178 dprint(xbee,
"After\n");
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);
247 ee_putInt(lcnt, eeAddr);
249 ee_putInt(zstart, eeAddr);
251 for(r = 0; r < lcnt; r++)
255 ee_putByte((
char) spdr[r], eeAddr);
258 ee_putByte((
char) (spdr[r] >> 8), eeAddr);
261 ee_putByte((
char) (spdr[r] >> 16), eeAddr);
264 ee_putByte((
char) (spdr[r] >> 24), eeAddr);
271 ee_putByte((
char) spdm[r], eeAddr);
274 ee_putByte((
char) (spdm[r] >> 8), eeAddr);
277 ee_putByte((
char) (spdm[r] >> 16), eeAddr);
280 ee_putByte((
char) (spdm[r] >> 24), eeAddr);
300 #ifdef xbee_abcalibrate_debug 301 print(
"left eeAddr = %d\n", eeAddr);
304 lcnt = ee_getInt(eeAddr);
306 zstart = ee_getInt(eeAddr);
324 for(r = 0; r < lcnt; r++)
326 if(spdr[r] != ee_getInt(eeAddr)) ee_putInt(spdr[r], eeAddr);
328 if(spdm[r] != ee_getInt(eeAddr)) ee_putInt(spdm[r], eeAddr);
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 431 dprint(xbee,
"Before\n");
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 452 dprint(xbee,
"After\n");
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);
523 ee_putInt(rcnt, eeAddr);
525 ee_putInt(zstart, eeAddr);
527 for(r = 0; r < rcnt; r++)
531 ee_putByte((
char) spdr[r], eeAddr);
534 ee_putByte((
char) (spdr[r] >> 8), eeAddr);
537 ee_putByte((
char) (spdr[r] >> 16), eeAddr);
540 ee_putByte((
char) (spdr[r] >> 24), eeAddr);
547 ee_putByte((
char) spdm[r], eeAddr);
550 ee_putByte((
char) (spdm[r] >> 8), eeAddr);
553 ee_putByte((
char) (spdm[r] >> 16), eeAddr);
556 ee_putByte((
char) (spdm[r] >> 24), eeAddr);
563 #ifdef xbee_abcalibrate_debug 564 print(
"right eeAddr = %d\n", eeAddr);
567 rcnt = ee_getInt(eeAddr);
569 zstart = ee_getInt(eeAddr);
574 for(r = 0; r < rcnt; r++)
576 if(spdr[r] != ee_getInt(eeAddr)) ee_putInt(spdr[r], eeAddr);
578 if(spdm[r] != ee_getInt(eeAddr)) ee_putInt(spdm[r], eeAddr);
589 rcnt = ee_getInt(eeAddr);
591 zstart = ee_getInt(eeAddr);
595 for(r = 0; r < 120; r++)
604 for(r = 0; r < rcnt; r++)
606 spdr[r] = ee_getInt(eeAddr);
608 spdm[r] = ee_getInt(eeAddr);
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");
628 for(i = 0; i <=16; i++) ee_putByte(0xFF, i);
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)
671 servo_speed(sPinL, left);
672 servo_speed(sPinR, -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)
705 servo_setramp(sPinL, left);
706 servo_setramp(sPinR, 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;
742 if((CNT - tiL) > (CLKFREQ/400))
744 tcL = ((2*CLKFREQ)/(CNT - tiL));
751 if(((INA >> ePinR) & 1) != stateR)
753 stateR = (~stateR) & 1;
757 if((CNT - tiR) > (CLKFREQ/400))
759 tcR = ((2*CLKFREQ)/(CNT - tiR));
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;
782 ee_putStr(pinInfo, 8, eeAddr);
793 unsigned char pinInfo[8] = {
'e',
'p',
'L', 14,
' ',
'R', 15,
' '};
794 pinInfo[3] = (char) encPinLeft;
795 pinInfo[6] = (char) encPinRight;
797 ee_putStr(pinInfo, 8, eeAddr);
#define _ActivityBot_EE_Start_
ActivityBot EEPROM calibration data start address.
void cal_encoderPins(int encPinLeft, int encPinRight)
Set the ActivityBot's servo pin connections to something other than the default P14 (left encoder) an...
void cal_activityBot()
Run the ActivityBot calibration function. Let it run until the P26 and P27 lights turn off...
This library has a function you can call to calibrate your ActivityBot. Example code that uses this l...
Structure that contains data used by simple text device libraries.
void cal_servoPins(int servoPinLeft, int servoPinRight)
Set the ActivityBot's servo pin connections to something other than the default P12 (left servo) and ...