drive down
Dependencies: BMP280 BNO055_fusion PowerControl mbed
Fork of TEAM_G_FLOW_RIDA by
Diff: main.cpp
- Revision:
- 27:fb1298d35bd1
- Parent:
- 26:0ea6a550a99d
- Child:
- 28:65daccc10f31
--- a/main.cpp Wed Apr 06 04:04:00 2016 +0000 +++ b/main.cpp Wed Apr 13 03:14:43 2016 +0000 @@ -3,7 +3,7 @@ #include "SAFETY.h" #define WATCHDOG_TIME 10 -#define PC_MODE 1 +//#define PC_MODE 1 #if defined (PC_MODE) Serial pc(USBTX, USBRX); @@ -20,16 +20,17 @@ SAFETY safe; PwmOut suction(p26); -I2C i2c1(p28, p27); -I2C i2c2(p9, p10); +I2C i2c2(p28, p27); +I2C i2c1(p9, p10); LOCALIZE loc(i2c1, i2c2, p29, p8, p7, p6, p5, led1, led2, led3, led4); LOCALIZE_xya xya; -LOCOMOTION motion(p11, p21, p22, p23, p24, p15, p16, led1, led2, led3, led4); +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=20; -int angle_error=2; +int angle_error=5; bool xGood=false; bool yGood=false; bool angleGood=false; @@ -49,6 +50,9 @@ 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; @@ -71,8 +75,17 @@ led3=0; led4=0; while(1) { - suction.pulsewidth_us(2000); - if (yTarget>120) { + suction.pulsewidth_us(1600); + loc.get_angle(&xya); + motion.setAngle(90,xya.a,angle_error,ANGLE_TURN); + //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; @@ -88,7 +101,7 @@ 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