drive down
Dependencies: BMP280 BNO055_fusion PowerControl mbed
Fork of TEAM_G_FLOW_RIDA by
Diff: main.cpp
- Revision:
- 30:116cd143fdf7
- Parent:
- 28:65daccc10f31
- Child:
- 31:69caabc10879
--- a/main.cpp Wed Apr 13 07:48:49 2016 +0000 +++ b/main.cpp Tue Apr 19 02:04:10 2016 +0000 @@ -1,6 +1,7 @@ #include "LOCALIZE.h" #include "LOCOMOTION.h" #include "SAFETY.h" +#include "BMP280.h" #define WATCHDOG_TIME 10 //#define PC_MODE 1 @@ -22,6 +23,7 @@ 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); @@ -29,14 +31,15 @@ void BrownOut(); -int xTarget=20; +int xTarget=120; int angle_error=5; bool xGood=false; bool yGood=false; bool angleGood=false; int xState=X_INCREASE; -int angleTarget=0; -int yTarget=30; +int angleTarget=5; +int yTarget=80; +int pressure; //bool flag=false; //int target=20; @@ -66,6 +69,7 @@ pc.baud(9600); //Initialize Locomotion loc.init(); + //pres.initialize(); //Attach Periodic Wireless Printing #if not defined(PC_MODE) t.attach(&send,1); @@ -74,12 +78,31 @@ 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(1600); + suction.pulsewidth_us(1750); //loc.get_angle(&xya); loc.get_xy(&xya); - //motion.setAngle(90,xya.a,angle_error,ANGLE_TURN); - //motion.moveF(xya.a); + //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 @@ -114,7 +137,7 @@ 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); + pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,pressure/10,pressure%10); } void BrownOut()