Diff: LOCALIZE.cpp

Revision:

25:f3bf8351bbd4

Parent:

24:fb1f083ebd62

Child:

26:0ea6a550a99d

--- a/LOCALIZE.cpp	Sun Apr 03 07:25:07 2016 +0000
+++ b/LOCALIZE.cpp	Tue Apr 05 02:30:40 2016 +0000
@@ -81,29 +81,29 @@
     get_angle(xya);
     ledSw();
     if(_xya.a<R_ERROR || _xya.a>360-R_ERROR) {
-        _rx=_rx_p<_rx_n && _rx_p>UR_DEADZONE ? _rx_p : FRAME_W-RX_OFF-_rx_n;
-        _ry=_ry_p<_ry_n && _ry_p>UR_DEADZONE ? _ry_p : FRAME_H-RY_OFF-_ry_n;
+        _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;
         if(!_sw1 && !_sw2)
             _rx=RX_OFF;
         else if(!_sw3 && !_sw4)
             _rx=FRAME_W-RX_OFF;
     } else if(abs(_xya.a-270)<R_ERROR) {
-        _rx=_ry_p<_ry_n && _ry_p>UR_DEADZONE ? _ry_p : FRAME_W-RY_OFF-_ry_n;
-        _ry=_rx_p<_rx_n && _rx_p>UR_DEADZONE ? FRAME_H-RX_OFF-_rx_p : _rx_n;
+        _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;
         if(!_sw1 && !_sw2)
             _ry=FRAME_H-RY_OFF;
         else if(!_sw3 && !_sw4)
             _ry=RY_OFF;
     } else if(abs(_xya.a-180)<R_ERROR) {
-        _rx=_rx_p<_rx_n && _rx_p>UR_DEADZONE ? FRAME_W-RX_OFF-_rx_p : _rx_n;
-        _ry=_ry_p<_ry_n && _ry_p>UR_DEADZONE ? FRAME_H-RY_OFF-_ry_p : _ry_n;
+        _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;
         if(!_sw1 && !_sw2)
             _rx=FRAME_W-RX_OFF;
         else if(!_sw3 && !_sw4)
             _rx=RX_OFF;
     } else if(abs(_xya.a-90)<R_ERROR) {
-        _rx=_ry_p<_ry_n && _ry_p>UR_DEADZONE ? FRAME_W-RY_OFF-_ry_p : _ry_n;
-        _ry=_rx_p<_rx_n && _rx_p>UR_DEADZONE ? _rx_p : FRAME_H-RX_OFF-_rx_n;
+        _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;
         if(!_sw1 && !_sw2)
             _ry=RY_OFF;
         else if(!_sw3 && !_sw4)
@@ -113,12 +113,18 @@
         _rx=_xya.x;
         _ry=_xya.x;
     }
-    if(abs(_rx-_xya.x)>UR_JUMP) {
+    if(abs(_rx-_xya.x)>UR_JUMP && jumpsX<UR_JUMPS) {
         _rx=_xya.x;
+        jumpsX++;
     }
-    if(abs(_ry-_xya.y)>UR_JUMP) {
+    else
+        jumpsX=0;
+    if(abs(_ry-_xya.y)>UR_JUMP && jumpsY<UR_JUMPS) {
         _ry=_xya.y;
+        jumpsY++;
     }
+    else
+        jumpsY=0;
     _xya.x=_rx;
     _xya.y=_ry;
     xya->x=_xya.x;