drive down
Dependencies: BMP280 BNO055_fusion PowerControl mbed
Fork of TEAM_G_FLOW_RIDA by
Diff: LOCALIZE.cpp
- Revision:
- 9:4fc5d70c3bab
- Parent:
- 8:b36be08c44f8
- Child:
- 10:cf44f4387bc7
--- 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 {