DERPS
Dependencies: BMP280 BNO055_fusion PowerControl mbed
Fork of STRAIGHT_DRIVE_NO_SEP by
Diff: LOCALIZE.cpp
- Revision:
- 20:10a305298e27
- Parent:
- 19:2dd81b864e14
- Child:
- 23:455f7da3dd7a
--- a/LOCALIZE.cpp Sun Apr 03 05:48:37 2016 +0000 +++ b/LOCALIZE.cpp Sun Apr 03 06:08:08 2016 +0000 @@ -41,7 +41,7 @@ { _imu.get_Euler_Angles(&_euler); _imu.get_gravity(&_gravity); - unsigned int quad=0; + volatile unsigned int quad=0; if(_gravity.y>=0) quad&=~1; else @@ -115,6 +115,12 @@ _rx=_xya.x; _ry=_xya.x; } + if(abs(_rx-_xya.x)>UR_JUMP) { + _rx=_xya.x; + } + if(abs(_ry-_xya.y)>UR_JUMP) { + _ry=_xya.y; + } _xya.x=_rx; _xya.y=_ry; xya->x=_xya.x; @@ -138,7 +144,7 @@ _ry_p=data[1]; } -void LOCALIZE::ledSw(void) +inline void LOCALIZE::ledSw(void) { _led1=!_sw1; _led2=!_sw2;