Diff: main.cpp

Revision:

31:69caabc10879

Parent:

30:116cd143fdf7

Child:

33:baf24c59affc

--- a/main.cpp	Tue Apr 19 02:04:10 2016 +0000
+++ b/main.cpp	Wed Apr 20 08:21:21 2016 +0000
@@ -32,13 +32,13 @@
 void BrownOut();
 
 int xTarget=120;
-int angle_error=5;
+int angle_error=2;
 bool xGood=false;
 bool yGood=false;
 bool angleGood=false;
-int xState=X_INCREASE;
-int angleTarget=5;
-int yTarget=80;
+int xState=X_BEGINING;
+int angleTarget=180;
+int yTarget=ROB_SIZE/2;
 int pressure;
 
 //bool flag=false;
@@ -78,47 +78,33 @@
     led2=0;
     led3=0;
     led4=0;
-    suction.pulsewidth_us(1200);
+    suction.pulsewidth_us(1000);
     wait(0.5);
-    suction.pulsewidth_us(1300);
+    suction.pulsewidth_us(1050);
     wait(0.5);
-    suction.pulsewidth_us(1400);
+    suction.pulsewidth_us(1100);
     wait(0.5);
-    suction.pulsewidth_us(1500);
+    suction.pulsewidth_us(1150);
     wait(0.5);
-    suction.pulsewidth_us(1600);
+    suction.pulsewidth_us(1200);
     wait(0.5);
     while(1) {
-        suction.pulsewidth_us(1750);
-        //loc.get_angle(&xya);
-        loc.get_xy(&xya);
-        //pressure=(int)read[0];//(int)pres.getTemperature();
-        //if(pressure==88)
-        //    led1=1;
-        //else
-        //    led1=0;
-        /*if(motion.setAngle(5,xya.a,angle_error,ANGLE_TURN))
-        {
-            motion.setXPos(xTarget,xya.x,2,angleTarget);
-            motion.setYBias(yTarget,xya.y,2,angleTarget);
-        }*/
-            //motion.moveF(xya.a);
-        /*if(abs(xya.a-180)<90)
-            motion.moveB();
+        suction.pulsewidth_us(1250);
+        /*pressure=(int)read[0];//(int)pres.getTemperature();
+        if(pressure==88)
+            led1=1;
         else
-            motion.moveB();*/
-        //loc.get_xy(&xya);
-        
-        /*if (yTarget>120) {
+            led1=0;*/
+        /*uncomment this part if you want the robot to just drive down the window with no separtor
+        if (xState==0) {
             motion.mStop();
             safe.kick();
             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);
+        motion.setXstate(&xState,&xTarget,xGood,angleGood,&angleTarget,&angle_error,yTarget);
         motion.setAngleTol(&angle_error,yGood,xGood);
         motion.setYgoal(xGood,angleGood,yGood,&yTarget);
         if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) {