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