DERPS
Dependencies: BMP280 BNO055_fusion PowerControl mbed
Fork of STRAIGHT_DRIVE_NO_SEP by
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;