Diff: main.cpp

Revision:

38:16208e003dc9

Parent:

37:e8a6ea255c09

Child:

39:ecc9defe3dc0

Child:

40:9a97c4403c0a

--- a/main.cpp	Tue Apr 26 20:09:25 2016 +0000
+++ b/main.cpp	Wed Apr 27 04:24:11 2016 +0000
@@ -24,14 +24,13 @@
 I2C i2c2(p28, p27);
 I2C i2c1(p9, p10);
 BMP280 pres(i2c2);
-LOCALIZE loc(i2c1, i2c2, p29, p8, p7, p6, p5, led1, led2, led3, led4);
+LOCALIZE loc(i2c1, i2c2, p29, p5, p6, p7, p8, led1, led2, led3, led4);
 LOCALIZE_xya xya;
 LOCOMOTION motion(p11, p23, p24, p21, p22, p16, p15, led1, led2, led3, led4);
-//LOCOMOTION motion(p11, p21, p22, p23, p24, p15, p16, led1, led2, led3, led4);
 
 void BrownOut();
 
-int xTarget=120;
+int xTarget=FRAME_W;
 int angle_error=1;
 bool xGood=false;
 bool yGood=false;
@@ -93,33 +92,26 @@
     wait(0.5);
     while(1) {
         suction.pulsewidth_us(1285);
-        /*pressure=(int)read[0];//(int)pres.getTemperature();
-        if(pressure==88)
-           led1=1;
-        else
-           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;
+        if (xya.y>FRAME_H*0.65) {
+            while(1)
+            {
+                suction.pulsewidth_us(1285);
+                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,yTarget);
-        //motion.setAngleTol(&angle_error,yGood,xGood);
-        // 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(angleTarget,xya.a,angle_error,ANGLE_BIAS);
         else {
-            xTarget=(xTarget==120)?30:120;
+            xTarget=(xTarget==FRAME_W)?0:FRAME_W;
             angleTarget=(angleTarget==5)?-5:5;
         }
             //motion.setYBias(0,xya.y,2,angleTarget);
-            //loc.get_xy(&xya);
+            //loc.get_xy(&xya);5
 #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);
 #endif
@@ -131,7 +123,7 @@
 void send()
 {
     //Print over serial port to WiFi MCU
-    pc.printf("%c%c%c%c\n",(char)xya.x,(char)xTarget,xya.a/10,xya.a%10);
+    pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10);
 }
 
 void BrownOut()