Diff: main.cpp

Revision:

17:2f89826b5679

Parent:

16:d6f15a13c3aa

Child:

18:f9012e93edb8

--- a/main.cpp	Sun Mar 27 02:39:07 2016 +0000
+++ b/main.cpp	Tue Mar 29 01:11:09 2016 +0000
@@ -17,8 +17,13 @@
 LOCOMOTION motion(p21, p22, p23, p24, p15, p16);
 
 Ticker t;
+Ticker tTarget;
 bool flag=false;
+int target=20;
+int angle_error=2;
+bool xGood=false;
 
+void setTarget();
 void send();
 //void setAngle(int angle);
 int wrap(int a);
@@ -29,10 +34,17 @@
     pc.baud(9600);
     //pc.printf("Initialized Localization: %d\n",loc.init());
     t.attach(&send,1);
+    tTarget.attach(&setTarget,7);
     while(1) {
         //loc.get_angle(&xya);
         loc.get_xy(&xya);
-        motion.setAngle(0,xya.a,2,ANGLE_TURN);
+        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)
+                angle_error=2;
+            else
+                angle_error=10;
+        }
         //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
         wdt.kick();
     }
@@ -42,35 +54,8 @@
 {
     pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10);
 }
-/*
-void setAngle(int angle)
+
+void setTarget()
 {
-    float s = 0;
-    int diff = 0;
-    diff = 180-wrap(angle);
-    if(abs(wrap(xya.a+diff)-180)<=5)
-        s=SPEED_TURN_MIN;
-    else
-        s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(xya.a+diff)-180)/180)+SPEED_TURN_MIN;
-    motor1F=s;
-    motor1B=s;
-    motor2F=s;
-    motor2B=s;
-    if(wrap(xya.a+diff)>180+2) {
-        dir1=1;
-        dir2=0;
-    } else if(wrap(xya.a+diff)<180-2) {
-        dir1=0;
-        dir2=1;
-    } else {
-        motor1F=0;
-        motor1B=0;
-        motor2F=0;
-        motor2B=0;
-    }
-}*/
-
-int wrap(int a)
-{
-    return a%360;
-}
+    target=target==20?80:20;
+}
\ No newline at end of file