![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
added
Dependencies: BNO055_fusion PowerControl mbed
Fork of TEAM_G_FLOW_RIDA by
Diff: LOCALIZE.cpp
- Revision:
- 26:5ae5ef623b72
- Parent:
- 25:f3bf8351bbd4
diff -r f3bf8351bbd4 -r 5ae5ef623b72 LOCALIZE.cpp --- a/LOCALIZE.cpp Tue Apr 05 02:30:40 2016 +0000 +++ b/LOCALIZE.cpp Wed Apr 06 03:54:29 2016 +0000 @@ -84,30 +84,30 @@ _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; + _rx=0;//RX_OFF; else if(!_sw3 && !_sw4) - _rx=FRAME_W-RX_OFF; + _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; if(!_sw1 && !_sw2) - _ry=FRAME_H-RY_OFF; + _ry=FRAME_H;//-RY_OFF; else if(!_sw3 && !_sw4) - _ry=RY_OFF; + _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; if(!_sw1 && !_sw2) - _rx=FRAME_W-RX_OFF; + _rx=FRAME_W;//-RX_OFF; else if(!_sw3 && !_sw4) - _rx=RX_OFF; + _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; if(!_sw1 && !_sw2) - _ry=RY_OFF; + _ry=0;//RY_OFF; else if(!_sw3 && !_sw4) - _ry=FRAME_H-RY_OFF; + _ry=FRAME_H;//-RY_OFF; } else { //error last value _rx=_xya.x;