added
Dependencies: BNO055_fusion PowerControl mbed
Fork of TEAM_G_FLOW_RIDA by
Diff: main.cpp
- Revision:
- 18:f9012e93edb8
- Parent:
- 17:2f89826b5679
- Child:
- 19:2dd81b864e14
--- a/main.cpp Tue Mar 29 01:11:09 2016 +0000 +++ b/main.cpp Tue Mar 29 02:12:08 2016 +0000 @@ -22,6 +22,7 @@ int target=20; int angle_error=2; bool xGood=false; +int angleTarget=0; void setTarget(); void send(); @@ -34,15 +35,19 @@ pc.baud(9600); //pc.printf("Initialized Localization: %d\n",loc.init()); t.attach(&send,1); - tTarget.attach(&setTarget,7); + //tTarget.attach(&setTarget,7); while(1) { //loc.get_angle(&xya); loc.get_xy(&xya); - if(motion.setAngle(0,xya.a,angle_error,ANGLE_TURN)) { - xGood = motion.setXPos(target,xya.x,2); - if(motion.setYPos(130,xya.y,2) || xGood) + 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)) { angle_error=2; - else + } else if(xGood) { + target=target==20?80:20; + angleTarget=angleTarget==0?180:0; + angle_error=2; + } else angle_error=10; } //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);