version2
Dependencies: BNO055_fusion mbed
Fork of DEMO2 by
Diff: LOCALIZE.cpp
- Revision:
- 1:8966a48ce8d5
- Parent:
- 0:96d6eb224379
- Child:
- 2:634c1adf89b2
--- a/LOCALIZE.cpp Tue Mar 01 18:20:45 2016 +0000 +++ b/LOCALIZE.cpp Tue Mar 01 19:26:32 2016 +0000 @@ -1,7 +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_reset(imu_reset), _imu(x_imu_i2c, imu_reset) + _y_i2c(y_i2c), _x_imu_i2c(x_imu_i2c), _imu(x_imu_i2c, imu_reset) { } @@ -12,40 +12,82 @@ return ERROR_IMU; _imu.set_mounting_position(MT_P0); _imu.change_fusion_mode(MODE_IMU); + char data[]= {0,0,0}; + if(_x_imu_i2c.read(R_P_ADDR<<1, data, 3)!=0) + return ERROR_RX_P; + if(_y_i2c.read(R_P_ADDR<<1, data, 3)!=0) + return ERROR_RY_P; return 0; } void LOCALIZE::get_angle(LOCALIZE_xya *xya) { - _imu.get_Euler_Angles(&euler); - _imu.get_gravity(&gravity); - int angle=0; + _imu.get_Euler_Angles(&_euler); + _imu.get_gravity(&_gravity); + int _angle=-1; unsigned int quad=0; - if(gravity.y>=0) + if(_gravity.y>=0) quad&=~1; else quad|=1; - if(gravity.x>=0) + if(_gravity.x>=0) quad&=~(1<<1); else quad|=(1<<1); quad&=3; switch(quad) { case 0: - angle=euler.p; + _angle=_euler.p; break; case 1: - angle=180-euler.p; + _angle=180-_euler.p; break; case 2: - angle=360+euler.p; + _angle=360+_euler.p; break; case 3: - angle=180+abs(euler.p); + _angle=180+abs(_euler.p); break; default: - angle=-1; + _angle=-1; break; } - xya->a=angle; + _xya.a=_angle; + xya->a=_xya.a; } + +void LOCALIZE::get_xy(LOCALIZE_xya *xya) +{ + char data[]= {0,0,0}; + int _rx_p, _ry_p, _rx, _ry; + _x_imu_i2c.read(R_P_ADDR<<1, data, 3); + //longer wait for longer distance + wait(0.1); + _rx_p=data[1]; + _y_i2c.read(R_P_ADDR<<1, data, 3); + wait(0.1); + _ry_p=data[1]; + //check angle again + get_angle(xya); + if(_xya.a<R_ERROR || _xya.a>360-R_ERROR) { + _rx=_rx_p; + _ry=_ry_p; + } else if(abs(_xya.a-90)<R_ERROR) { + _rx=_ry_p; + _ry=FRAME_H-RX_OFF-_rx_p; + } else if(abs(_xya.a-180)<R_ERROR) { + _rx=FRAME_W-RX_OFF-_rx_p; + _ry=FRAME_H-RY_OFF-_ry_p; + } else if(abs(_xya.a-270)<R_ERROR) { + _rx=FRAME_W-RY_OFF-_ry_p; + _ry=_rx_p; + } else { + _rx=_rx_p; + _ry=_ry_p; + } + _xya.x=_rx; + _xya.y=_ry; + xya->x=_xya.x; + xya->y=_xya.y; +} +