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