drive down
Dependencies: BMP280 BNO055_fusion PowerControl mbed
Fork of TEAM_G_FLOW_RIDA by
Diff: main.cpp
- Revision:
- 26:0ea6a550a99d
- Parent:
- 25:f3bf8351bbd4
- Child:
- 27:fb1298d35bd1
--- a/main.cpp Tue Apr 05 02:30:40 2016 +0000 +++ b/main.cpp Wed Apr 06 04:04:00 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); @@ -35,7 +35,7 @@ bool angleGood=false; int xState=X_INCREASE; int angleTarget=0; -int yTarget=100; +int yTarget=30; //bool flag=false; //int target=20; @@ -60,6 +60,8 @@ safe.init((unsigned)BrownOut); //Serial Baudrate pc.baud(9600); + //Initialize Locomotion + loc.init(); //Attach Periodic Wireless Printing #if not defined(PC_MODE) t.attach(&send,1); @@ -68,13 +70,18 @@ led2=0; led3=0; led4=0; - suction.pulsewidth_us(1000); while(1) { - //suction.pulsewidth_us(1900); + suction.pulsewidth_us(2000); + 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); + + 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)) { @@ -82,21 +89,8 @@ motion.setYBias(yTarget,xya.y,2,angleTarget); } - /*if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) { - xGood = motion.setXPos(target,xya.x,2,angleTarget); - if(motion.setYBias(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\txGood: %d\tAngleGood: %d\tState: %d \n",xya.x,xya.y,xya.a,xGood,angleGood,xState); + 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();