![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
added
Dependencies: BNO055_fusion PowerControl mbed
Fork of TEAM_G_FLOW_RIDA by
main.cpp
- Committer:
- 12104404
- Date:
- 2016-04-03
- Revision:
- 20:10a305298e27
- Parent:
- 19:2dd81b864e14
- Child:
- 23:455f7da3dd7a
File content as of revision 20:10a305298e27:
#include "PowerControl/PowerControl.h" #include "PowerControl/EthernetPowerControl.h" #include "LOCALIZE.h" #include "LOCOMOTION.h" #include "WATCHDOG.h" #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); 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, p29, p8, p7, p6, p5); LOCALIZE_xya xya; LOCOMOTION motion(p21, p22, p23, p24, p15, p16); void BrownOut(); //bool flag=false; //int target=20; //int angle_error=2; //bool xGood=false; //int angleTarget=0; int main() { //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); //Attach Periodic Wireless Printing #if not defined(PC_MODE) t.attach(&send,1); #endif while(1) { //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; } else if(xGood) { target=target==20?80:20; angleTarget=angleTarget==0?180:0; angle_error=2; } 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(); } } 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 BrownOut() { //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(); }