Diff: main.cpp

Revision:

20:1da786e205eb

Parent:

19:5832e34b7533

--- a/main.cpp	Tue Mar 29 21:58:26 2016 +0000
+++ b/main.cpp	Tue Mar 29 22:49:05 2016 +0000
@@ -19,15 +19,15 @@
 Ticker t;
 Ticker tTarget;
 bool flag=false;
-int target=20;
+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=30;
-void setTarget();
+//void setTarget();
 void send();
 //void setAngle(int angle);
 int wrap(int a);
@@ -42,23 +42,23 @@
     while(1) {
         //loc.get_angle(&xya);
         loc.get_xy(&xya);
-        motion.check_xya(&xGood,&yGood,&angleGood,target,angleTarget,yTarget,angle_error)
+        motion.check_xya(&xGood,&yGood,&angleGood,xTarget,angleTarget,yTarget,xya,2,2,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)) {
-            xGood = motion.setXPos(target,xya.x,2,angleTarget);
-            if(motion.setYPos(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;
+            motion.setXPos(xTarget,xya.x,2,angleTarget);
+            motion.setYPos(yTarget,xya.y,2,angleTarget);
+
         }
+
         //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
         wdt.kick();
     }
 }
 
+
 void send()
 {
     pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10);
@@ -66,5 +66,5 @@
 
 void setTarget()
 {
-    target=target==20?80:20;
+    xTarget=xTarget==20?80:20;
 }
\ No newline at end of file