drive down
Dependencies: BMP280 BNO055_fusion PowerControl mbed
Fork of TEAM_G_FLOW_RIDA by
Diff: main.cpp
- Revision:
- 23:455f7da3dd7a
- Parent:
- 20:10a305298e27
- Child:
- 24:fb1f083ebd62
--- a/main.cpp Sun Apr 03 06:15:52 2016 +0000 +++ b/main.cpp Sun Apr 03 07:08:18 2016 +0000 @@ -1,12 +1,9 @@ -#include "PowerControl/PowerControl.h" -#include "PowerControl/EthernetPowerControl.h" #include "LOCALIZE.h" #include "LOCOMOTION.h" -#include "WATCHDOG.h" +#include "SAFETY.h" #define WATCHDOG_TIME 10 //#define PC_MODE 1 -#define USR_POWERDOWN (0x104) #if defined (PC_MODE) Serial pc(USBTX, USBRX); @@ -16,19 +13,18 @@ void send(); #endif -int semihost_powerdown() -{ - uint32_t arg; - return __semihost(USR_POWERDOWN, &arg); -} +DigitalOut led1(LED1); +DigitalOut led2(LED2); +DigitalOut led3(LED3); +DigitalOut led4(LED4); -Watchdog wdt; +SAFETY safe; PwmOut suction(p26); I2C i2c1(p28, p27); I2C i2c2(p9, p10); -LOCALIZE loc(i2c1, i2c2, p29, p8, p7, p6, p5); +LOCALIZE loc(i2c1, i2c2, p29, p8, p7, p6, p5, led1, led2, led3, led4); LOCALIZE_xya xya; -LOCOMOTION motion(p21, p22, p23, p24, p15, p16); +LOCOMOTION motion(p11, p21, p22, p23, p24, p15, p16); void BrownOut(); @@ -45,15 +41,9 @@ //Initialize to 0 suction.pulsewidth_us(1000); //Start Watchdog - wdt.kick(WATCHDOG_TIME); - //Power Down Ethernet - PHY_PowerDown(); - //Power Down USB Chip - semihost_powerdown(); - //Setup Brown Out Interrupt - NVIC_SetVector(BOD_IRQn, (unsigned)BrownOut); - //Enable Brown Out Interrupt - NVIC_EnableIRQ(BOD_IRQn); + safe.kick(WATCHDOG_TIME); + //Power Down Ethernet and USB and Enable Brown Out Interrupt + safe.init((unsigned)BrownOut); //Serial Baudrate pc.baud(9600); //Attach Periodic Wireless Printing @@ -73,11 +63,13 @@ } else angle_error=10; }*/ + + #if defined(PC_MODE) pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a); #endif //Feed the dog - wdt.kick(); + safe.kick(); } } @@ -90,10 +82,14 @@ void BrownOut() { //Ensure suction on - motion.stopMotors(); + motion.eStop(); suction.pulsewidth_us(2000); + led1=1; + led2=0; + led3=1; + led4=0; //Power Down Non Essential Peripheral_PowerDown(~(LPC1768_PCONP_PCPWM1 | LPC1768_PCONP_PCGPIO | LPC1768_PCONP_PCTIM0)); //Sleep wake by interrupt - Sleep(); + //Sleep(); } \ No newline at end of file