version2
Dependencies: BNO055_fusion mbed
Fork of DEMO2 by
LOCALIZE.cpp
00001 #include "LOCALIZE.h" 00002 00003 DigitalOut led1(LED1); 00004 DigitalOut led2(LED2); 00005 DigitalOut led3(LED3); 00006 DigitalOut led4(LED4); 00007 00008 LOCALIZE::LOCALIZE (I2C& y_i2c, I2C& x_imu_i2c, PinName imu_reset, PinName sw1, PinName sw2, PinName sw3, PinName sw4): 00009 _y_i2c(y_i2c), _x_imu_i2c(x_imu_i2c), _imu(x_imu_i2c, imu_reset), _sw1(sw1), _sw2(sw2), _sw3(sw3), _sw4(sw4) 00010 { 00011 _sw1.mode(PullUp); 00012 _sw2.mode(PullUp); 00013 _sw3.mode(PullUp); 00014 _sw4.mode(PullUp); 00015 } 00016 00017 int LOCALIZE::init (void) 00018 { 00019 if(_imu.chip_ready()==0) 00020 return ERROR_IMU; 00021 _imu.set_mounting_position(MT_P0); 00022 //_imu.change_fusion_mode(MODE_IMU); 00023 _imu.change_fusion_mode(MODE_NDOF); 00024 char data[]= {0,0,0}; 00025 if(_x_imu_i2c.read(R_P_ADDR<<1, data, 3)!=0) 00026 return ERROR_RX_P; 00027 if(_x_imu_i2c.read(R_N_ADDR<<1, data, 3)!=0) 00028 return ERROR_RX_N; 00029 if(_y_i2c.read(R_P_ADDR<<1, data, 3)!=0) 00030 return ERROR_RY_P; 00031 if(_y_i2c.read(R_N_ADDR<<1, data, 3)!=0) 00032 return ERROR_RY_N; 00033 return 0; 00034 } 00035 00036 void LOCALIZE::get_angle(LOCALIZE_xya *xya) 00037 { 00038 _imu.get_Euler_Angles(&_euler); 00039 _imu.get_gravity(&_gravity); 00040 unsigned int quad=0; 00041 if(_gravity.y>=0) 00042 quad&=~1; 00043 else 00044 quad|=1; 00045 if(_gravity.x>=0) 00046 quad&=~(1<<1); 00047 else 00048 quad|=(1<<1); 00049 quad&=3; 00050 switch(quad) { 00051 case 0: 00052 _angle=_euler.p; 00053 break; 00054 case 1: 00055 _angle=180-_euler.p; 00056 break; 00057 case 2: 00058 _angle=360+_euler.p; 00059 break; 00060 case 3: 00061 _angle=180+abs(_euler.p); 00062 break; 00063 default: 00064 _angle=-1; 00065 break; 00066 } 00067 //xya->a=_angle; 00068 xya->a=_euler.h; 00069 _xya.a=xya->a; 00070 } 00071 00072 void LOCALIZE::get_xy(LOCALIZE_xya *xya) 00073 { 00074 int _rx, _ry; 00075 get_raw_xy(); 00076 get_angle(xya); 00077 ledSw(); 00078 if(_xya.a<R_ERROR || _xya.a>360-R_ERROR) { 00079 _rx=_rx_p<_rx_n ? _rx_p : FRAME_W-RX_OFF-_rx_n; 00080 _ry=_ry_p<_ry_n ? _ry_p : FRAME_H-RY_OFF-_ry_n; 00081 if(!_sw1 && !_sw2) 00082 _rx=RX_OFF; 00083 else if(!_sw3 && !_sw4) 00084 _rx=FRAME_W-RX_OFF; 00085 } else if(abs(_xya.a-270)<R_ERROR) { 00086 _rx=_ry_p<_ry_n ? _ry_p : FRAME_W-RY_OFF-_ry_n; 00087 _ry=_rx_p<_rx_n ? FRAME_H-RX_OFF-_rx_p : _rx_n; 00088 if(!_sw1 && !_sw2) 00089 _ry=FRAME_H-RY_OFF; 00090 else if(!_sw3 && !_sw4) 00091 _ry=RY_OFF; 00092 } else if(abs(_xya.a-180)<R_ERROR) { 00093 _rx=_rx_p<_rx_n ? FRAME_W-RX_OFF-_rx_p : _rx_n; 00094 _ry=_ry_p<_ry_n ? FRAME_H-RY_OFF-_ry_p : _ry_n; 00095 if(!_sw1 && !_sw2) 00096 _rx=FRAME_W-RX_OFF; 00097 else if(!_sw3 && !_sw4) 00098 _rx=RX_OFF; 00099 } else if(abs(_xya.a-90)<R_ERROR) { 00100 _rx=_ry_p<_ry_n ? FRAME_W-RY_OFF-_ry_p : _ry_n; 00101 _ry=_rx_p<_rx_n ? _rx_p : FRAME_H-RX_OFF-_rx_n; 00102 if(!_sw1 && !_sw2) 00103 _ry=RY_OFF; 00104 else if(!_sw3 && !_sw4) 00105 _ry=FRAME_H-RY_OFF; 00106 } else { 00107 //error 00108 _rx=_xya.x; 00109 _ry=_xya.x; 00110 } 00111 _xya.x=_rx; 00112 _xya.y=_ry; 00113 xya->x=_xya.x; 00114 xya->y=_xya.y; 00115 } 00116 00117 void LOCALIZE::get_raw_xy(void) 00118 { 00119 char data[]= {0,0,0}; 00120 _x_imu_i2c.read(R_P_ADDR<<1, data, 3); 00121 wait(R_WAIT); 00122 _rx_n=data[1]; 00123 _x_imu_i2c.read(R_N_ADDR<<1, data, 3); 00124 wait(R_WAIT); 00125 _rx_p=data[1]; 00126 _y_i2c.read(R_P_ADDR<<1, data, 3); 00127 wait(R_WAIT); 00128 _ry_n=data[1]; 00129 _y_i2c.read(R_N_ADDR<<1, data, 3); 00130 wait(R_WAIT); 00131 _ry_p=data[1]; 00132 //printf("%d %d %d %d\n",_rx_p,_rx_n,_ry_p,_ry_n); 00133 } 00134 00135 void LOCALIZE::ledSw(void) 00136 { 00137 led1=!_sw1; 00138 led2=!_sw2; 00139 led3=!_sw3; 00140 led4=!_sw4; 00141 }
Generated on Wed Jul 27 2022 17:03:23 by 1.7.2