Revision 9:4fc5d70c3bab, committed 2016-03-23

Comitter:

12104404

Date:

Wed Mar 23 07:53:32 2016 +0000

Parent:

8:b36be08c44f8

Child:

10:cf44f4387bc7

Commit message:

GOOD WORKING

Changed in this revision

--- a/LOCALIZE.cpp	Wed Mar 23 06:55:39 2016 +0000
+++ b/LOCALIZE.cpp	Wed Mar 23 07:53:32 2016 +0000
@@ -67,13 +67,13 @@
     if(_xya.a<R_ERROR || _xya.a>360-R_ERROR) {
         _rx=_rx_p<_rx_n ? _rx_p : FRAME_W-RX_OFF-_rx_n;
         _ry=_ry_p<_ry_n ? _ry_p : FRAME_H-RY_OFF-_ry_n;
-    } else if(abs(_xya.a-90)<R_ERROR) {
+    } else if(abs(_xya.a-270)<R_ERROR) {
         _rx=_ry_p<_ry_n ? _ry_p : FRAME_W-RY_OFF-_ry_n;
         _ry=_rx_p<_rx_n ? FRAME_H-RX_OFF-_rx_p : _rx_n;
     } else if(abs(_xya.a-180)<R_ERROR) {
         _rx=_rx_p<_rx_n ? FRAME_W-RX_OFF-_rx_p : _rx_n;
         _ry=_ry_p<_ry_n ? FRAME_H-RY_OFF-_ry_p : _ry_n;
-    } else if(abs(_xya.a-270)<R_ERROR) {
+    } else if(abs(_xya.a-90)<R_ERROR) {
         _rx=_ry_p<_ry_n ? FRAME_W-RY_OFF-_ry_p : _ry_n;
         _ry=_rx_p<_rx_n ? _rx_p : FRAME_H-RX_OFF-_rx_n; 
     } else {

--- a/main.cpp	Wed Mar 23 06:55:39 2016 +0000
+++ b/main.cpp	Wed Mar 23 07:53:32 2016 +0000
@@ -56,10 +56,6 @@
     sw2.mode(PullUp);
     sw3.mode(PullUp);
     sw4.mode(PullUp);
-    /*while(abs(xya.a-180)>5) {
-        loc.get_angle(&xya);
-        turn(180);
-    }*/
     while(1) {
         led1=!sw1;
         led2=!sw2;
@@ -68,58 +64,7 @@
         //loc.get_angle(&xya);
         loc.get_xy(&xya);
         //loc.get_raw_xy();
-        //pc.printf("%c%c%c%c\n",xya.x,xya.y,xya.a/10,xya.a%10);
         //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
-        /*if(loc._rx_n>20) {
-            dir1=1;
-            dir2=1;
-        } else if(loc._rx_n<10) {
-            dir1=0;
-            dir2=0;
-        }*/
-/*
-        if((!sw1==1 && !sw2==1) && flag) {
-            motor1F=0.3;
-            motor1B=0.3;
-            motor2F=0.3;
-            motor2B=0.3;
-            dir1=0;
-            dir2=0;
-            wait(0.5);
-            while((xya.a>5 && xya.a<355)) {
-                loc.get_angle(&xya);
-                turn(0);
-            }
-            flag=false;
-        } else if((!sw1==1 && !sw2==1) && !flag) {
-            motor1F=0.3;
-            motor1B=0.3;
-            motor2F=0.3;
-            motor2B=0.3;
-            dir1=0;
-            dir2=0;
-            wait(0.5);
-            while(abs(xya.a-180)>=5) {
-                loc.get_angle(&xya);
-                turn(180);
-            }
-            flag=true;
-        } else {
-            float s=0;
-            if(loc._rx_n<=10)
-                s=0.1;
-            else if(loc._rx_n<=30)
-                s=0.4*abs(loc._rx_n-30)/30+0.1;
-            else
-                s=0.5;
-            motor1F=s;
-            motor1B=s;
-            motor2F=s;
-            motor2B=s;
-            dir1=1;
-            dir2=1;
-        }
-*/
         wdt.kick();
     }
 }