Mekatronics Team G
Dependencies: BNO055_fusion PowerControl mbed BMP280
Fork of DEMO3 by
main.cpp
- Committer:
- 12104404
- Date:
- 2016-04-19
- Revision:
- 30:116cd143fdf7
- Parent:
- 28:65daccc10f31
File content as of revision 30:116cd143fdf7:
#include "LOCALIZE.h" #include "LOCOMOTION.h" #include "SAFETY.h" #include "BMP280.h" #define WATCHDOG_TIME 10 //#define PC_MODE 1 #if defined (PC_MODE) Serial pc(USBTX, USBRX); #else Serial pc(p13, p14); Ticker t; void send(); #endif DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalOut led4(LED4); SAFETY safe; PwmOut suction(p26); I2C i2c2(p28, p27); I2C i2c1(p9, p10); BMP280 pres(i2c2); LOCALIZE loc(i2c1, i2c2, p29, p8, p7, p6, p5, led1, led2, led3, led4); LOCALIZE_xya xya; LOCOMOTION motion(p11, p23, p24, p21, p22, p16, p15, led1, led2, led3, led4); //LOCOMOTION motion(p11, p21, p22, p23, p24, p15, p16, led1, led2, led3, led4); void BrownOut(); int xTarget=120; int angle_error=5; bool xGood=false; bool yGood=false; bool angleGood=false; int xState=X_INCREASE; int angleTarget=5; int yTarget=80; int pressure; //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); wait(2); /*suction.pulsewidth_us(1000); wait(2);*/ //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 safe.init((unsigned)BrownOut); //Serial Baudrate pc.baud(9600); //Initialize Locomotion loc.init(); //pres.initialize(); //Attach Periodic Wireless Printing #if not defined(PC_MODE) t.attach(&send,1); #endif led1=0; led2=0; led3=0; led4=0; suction.pulsewidth_us(1200); wait(0.5); suction.pulsewidth_us(1300); wait(0.5); suction.pulsewidth_us(1400); wait(0.5); suction.pulsewidth_us(1500); wait(0.5); suction.pulsewidth_us(1600); wait(0.5); while(1) { suction.pulsewidth_us(1750); //loc.get_angle(&xya); loc.get_xy(&xya); //pressure=(int)read[0];//(int)pres.getTemperature(); //if(pressure==88) // led1=1; //else // led1=0; /*if(motion.setAngle(5,xya.a,angle_error,ANGLE_TURN)) { motion.setXPos(xTarget,xya.x,2,angleTarget); motion.setYBias(yTarget,xya.y,2,angleTarget); }*/ //motion.moveF(xya.a); /*if(abs(xya.a-180)<90) motion.moveB(); else motion.moveB();*/ //loc.get_xy(&xya); /*if (yTarget>120) { motion.mStop(); safe.kick(); continue; } loc.get_xy(&xya); motion.check_xya(&xGood,&yGood,&angleGood,xTarget,angleTarget,yTarget,xya,3,3,angle_error); motion.setXstate(&xState,&xTarget,xGood,angleGood,&angleTarget,&angle_error); motion.setAngleTol(&angle_error,yGood,xGood); motion.setYgoal(xGood,angleGood,yGood,&yTarget); if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) { motion.setXPos(xTarget,xya.x,2,angleTarget); motion.setYBias(yTarget,xya.y,2,angleTarget); }*/ #if defined(PC_MODE) pc.printf("X: %3d\tY: %3d\tP: %3d\txGood: %d\tAngleGood: %d\tState: %d \n",xya.y,yTarget,xya.a,xGood,angleGood,xState); #endif //Feed the dog safe.kick(); } } void send() { //Print over serial port to WiFi MCU pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,pressure/10,pressure%10); } void BrownOut() { //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)); //Sleep wake by interrupt //Sleep(); }