Mekatronics Team G
Dependencies: BNO055_fusion PowerControl mbed BMP280
Fork of DEMO3 by
Diff: LOCALIZE.cpp
- Revision:
- 26:0ea6a550a99d
- Parent:
- 25:f3bf8351bbd4
- Child:
- 28:65daccc10f31
--- a/LOCALIZE.cpp Tue Apr 05 02:30:40 2016 +0000 +++ b/LOCALIZE.cpp Wed Apr 06 04:04:00 2016 +0000 @@ -1,5 +1,5 @@ #include "LOCALIZE.h" - + LOCALIZE::LOCALIZE (I2C& y_i2c, I2C& x_imu_i2c, PinName imu_reset, PinName sw1, PinName sw2, PinName sw3, PinName sw4, DigitalOut& led1, DigitalOut& led2, DigitalOut& led3, DigitalOut& led4): _y_i2c(y_i2c), _x_imu_i2c(x_imu_i2c), _imu(x_imu_i2c, imu_reset), _sw1(sw1), _sw2(sw2), _sw3(sw3), _sw4(sw4), _led1(led1), _led2(led2), _led3(led3), _led4(led4) { @@ -11,9 +11,10 @@ wait(0.01); _led2=0; } - + int LOCALIZE::init (void) { + _imu.reset(); if(_imu.chip_ready()==0) return ERROR_IMU; _imu.set_mounting_position(MT_P0); @@ -22,7 +23,7 @@ #else _imu.change_fusion_mode(MODE_IMU); #endif - + char data[]= {0,0,0}; if(_x_imu_i2c.read(R_P_ADDR<<1, data, 3)!=0) return ERROR_RX_P; @@ -34,7 +35,7 @@ return ERROR_RY_N; return 0; } - + void LOCALIZE::get_angle(LOCALIZE_xya *xya) { _imu.get_Euler_Angles(&_euler); @@ -73,7 +74,7 @@ #endif _xya.a=xya->a; } - + void LOCALIZE::get_xy(LOCALIZE_xya *xya) { int _rx, _ry; @@ -84,30 +85,30 @@ _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; if(!_sw1 && !_sw2) - _rx=RX_OFF; + _rx=0;//RX_OFF; else if(!_sw3 && !_sw4) - _rx=FRAME_W-RX_OFF; + _rx=FRAME_W;//-RX_OFF; } else if(abs(_xya.a-270)<R_ERROR) { _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; if(!_sw1 && !_sw2) - _ry=FRAME_H-RY_OFF; + _ry=FRAME_H;//-RY_OFF; else if(!_sw3 && !_sw4) - _ry=RY_OFF; + _ry=0;//RY_OFF; } else if(abs(_xya.a-180)<R_ERROR) { _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; if(!_sw1 && !_sw2) - _rx=FRAME_W-RX_OFF; + _rx=FRAME_W;//-RX_OFF; else if(!_sw3 && !_sw4) - _rx=RX_OFF; + _rx=0;//RX_OFF; } else if(abs(_xya.a-90)<R_ERROR) { _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; if(!_sw1 && !_sw2) - _ry=RY_OFF; + _ry=0;//RY_OFF; else if(!_sw3 && !_sw4) - _ry=FRAME_H-RY_OFF; + _ry=FRAME_H;//-RY_OFF; } else { //error last value _rx=_xya.x; @@ -130,7 +131,7 @@ xya->x=_xya.x; xya->y=_xya.y; } - + void LOCALIZE::get_raw_xy(void) { char data[]= {0,0,0}; @@ -147,7 +148,7 @@ wait(R_WAIT); _ry_p=data[1]; } - + inline void LOCALIZE::ledSw(void) { _led1=!_sw1;