LOCALIZE.cpp

00001 #include "LOCALIZE.h"
00002  
00003 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):
00004     _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)
00005 {
00006     _sw1.mode(PullUp);
00007     _sw2.mode(PullUp);
00008     _sw3.mode(PullUp);
00009     _sw4.mode(PullUp);
00010     _led2=1;
00011     wait(0.01);
00012     _led2=0;
00013 }
00014  
00015 int LOCALIZE::init (void)
00016 {
00017     _imu.reset();
00018     if(_imu.chip_ready()==0)
00019         return ERROR_IMU;
00020     _imu.set_mounting_position(MT_P4);
00021 #if defined (IMU_GROUND)
00022     _imu.change_fusion_mode(MODE_NDOF);
00023 #else
00024     _imu.change_fusion_mode(MODE_IMU);
00025 #endif
00026  
00027     char data[]= {0,0,0};
00028     if(_x_imu_i2c.read(R_P_ADDR<<1, data, 3)!=0)
00029         return ERROR_RX_P;
00030     if(_x_imu_i2c.read(R_N_ADDR<<1, data, 3)!=0)
00031         return ERROR_RX_N;
00032     if(_y_i2c.read(R_P_ADDR<<1, data, 3)!=0)
00033         return ERROR_RY_P;
00034     if(_y_i2c.read(R_N_ADDR<<1, data, 3)!=0)
00035         return ERROR_RY_N;
00036     return 0;
00037 }
00038  
00039 void LOCALIZE::get_angle(LOCALIZE_xya *xya)
00040 {
00041     _imu.get_Euler_Angles(&_euler);
00042     _imu.get_gravity(&_gravity);
00043     volatile unsigned int quad=0;
00044     if(_gravity.y>=0)
00045         quad&=~1;
00046     else
00047         quad|=1;
00048     if(_gravity.x>=0)
00049         quad&=~(1<<1);
00050     else
00051         quad|=(1<<1);
00052     quad&=3;
00053     switch(quad) {
00054         case 0:
00055             _angle=_euler.p;
00056             break;
00057         case 1:
00058             _angle=180-_euler.p;
00059             break;
00060         case 2:
00061             _angle=360+_euler.p;
00062             break;
00063         case 3:
00064             _angle=180+abs(_euler.p);
00065             break;
00066         default:
00067             _angle=-1;
00068             break;
00069     }
00070 #if defined (IMU_GROUND)
00071     xya->a=_euler.h;
00072 #else
00073     xya->a=_angle;
00074 #endif
00075     _xya.a=xya->a;
00076 }
00077  
00078 void LOCALIZE::get_xy(LOCALIZE_xya *xya)
00079 {
00080     int _rx, _ry;
00081     get_raw_xy();
00082     get_angle(xya);
00083     ledSw();
00084     if(_xya.a<R_ERROR || _xya.a>360-R_ERROR) {
00085         _rx=_rx_p<_rx_n? _rx_p+RX_OFF : FRAME_W-RX_OFF-_rx_n;
00086         _ry=_ry_p<_ry_n? _ry_p+RY_OFF : FRAME_H-RY_OFF-_ry_n;
00087         if(!_sw1 || !_sw2)
00088             _rx=0;//RX_OFF;
00089         else if(!_sw3 || !_sw4)
00090             _rx=FRAME_W;//-RX_OFF;
00091     } else if(abs(_xya.a-270)<R_ERROR) {
00092         _rx=_ry_p<_ry_n? _ry_p+RY_OFF : FRAME_W-RY_OFF-_ry_n;
00093         _ry=_rx_p<_rx_n? FRAME_H-RX_OFF-_rx_p : _rx_n+RX_OFF;
00094         if(!_sw1 || !_sw2)
00095             _ry=FRAME_H;//-RY_OFF;
00096         else if(!_sw3 || !_sw4)
00097             _ry=0;//RY_OFF;
00098     } else if(abs(_xya.a-180)<R_ERROR) {
00099         _rx=_rx_p<_rx_n? FRAME_W-RX_OFF-_rx_p : _rx_n+RX_OFF;
00100         _ry=_ry_p<_ry_n? FRAME_H-RY_OFF-_ry_p : _ry_n+RY_OFF;
00101         if(!_sw1 || !_sw2)
00102             _rx=FRAME_W;//-RX_OFF;
00103         else if(!_sw3 || !_sw4)
00104             _rx=0;//RX_OFF;
00105     } else if(abs(_xya.a-90)<R_ERROR) {
00106         _rx=_ry_p<_ry_n? FRAME_W-RY_OFF-_ry_p : _ry_n+RY_OFF;
00107         _ry=_rx_p<_rx_n? _rx_p+RX_OFF : FRAME_H-RX_OFF-_rx_n;
00108         if(!_sw1 || !_sw2)
00109             _ry=0;//RY_OFF;
00110         else if(!_sw3 || !_sw4)
00111             _ry=FRAME_H;//-RY_OFF;
00112     } else {
00113         //error last value
00114         _rx=_xya.x;
00115         _ry=_xya.x;
00116     }
00117     if(abs(_rx-_xya.x)>UR_JUMP && jumpsX<UR_JUMPS) {
00118         _rx=_xya.x;
00119         jumpsX++;
00120     }
00121     else
00122         jumpsX=0;
00123     if(abs(_ry-_xya.y)>UR_JUMP && jumpsY<UR_JUMPS) {
00124         _ry=_xya.y;
00125         jumpsY++;
00126     }
00127     else
00128         jumpsY=0;
00129     _xya.x=_rx;
00130     _xya.y=_ry;
00131     xya->x=_xya.x;
00132     xya->y=_xya.y;
00133 }
00134  
00135 void LOCALIZE::get_raw_xy(void)
00136 {
00137     char data[]= {0,0,0};
00138     _x_imu_i2c.read(R_P_ADDR<<1, data, 3);
00139     wait(R_WAIT);
00140     _rx_n=data[1];
00141     _x_imu_i2c.read(R_N_ADDR<<1, data, 3);
00142     wait(R_WAIT);
00143     _rx_p=data[1];
00144     _y_i2c.read(R_P_ADDR<<1, data, 3);
00145     wait(R_WAIT);
00146     _ry_n=data[1];
00147     _y_i2c.read(R_N_ADDR<<1, data, 3);
00148     wait(R_WAIT);
00149     _ry_p=data[1];
00150 }
00151  
00152 inline void LOCALIZE::ledSw(void)
00153 {
00154     _led1=!_sw1;
00155     _led2=!_sw2;
00156     _led3=!_sw3;
00157     _led4=!_sw4;
00158 }