Revision 38:16208e003dc9, committed 2016-04-27

Comitter:

12104404

Date:

Wed Apr 27 04:24:11 2016 +0000

Parent:

37:e8a6ea255c09

Child:

39:ecc9defe3dc0

Child:

40:9a97c4403c0a

Commit message:

good good;

Changed in this revision

--- a/LOCALIZE.cpp	Tue Apr 26 20:09:25 2016 +0000
+++ b/LOCALIZE.cpp	Wed Apr 27 04:24:11 2016 +0000
@@ -84,30 +84,30 @@
     if(_xya.a<R_ERROR || _xya.a>360-R_ERROR) {
         _rx=_rx_p<_rx_n? _rx_p+RX_OFF : FRAME_W-RX_OFF-_rx_n;
         _ry=_ry_p<_ry_n? _ry_p+RY_OFF : FRAME_H-RY_OFF-_ry_n;
-        if(!_sw1 && !_sw2)
+        if(!_sw1 || !_sw2)
             _rx=0;//RX_OFF;
-        else if(!_sw3 && !_sw4)
+        else if(!_sw3 || !_sw4)
             _rx=FRAME_W;//-RX_OFF;
     } else if(abs(_xya.a-270)<R_ERROR) {
         _rx=_ry_p<_ry_n? _ry_p+RY_OFF : FRAME_W-RY_OFF-_ry_n;
         _ry=_rx_p<_rx_n? FRAME_H-RX_OFF-_rx_p : _rx_n+RX_OFF;
-        if(!_sw1 && !_sw2)
+        if(!_sw1 || !_sw2)
             _ry=FRAME_H;//-RY_OFF;
-        else if(!_sw3 && !_sw4)
+        else if(!_sw3 || !_sw4)
             _ry=0;//RY_OFF;
     } else if(abs(_xya.a-180)<R_ERROR) {
         _rx=_rx_p<_rx_n? FRAME_W-RX_OFF-_rx_p : _rx_n+RX_OFF;
         _ry=_ry_p<_ry_n? FRAME_H-RY_OFF-_ry_p : _ry_n+RY_OFF;
-        if(!_sw1 && !_sw2)
+        if(!_sw1 || !_sw2)
             _rx=FRAME_W;//-RX_OFF;
-        else if(!_sw3 && !_sw4)
+        else if(!_sw3 || !_sw4)
             _rx=0;//RX_OFF;
     } else if(abs(_xya.a-90)<R_ERROR) {
         _rx=_ry_p<_ry_n? FRAME_W-RY_OFF-_ry_p : _ry_n+RY_OFF;
         _ry=_rx_p<_rx_n? _rx_p+RX_OFF : FRAME_H-RX_OFF-_rx_n;
-        if(!_sw1 && !_sw2)
+        if(!_sw1 || !_sw2)
             _ry=0;//RY_OFF;
-        else if(!_sw3 && !_sw4)
+        else if(!_sw3 || !_sw4)
             _ry=FRAME_H;//-RY_OFF;
     } else {
         //error last value

--- a/LOCOMOTION.cpp	Tue Apr 26 20:09:25 2016 +0000
+++ b/LOCOMOTION.cpp	Wed Apr 27 04:24:11 2016 +0000
@@ -56,7 +56,7 @@
         s=SPEED_X_MIN;
     else
         s=((SPEED_X_MAX-SPEED_X_MIN)*abs(current-target)/FRAME_W)+SPEED_X_MIN;*/
-    if(abs(current-target)<25)
+    if(abs(current-target)<30)
         s=((SPEED_X_MAX-SPEED_X_MIN)*abs(current-target)/FRAME_W)+SPEED_X_MIN;
     else
         s=SPEED_X_MAX;

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