Diff: main.cpp

Revision:

36:dc69442c4c47

Parent:

35:68917796c30a

Child:

37:e8a6ea255c09

--- a/main.cpp	Sun Apr 24 22:37:07 2016 +0000
+++ b/main.cpp	Mon Apr 25 17:47:51 2016 +0000
@@ -32,7 +32,7 @@
 void BrownOut();
 
 int xTarget=120;
-int angle_error=0;
+int angle_error=1;
 bool xGood=false;
 bool yGood=false;
 bool angleGood=false;
@@ -95,11 +95,11 @@
     wait(0.5);*/
     while(1) {
         suction.pulsewidth_us(1300);
-         /*pressure=(int)read[0];//(int)pres.getTemperature();
+        /*pressure=(int)read[0];//(int)pres.getTemperature();
         if(pressure==88)
-            led1=1;
+           led1=1;
         else
-            led1=0;*/
+           led1=0;*/
         //uncomment this part if you want the robot to just drive down the window with no separtor
         if (xState==0) {
             motion.mStop();
@@ -107,17 +107,22 @@
             continue;
         }
         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,&angle_error,yTarget);
+        //motion.check_xya(&xGood,&yGood,&angleGood,xTarget,angleTarget,yTarget,xya,3,3,angle_error);
+
+        //motion.setXstate(&xState,&xTarget,xGood,angleGood,&angleTarget,&angle_error,yTarget);
         //motion.setAngleTol(&angle_error,yGood,xGood);
-       // motion.setYgoal(xGood,angleGood,yGood,&yTarget);
-        motion.setXPos(xTarget,xya.x,5,angleTarget,xya.a);
-        motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_BIAS);
+        // motion.setYgoal(xGood,angleGood,yGood,&yTarget);
+        //motion.setXPos(xTarget,xya.x,5,angleTarget,xya.a);
+        xGood=motion.setXPos(xTarget,xya.x,2,0);
+        if(!xGood)
+            motion.setAngle(0,xya.a,angle_error,ANGLE_BIAS);
+        else {
+            xTarget=(xTarget==120)?30:120;
+        }
             //motion.setYBias(0,xya.y,2,angleTarget);
-        //loc.get_xy(&xya);
+            //loc.get_xy(&xya);
 #if defined(PC_MODE)
-        pc.printf("X: %3d\tY: %3d\tP: %3d\txGood: %d\tAngleGood: %d\tState: %d \n",xya.y,yTarget,xya.a,xGood,angleGood,xState);
+            pc.printf("X: %3d\tY: %3d\tP: %3d\txGood: %d\tAngleGood: %d\tState: %d \n",xya.y,yTarget,xya.a,xGood,angleGood,xState);
 #endif
         //Feed the dog
         safe.kick();
@@ -127,7 +132,7 @@
 void send()
 {
     //Print over serial port to WiFi MCU
-    pc.printf("%c%c%c%c\n",(char)xya.x,(char)xGood,xya.a/10,xya.a%10);
+    pc.printf("%c%c%c%c\n",(char)xya.x,(char)xTarget,xya.a/10,xya.a%10);
 }
 
 void BrownOut()