added
Dependencies: BNO055_fusion PowerControl mbed
Fork of TEAM_G_FLOW_RIDA by
Diff: main.cpp
- Revision:
- 24:fb1f083ebd62
- Parent:
- 23:455f7da3dd7a
- Child:
- 25:f3bf8351bbd4
diff -r 455f7da3dd7a -r fb1f083ebd62 main.cpp --- a/main.cpp Sun Apr 03 07:08:18 2016 +0000 +++ b/main.cpp Sun Apr 03 07:25:07 2016 +0000 @@ -24,7 +24,7 @@ I2C i2c2(p9, p10); LOCALIZE loc(i2c1, i2c2, p29, p8, p7, p6, p5, led1, led2, led3, led4); LOCALIZE_xya xya; -LOCOMOTION motion(p11, p21, p22, p23, p24, p15, p16); +LOCOMOTION motion(p11, p21, p22, p23, p24, p15, p16, led1, led2, led3, led4); void BrownOut(); @@ -40,6 +40,11 @@ suction.period_ms(2); //Initialize to 0 suction.pulsewidth_us(1000); + //Watchdog Reset Indicator + if ((LPC_WDT->WDMOD>>2)&1) + led4=1; + else + led3=1; //Start Watchdog safe.kick(WATCHDOG_TIME); //Power Down Ethernet and USB and Enable Brown Out Interrupt @@ -50,6 +55,10 @@ #if not defined(PC_MODE) t.attach(&send,1); #endif + led1=0; + led2=0; + led3=0; + led4=0; while(1) { //loc.get_xy(&xya); /*if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) { @@ -64,7 +73,7 @@ angle_error=10; }*/ - + #if defined(PC_MODE) pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a); #endif @@ -81,15 +90,17 @@ void BrownOut() { - //Ensure suction on + //Stop Motors and Driver motion.eStop(); + //Ensure suction on suction.pulsewidth_us(2000); + //Light Up Error Light Pattern led1=1; led2=0; led3=1; led4=0; //Power Down Non Essential - Peripheral_PowerDown(~(LPC1768_PCONP_PCPWM1 | LPC1768_PCONP_PCGPIO | LPC1768_PCONP_PCTIM0)); + Peripheral_PowerDown(~(LPC1768_PCONP_PCPWM1 | LPC1768_PCONP_PCGPIO)); //Sleep wake by interrupt //Sleep(); } \ No newline at end of file