drive down
Dependencies: BMP280 BNO055_fusion PowerControl mbed
Fork of TEAM_G_FLOW_RIDA by
Diff: main.cpp
- Revision:
- 19:2dd81b864e14
- Parent:
- 18:f9012e93edb8
- Child:
- 20:10a305298e27
--- a/main.cpp Tue Mar 29 02:12:08 2016 +0000 +++ b/main.cpp Sun Apr 03 05:48:37 2016 +0000 @@ -1,45 +1,68 @@ +#include "PowerControl/PowerControl.h" +#include "PowerControl/EthernetPowerControl.h" #include "LOCALIZE.h" #include "LOCOMOTION.h" #include "WATCHDOG.h" -#define SPEED_FB_MIN 0.15 -#define SPEED_FB_MAX 0.50 +#define WATCHDOG_TIME 10 +//#define PC_MODE 1 +#define USR_POWERDOWN (0x104) +#if defined (PC_MODE) +Serial pc(USBTX, USBRX); +#else Serial pc(p13, p14); -//Serial pc(USBTX, USBRX); +Ticker t; +void send(); +#endif + +int semihost_powerdown() +{ + uint32_t arg; + return __semihost(USR_POWERDOWN, &arg); +} Watchdog wdt; - +PwmOut suction(p26); I2C i2c1(p28, p27); I2C i2c2(p9, p10); -LOCALIZE loc(i2c1, i2c2, p26, p8, p7, p6, p5); +LOCALIZE loc(i2c1, i2c2, p29, p8, p7, p6, p5); LOCALIZE_xya xya; LOCOMOTION motion(p21, p22, p23, p24, p15, p16); -Ticker t; -Ticker tTarget; bool flag=false; int target=20; int angle_error=2; bool xGood=false; int angleTarget=0; -void setTarget(); -void send(); -//void setAngle(int angle); -int wrap(int a); +void BrownOut(); int main() { - wdt.kick(5); + //Set ESC Period + suction.period_ms(2); + //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); + //Serial Baudrate pc.baud(9600); - //pc.printf("Initialized Localization: %d\n",loc.init()); + //Attach Periodic Wireless Printing +#if not defined(PC_MODE) t.attach(&send,1); - //tTarget.attach(&setTarget,7); +#endif while(1) { - //loc.get_angle(&xya); - loc.get_xy(&xya); - if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) { + //loc.get_xy(&xya); + /*if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) { xGood = motion.setXPos(target,xya.x,2,angleTarget); if(motion.setYPos(130,xya.y,2,angleTarget)) { angle_error=2; @@ -49,18 +72,28 @@ angle_error=2; } else angle_error=10; - } - //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a); + }*/ +#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(); } } void send() { + //Print over serial port to WiFi MCU pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10); } -void setTarget() +void BrownOut() { - target=target==20?80:20; + //Ensure suction on + motion.stopMotors(); + suction.pulsewidth_us(2000); + //Power Down Non Essential + Peripheral_PowerDown(~(LPC1768_PCONP_PCPWM1 | LPC1768_PCONP_PCGPIO | LPC1768_PCONP_PCTIM0)); + //Sleep wake by interrupt + Sleep(); } \ No newline at end of file