version2
Dependencies: BNO055_fusion mbed
Fork of DEMO2 by
Diff: LOCALIZE.cpp
- Revision:
- 3:c58eb4be51de
- Parent:
- 2:634c1adf89b2
- Child:
- 4:d70375cfa533
--- a/LOCALIZE.cpp Wed Mar 02 07:21:05 2016 +0000 +++ b/LOCALIZE.cpp Thu Mar 03 01:00:32 2016 +0000 @@ -62,22 +62,8 @@ void LOCALIZE::get_xy(LOCALIZE_xya *xya) { - char data[]= {0,0,0}; - int _rx_p, _ry_p, _rx_n, _ry_n, _rx, _ry; - _x_imu_i2c.read(R_P_ADDR<<1, data, 3); - //longer wait for longer distance - wait(0.1); - _rx_p=data[1]; - _x_imu_i2c.read(R_N_ADDR<<1, data, 3); - wait(0.1); - _rx_n=data[1]; - _y_i2c.read(R_P_ADDR<<1, data, 3); - wait(0.1); - _ry_p=data[1]; -// _y_i2c.read(R_N_ADDR<<1, data, 3); -// wait(0.1); -// _ry_n=data[1]; - //check angle again + int _rx, _ry; + get_raw_xy(); get_angle(xya); if(_xya.a<R_ERROR || _xya.a>360-R_ERROR) { _rx=_rx_n; @@ -101,3 +87,19 @@ xya->y=_xya.y; } +void LOCALIZE::get_raw_xy(void) +{ + char data[]= {0,0,0}; + _x_imu_i2c.read(R_P_ADDR<<1, data, 3); + wait(0.1); + _rx_p=data[1]; + _x_imu_i2c.read(R_N_ADDR<<1, data, 3); + wait(0.1); + _rx_n=data[1]; + _y_i2c.read(R_P_ADDR<<1, data, 3); + wait(0.1); + _ry_p=data[1]; + _y_i2c.read(R_N_ADDR<<1, data, 3); + wait(0.1); + _ry_n=data[1]; +} \ No newline at end of file