DERPS
Dependencies: BMP280 BNO055_fusion PowerControl mbed
Fork of STRAIGHT_DRIVE_NO_SEP by
Diff: LOCALIZE.cpp
- Revision:
- 2:634c1adf89b2
- Parent:
- 1:8966a48ce8d5
- Child:
- 3:c58eb4be51de
diff -r 8966a48ce8d5 -r 634c1adf89b2 LOCALIZE.cpp --- a/LOCALIZE.cpp Tue Mar 01 19:26:32 2016 +0000 +++ b/LOCALIZE.cpp Wed Mar 02 07:21:05 2016 +0000 @@ -15,8 +15,12 @@ char data[]= {0,0,0}; if(_x_imu_i2c.read(R_P_ADDR<<1, data, 3)!=0) return ERROR_RX_P; + if(_x_imu_i2c.read(R_N_ADDR<<1, data, 3)!=0) + return ERROR_RX_N; if(_y_i2c.read(R_P_ADDR<<1, data, 3)!=0) return ERROR_RY_P; + if(_y_i2c.read(R_N_ADDR<<1, data, 3)!=0) + return ERROR_RY_N; return 0; } @@ -59,18 +63,24 @@ void LOCALIZE::get_xy(LOCALIZE_xya *xya) { char data[]= {0,0,0}; - int _rx_p, _ry_p, _rx, _ry; + 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 get_angle(xya); if(_xya.a<R_ERROR || _xya.a>360-R_ERROR) { - _rx=_rx_p; + _rx=_rx_n; _ry=_ry_p; } else if(abs(_xya.a-90)<R_ERROR) { _rx=_ry_p;