DERPS
Dependencies: BMP280 BNO055_fusion PowerControl mbed
Fork of STRAIGHT_DRIVE_NO_SEP by
LOCALIZE.cpp
- Committer:
- 12104404
- Date:
- 2016-04-03
- Revision:
- 23:455f7da3dd7a
- Parent:
- 20:10a305298e27
- Child:
- 24:fb1f083ebd62
File content as of revision 23:455f7da3dd7a:
#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) { _sw1.mode(PullUp); _sw2.mode(PullUp); _sw3.mode(PullUp); _sw4.mode(PullUp); } int LOCALIZE::init (void) { if(_imu.chip_ready()==0) return ERROR_IMU; _imu.set_mounting_position(MT_P0); #if defined (IMU_GROUND) _imu.change_fusion_mode(MODE_NDOF); #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; 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; } void LOCALIZE::get_angle(LOCALIZE_xya *xya) { _imu.get_Euler_Angles(&_euler); _imu.get_gravity(&_gravity); volatile unsigned int quad=0; if(_gravity.y>=0) quad&=~1; else quad|=1; if(_gravity.x>=0) quad&=~(1<<1); else quad|=(1<<1); quad&=3; switch(quad) { case 0: _angle=_euler.p; break; case 1: _angle=180-_euler.p; break; case 2: _angle=360+_euler.p; break; case 3: _angle=180+abs(_euler.p); break; default: _angle=-1; break; } #if defined (IMU_GROUND) xya->a=_euler.h; #else xya->a=_angle; #endif _xya.a=xya->a; } void LOCALIZE::get_xy(LOCALIZE_xya *xya) { int _rx, _ry; get_raw_xy(); get_angle(xya); ledSw(); if(_xya.a<R_ERROR || _xya.a>360-R_ERROR) { _rx=_rx_p<_rx_n && _rx_p>UR_DEADZONE ? _rx_p : FRAME_W-RX_OFF-_rx_n; _ry=_ry_p<_ry_n && _ry_p>UR_DEADZONE ? _ry_p : FRAME_H-RY_OFF-_ry_n; if(!_sw1 && !_sw2) _rx=RX_OFF; else if(!_sw3 && !_sw4) _rx=FRAME_W-RX_OFF; } else if(abs(_xya.a-270)<R_ERROR) { _rx=_ry_p<_ry_n && _ry_p>UR_DEADZONE ? _ry_p : FRAME_W-RY_OFF-_ry_n; _ry=_rx_p<_rx_n && _rx_p>UR_DEADZONE ? FRAME_H-RX_OFF-_rx_p : _rx_n; if(!_sw1 && !_sw2) _ry=FRAME_H-RY_OFF; else if(!_sw3 && !_sw4) _ry=RY_OFF; } else if(abs(_xya.a-180)<R_ERROR) { _rx=_rx_p<_rx_n && _rx_p>UR_DEADZONE ? FRAME_W-RX_OFF-_rx_p : _rx_n; _ry=_ry_p<_ry_n && _ry_p>UR_DEADZONE ? FRAME_H-RY_OFF-_ry_p : _ry_n; if(!_sw1 && !_sw2) _rx=FRAME_W-RX_OFF; else if(!_sw3 && !_sw4) _rx=RX_OFF; } else if(abs(_xya.a-90)<R_ERROR) { _rx=_ry_p<_ry_n && _ry_p>UR_DEADZONE ? FRAME_W-RY_OFF-_ry_p : _ry_n; _ry=_rx_p<_rx_n && _rx_p>UR_DEADZONE ? _rx_p : FRAME_H-RX_OFF-_rx_n; if(!_sw1 && !_sw2) _ry=RY_OFF; else if(!_sw3 && !_sw4) _ry=FRAME_H-RY_OFF; } else { //error last value _rx=_xya.x; _ry=_xya.x; } if(abs(_rx-_xya.x)>UR_JUMP) { _rx=_xya.x; } if(abs(_ry-_xya.y)>UR_JUMP) { _ry=_xya.y; } _xya.x=_rx; _xya.y=_ry; xya->x=_xya.x; 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(R_WAIT); _rx_n=data[1]; _x_imu_i2c.read(R_N_ADDR<<1, data, 3); wait(R_WAIT); _rx_p=data[1]; _y_i2c.read(R_P_ADDR<<1, data, 3); wait(R_WAIT); _ry_n=data[1]; _y_i2c.read(R_N_ADDR<<1, data, 3); wait(R_WAIT); _ry_p=data[1]; } inline void LOCALIZE::ledSw(void) { _led1=!_sw1; _led2=!_sw2; _led3=!_sw3; _led4=!_sw4; }