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);