4 volatile char abd_str[128];
9 volatile int rec_t[8000 / 4];
15 void drive_com(
int arrayLcnt,
int arrayRcnt,
16 int centerL,
int centerR,
17 short *pwAddrL,
short *pwAddrR,
18 short *spdAddrL,
short *spdAddrR);
20 void abd_encoders(
void *par);
22 void interpolation_table_setup();
26 void set_drive_speed(
int left,
int right);
27 void interpolate2(
int *ltmp,
int *rtmp);
29 void abd_sample(
void);
33 volatile unsigned int _servoPulseReps;
35 volatile int abd_zeroDelay;
37 volatile int abd_intTabSetup;
62 static volatile int kp[6];
64 static volatile int ridx;
71 static volatile int pcount;
72 static volatile unsigned int _sprOld;
78 static volatile int phs[2];
80 static volatile int phsr[2];
85 volatile int abd_blockGoto;
94 volatile int abd_speedOld[2];
95 volatile int abd_stopCtr[2];
97 volatile int abd_stopPulseReps[2];
99 volatile int abd_ticks[2];
101 volatile int abd_speedT[2];
103 volatile int abd_speed[2];
106 volatile int abd_ticksi[2];
109 volatile int abd_ticksf[2];
112 volatile int abd_gotoFlag[2];
114 volatile int abd_rampStep[3];
115 volatile int abd_speedLimit[4];
117 volatile int abd_ticksGuard[2];
120 volatile int abd_dc[2];
123 volatile int abd_dca[2];
126 volatile int abd_ed[2];
129 volatile int abd_p[2];
132 volatile int abd_i[2];
135 volatile int abd_ea[2];
139 volatile int abd_sPin[2];
142 volatile int abd_ePin[2];
145 volatile int abd_elCnt[2];
146 volatile int abd_cntrIdx[2];
150 static volatile int cntrVal[2];
151 static volatile int ti[2];
152 static volatile int state[2];
154 static volatile int ssi[2];
155 static volatile int drive[2];
157 static volatile int speedPrev[2];
158 volatile int abd_nudgeCtr[2];
159 volatile int abd_nudgeInc[2];
160 volatile int abd_distError[2];
162 volatile int sign[2];
163 volatile int abd_dist[2];
165 volatile int abd_ditherA[2];
166 volatile int abd_ditherAa[2];
167 volatile int abd_ditherAd[2];
168 volatile int abd_ditherAp[2];
170 volatile int abd_ditherV[2];
171 volatile int abd_ditherVa[2];
172 volatile int abd_ditherVd[2];
173 volatile int abd_ditherVp[2];
175 volatile int abd_speedi[2];
176 volatile int abd_speedd[2];
177 volatile int abd_dvFlag[2];
179 int abd_abs(
int value);
198 int abd_checkActivityBotStrings(
void)
203 if(strncmp(str,
"spL", 3) != 0) state |= (1 << 0);
220 if(strncmp(str,
"epL", 3) != 0) state |= (1 << 1);
243 int abd_checkForSwappedCables(
void)
248 int avgPt = (int)((abd_spdmL[2] + abd_spdmL[3] + abd_spdmL[4]) / 3);
256 for(
int n = 5; n < 30; n ++)
259 if(abd_abs((
int)(abd_spdmL[n] - avgPt)) < 8)
265 if((deviations < 4) && (sum != 0)) state |= (1 << 0);
269 avgPt = (int)((abd_spdmR[50] + abd_spdmR[51] + abd_spdmR[52]) / 3);
278 for(
int n = 53; n < 53 + 17; n ++)
281 if(abd_abs((
int)(abd_spdmR[n] - avgPt)) < 8)
286 if((deviations < 4) && (sum != 0)) state |= (1 << 1);
292 int abd_checkForNoSignal(
void)
295 if( (abd_elCnt[ABD_L] == 67) && (abd_cntrIdx[ABD_L] == 1))
299 if( (abd_elCnt[ABD_R] == 199) && (abd_cntrIdx[ABD_R] == 49))
307 int abd_checkCenterPulseWidth(
void)
310 if( (abd_abs(abd_spdrL[abd_cntrIdx[ABD_L]]) > 45) )
314 if( (abd_abs(abd_spdrR[abd_cntrIdx[ABD_R]]) > 45) )
322 int abd_checkServoCalSupply(
int side)
325 for(
int n = 1; n <= 10; n++)
330 sum += (abd_spdmL[abd_elCnt[side] - 1 - n]);
335 sum += (abd_spdmR[abd_elCnt[side] - 1 - n]);
342 void abd_displaySide(
int side,
char *s)
348 sprint(s,
"the ActivityBot's left ");
353 sprint(s,
"the ActivityBot's right ");
358 sprint(s,
"either of the ActivityBot's ");
363 sprint(s,
"both of the ActivityBot's ");
374 if(!abd_intTabSetup) interpolation_table_setup();
375 int cfgStrs = abd_checkActivityBotStrings();
376 int cables = abd_checkForSwappedCables();
377 int noSignal = abd_checkForNoSignal();
378 int centerError = abd_checkCenterPulseWidth();
379 int supplyL = abd_checkServoCalSupply(ABD_L);
380 int supplyR = abd_checkServoCalSupply(ABD_R);
409 print(
"The calibration completed successfully!\r\r");
410 print(
"Have fun with your ActivityBot. Make sure\r\r");
411 print(
"to try the activities at learn.parallax.com.\r\r");
415 print(
"One or more problems were detected. For\r");
416 print(
"help, go to the troubleshooting page in\r");
417 print(
"the learn.parallax.com tutorial you are\r");
418 print(
"following.\r\r");
419 print(
"Details:\r\r");
425 print(
"It does not look like the calibration\r");
426 print(
"procedure has been completed.\r\r");
435 print(
"Either your ActivityBot's servo cables\r");
436 print(
"or its encoder cables are swapped.\r\r");
439 if(noSignal && (cables == 0))
441 print(
"The Propeller cannot detect\r");
442 abd_displaySide(noSignal, s);
444 print(
"encoder signal(s).\r\r");
459 if(centerError == 3) centerError = 4;
460 print(
"The servo on \r");
461 abd_displaySide(centerError, s);
463 print(
"side(s) needs mechanical\r");
464 print(
"calibration.\r\r");
479 int supply = (supplyL + supplyR) / 2;
482 print(
"The ActivityBot's batteries are either\r");
483 print(
"really dead, or one of the cells is in\r");
484 print(
"the battery holder backwards. A short \r");
485 print(
"circuit in the prototyping area could also \r");
486 print(
"explain the slow-moving behavior.\r\r");
488 else if(supply < 140)
490 print(
"The ActivityBot's batties are either dead,\r");
491 print(
"or the P13/P12 PWR select jumper is still at\r");
492 print(
"the 5V setting. It should be at the VIN\r");
493 print(
"setting.\r\r");
495 else if(supply < 150)
497 print(
"The ActivityBot's batteries are too low.\r");
498 print(
"Try a new set of 5 alkaline batteries, or\r");
499 print(
"recharge your power pack to the 7 to 8 V range.\r\r");
501 else if(supply > 205)
503 print(
"Yikes! Servo speeds indicate your\r");
504 print(
"ActivityBot may have a power pack that's\r");
505 print(
"more than 8.5 V. Use an appropriate\r");
506 print(
"power source to avoid servo damage.\r\r");