13 void get_pulse_left();
14 void get_pulse_right();
17 volatile int pulseLeft;
18 volatile int pulseRight;
19 static int ignore_low_battery;
28 static int errorVal = AB360_ERROR_CONDITION_UNKNOWN;
30 void cal_supply5V(
int setting)
32 ignore_low_battery = setting;
35 void playNotes(
float tempo,
float beatVal,
int *note,
float *hold);
37 enum { C6 = 1047, D6b = 1109, D6 = 1175, E6b = 1245, E6 = 1319, F6 = 1397,
38 G6b = 1480, G6 = 1568, A6b = 1661, A6 = 1760, B6b = 1865, B6 = 1976,
39 C7 = 2093, D7b = 2219, D7 = 2349, E7b = 2489, E7 = 2637, F7 = 2794,
40 G7b = 2960, G7 = 3136, A7b = 3322, A7 = 3520, B7b = 3729, B7 = 3951,
43 void playNotes(
float tempo,
float beatVal,
int *note,
float *hold)
45 float msBeat = 60000.0 / tempo;
46 float tFullNote = msBeat * beatVal;
52 t = (int) (hold[i] * tFullNote);
59 void cal_fail_dance(
void);
70 void get_pulse_right()
153 errorVal = AB360_ERROR_NO_ENC_SIG_BOTH;
164 errorVal = AB360_ERROR_NO_ENC_SIG_LEFT;
175 errorVal = AB360_ERROR_NO_ENC_SIG_RIGHT;
188 int brad12bitL, brad12bitR;
200 brad12bitL = servo360_getAngle12Bit(abd360_pinCtrlLeft);
201 brad12bitR = servo360_getAngle12Bit(abd360_pinCtrlRight);
210 brad12bitL = servo360_getAngle12Bit(abd360_pinCtrlLeft) - brad12bitL;
211 brad12bitR = servo360_getAngle12Bit(abd360_pinCtrlRight) - brad12bitR;
239 errorVal = AB360_ERROR_CABLE_SWAP;
256 errorVal = AB360_ERROR_NO_MOTION_BOTH;
265 errorVal = AB360_ERROR_NO_MOTION_LEFT;
274 errorVal = AB360_ERROR_NO_MOTION_RIGHT;
281 print(
"error = %d\r\r", errorVal);
283 if(errorVal == AB360_ERROR_NO_ENC_SIG_BOTH)
285 int pulseLeftPrev = pulseLeft;
286 int pulseRightPrev = pulseRight;
287 int countLeft = 0, countRight = 0;
288 for(
int n = 0; n < 50; n++)
292 if(pulseLeft != pulseLeftPrev)
295 pulseLeftPrev = pulseLeft;
297 if(pulseRight != pulseRightPrev)
300 pulseRightPrev = pulseRight;
304 if((countLeft > 3) || (countRight > 3))
306 errorVal = AB360_ERROR_POSSIBLE_AB_NOT_360;
319 int n,
x, angle, angleP;
320 int mVccwL, mVcwL, bVccwL, bVcwL;
321 int mVccwR, mVcwR, bVccwR, bVcwR;
322 int increases = 0, decreases = 0, diffCount = 0;
326 x = servo360_getAngle12Bit(abd360_pinCtrlLeft);
329 x = servo360_getAngle12Bit(abd360_pinCtrlLeft) -
x;
341 for(n = 0; n < 60; n++)
346 if(angle != angleP) increases++;
347 if(increases > 3)
break;
352 bVccwL = (n - 4) * 10 * 2 / 3;
357 mVccwL = 1000 * (2200 - bVccwL) /
x;
367 x = servo360_getAngle12Bit(abd360_pinCtrlLeft);
370 x = abs(servo360_getAngle12Bit(abd360_pinCtrlLeft) -
x);
382 for(n = 0; n > -60; n--)
387 if(angle != angleP) decreases++;
388 if(decreases > 3)
break;
393 bVcwL = abs((n + 4) * 10 * 2 / 3);
398 mVcwL = 1000 * (2200 - bVcwL) /
x;
410 x = servo360_getAngle12Bit(abd360_pinCtrlRight);
413 x = servo360_getAngle12Bit(abd360_pinCtrlRight) -
x;
426 for(n = 0; n < 60; n++)
431 if(angle != angleP) increases++;
432 if(increases > 3)
break;
437 bVccwR = (n - 4) * 10 * 2 / 3;
442 mVccwR = 1000 * (2200 - bVccwR) /
x;
452 x = servo360_getAngle12Bit(abd360_pinCtrlRight);
455 x = abs(servo360_getAngle12Bit(abd360_pinCtrlRight) -
x);
468 for(n = 0; n > -60; n--)
473 if(angle != angleP) decreases++;
474 if(decreases > 3)
break;
479 bVcwR = abs((n + 4) * 10 * 2 / 3);
484 mVcwR = 1000 * (2200 - bVcwR) /
x;
511 (mVccwL > (S360_VM_CCW * 3))
513 (mVccwL < (S360_VM_CCW / 3))
515 (mVcwL > (S360_VM_CW * 3))
517 (mVcwL < (S360_VM_CW / 3))
519 (mVccwR > (S360_VM_CCW * 3))
521 (mVccwR < (S360_VM_CCW / 3))
523 (mVcwR > (S360_VM_CW * 3))
525 (mVcwR < (S360_VM_CW / 3))
527 (bVccwL > (S360_VB_CCW + (S360_VB_CCW * 3)))
529 (bVccwL < (S360_VB_CCW - (S360_VB_CCW * 3)))
531 (bVcwL < (S360_VB_CCW - (S360_VB_CCW * 3)))
533 (bVcwL > (S360_VB_CCW + (S360_VB_CCW * 3)))
535 (bVccwR > (S360_VB_CCW + (S360_VB_CCW * 3)))
537 (bVccwR < (S360_VB_CCW - (S360_VB_CCW * 3)))
539 (bVcwL < (S360_VB_CCW - (S360_VB_CCW * 3)))
541 (bVcwL > (S360_VB_CCW + (S360_VB_CCW * 3)))
544 errorVal = AB360_ERROR_XFER_OUT_OF_RANGE;
546 else if( ((mVccwL+mVccwR+mVcwR+mVcwL) > 940) && (!ignore_low_battery))
548 errorVal = AB360_ERROR_BATTERIES_TOO_LOW;
550 else if( (mVccwL+mVccwR+mVcwR+mVcwL) < 575)
552 errorVal = AB360_ERROR_BATTERIES_TOO_HIGH;
560 int chargeNotes[] = { G6, C7, E7, G7, E7, G7,
'Q'};
562 float chargeHolds[] = {0.125, 0.125, 0.125, 0.187, 0.125, 0.50, 0 };
614 playNotes(128.0, 4.0, chargeNotes, chargeHolds);
632 char str[] =
"AB360 ";
633 sprint(&str[6],
"%d", errorVal);
634 print(
"str = %s", str);
646 unsigned char pinInfo[8] = {
's',
'p',
'L', 12,
' ',
'R', 13,
' '};
647 pinInfo[3] = (
char) servoPinLeft;
648 pinInfo[6] = (
char) servoPinRight;
652 abd360_pinCtrlLeft = servoPinLeft;
653 abd360_pinCtrlRight = servoPinRight;
660 unsigned char pinInfo[8] = {
'e',
'p',
'L', 14,
' ',
'R', 15,
' '};
661 pinInfo[3] = (
char) encPinLeft;
662 pinInfo[6] = (
char) encPinRight;
666 abd360_pinFbLeft = encPinLeft;
667 abd360_pinFbRight = encPinRight;
680 void cal_fail_dance(
void)
685 for(
int i = 0; i < 3; i++)
692 for(
int j = 0 ; j < 3; j++)
707 for(
int j = 0 ; j < 3; j++)
710 for(
int k = 0; k < 5; k++)