Diff: LOCALIZE.cpp

Revision:

29:e8ef4a2e628d

Parent:

28:65daccc10f31

Child:

31:69caabc10879

--- a/LOCALIZE.cpp	Wed Apr 13 07:19:18 2016 +0000
+++ b/LOCALIZE.cpp	Wed Apr 13 07:48:49 2016 +0000
@@ -82,29 +82,29 @@
     get_angle(xya);
     ledSw();
     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;
+        _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)
             _rx=0;//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 : FRAME_W-RY_OFF-_ry_n;
-        _ry=_rx_p<_rx_n? FRAME_H-RX_OFF-_rx_p : _rx_n;
+        _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)
             _ry=FRAME_H;//-RY_OFF;
         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;
-        _ry=_ry_p<_ry_n? FRAME_H-RY_OFF-_ry_p : _ry_n;
+        _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)
             _rx=FRAME_W;//-RX_OFF;
         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=_rx_p<_rx_n? _rx_p : FRAME_H-RX_OFF-_rx_n;
+        _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)
             _ry=0;//RY_OFF;
         else if(!_sw3 && !_sw4)