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 }