drive down
Dependencies: BMP280 BNO055_fusion PowerControl mbed
Fork of TEAM_G_FLOW_RIDA by
Diff: main.cpp
- Revision:
- 25:f3bf8351bbd4
- Parent:
- 24:fb1f083ebd62
- Child:
- 26:0ea6a550a99d
--- a/main.cpp Sun Apr 03 07:25:07 2016 +0000 +++ b/main.cpp Tue Apr 05 02:30:40 2016 +0000 @@ -28,6 +28,15 @@ void BrownOut(); +int xTarget=20; +int angle_error=2; +bool xGood=false; +bool yGood=false; +bool angleGood=false; +int xState=X_INCREASE; +int angleTarget=0; +int yTarget=100; + //bool flag=false; //int target=20; //int angle_error=2; @@ -59,11 +68,23 @@ led2=0; led3=0; led4=0; + suction.pulsewidth_us(1000); while(1) { - //loc.get_xy(&xya); + //suction.pulsewidth_us(1900); + 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.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(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)) { + if(motion.setYBias(130,xya.y,2,angleTarget)) { angle_error=2; } else if(xGood) { target=target==20?80:20; @@ -75,7 +96,7 @@ #if defined(PC_MODE) - pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a); + pc.printf("X: %3d\tY: %3d\tP: %3d\txGood: %d\tAngleGood: %d\tState: %d \n",xya.x,xya.y,xya.a,xGood,angleGood,xState); #endif //Feed the dog safe.kick();