version2
Dependencies: BNO055_fusion mbed
Fork of DEMO2 by
Diff: LOCALIZE.cpp
- Revision:
- 4:d70375cfa533
- Parent:
- 3:c58eb4be51de
- Child:
- 5:c89308dc1827
diff -r c58eb4be51de -r d70375cfa533 LOCALIZE.cpp --- a/LOCALIZE.cpp Thu Mar 03 01:00:32 2016 +0000 +++ b/LOCALIZE.cpp Fri Mar 04 03:02:36 2016 +0000 @@ -1,10 +1,7 @@ #include "LOCALIZE.h" LOCALIZE::LOCALIZE (I2C& y_i2c, I2C& x_imu_i2c, PinName imu_reset): - _y_i2c(y_i2c), _x_imu_i2c(x_imu_i2c), _imu(x_imu_i2c, imu_reset) -{ - -} + _y_i2c(y_i2c), _x_imu_i2c(x_imu_i2c), _imu(x_imu_i2c, imu_reset) {} int LOCALIZE::init (void) { @@ -28,7 +25,6 @@ { _imu.get_Euler_Angles(&_euler); _imu.get_gravity(&_gravity); - int _angle=-1; unsigned int quad=0; if(_gravity.y>=0) quad&=~1; @@ -66,18 +62,19 @@ get_raw_xy(); get_angle(xya); if(_xya.a<R_ERROR || _xya.a>360-R_ERROR) { - _rx=_rx_n; - _ry=_ry_p; + _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) { - _rx=_ry_p; - _ry=FRAME_H-RX_OFF-_rx_p; + _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=FRAME_W-RX_OFF-_rx_p; - _ry=FRAME_H-RY_OFF-_ry_p; + _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) { - _rx=FRAME_W-RY_OFF-_ry_p; - _ry=_rx_p; + _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 { + //error _rx=_rx_p; _ry=_ry_p; } @@ -91,15 +88,15 @@ { char data[]= {0,0,0}; _x_imu_i2c.read(R_P_ADDR<<1, data, 3); - wait(0.1); + wait(R_WAIT); _rx_p=data[1]; _x_imu_i2c.read(R_N_ADDR<<1, data, 3); - wait(0.1); + wait(R_WAIT); _rx_n=data[1]; _y_i2c.read(R_P_ADDR<<1, data, 3); - wait(0.1); + wait(R_WAIT); _ry_p=data[1]; _y_i2c.read(R_N_ADDR<<1, data, 3); - wait(0.1); + wait(R_WAIT); _ry_n=data[1]; } \ No newline at end of file