File content as of revision 40:9a97c4403c0a:

#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);
    _led2=1;
    wait(0.01);
    _led2=0;
}
 
int LOCALIZE::init (void)
{
    _imu.reset();
    if(_imu.chip_ready()==0)
        return ERROR_IMU;
    _imu.set_mounting_position(MT_P4);
#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+RX_OFF : FRAME_W-RX_OFF-_rx_n;
        _ry=_ry_p<_ry_n? _ry_p+RY_OFF : FRAME_H-RY_OFF-_ry_n;
        if(!_sw1 || !_sw2)
            _rx=0;//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+RY_OFF : FRAME_W-RY_OFF-_ry_n;
        _ry=_rx_p<_rx_n? FRAME_H-RX_OFF-_rx_p : _rx_n+RX_OFF;
        if(!_sw1 || !_sw2)
            _ry=FRAME_H;//-RY_OFF;
        else if(!_sw3 || !_sw4)
            _ry=0;//RY_OFF;
    } else if(abs(_xya.a-180)<R_ERROR) {
        _rx=_rx_p<_rx_n? FRAME_W-RX_OFF-_rx_p : _rx_n+RX_OFF;
        _ry=_ry_p<_ry_n? FRAME_H-RY_OFF-_ry_p : _ry_n+RY_OFF;
        if(!_sw1 || !_sw2)
            _rx=FRAME_W;//-RX_OFF;
        else if(!_sw3 || !_sw4)
            _rx=0;//RX_OFF;
    } else if(abs(_xya.a-90)<R_ERROR) {
        _rx=_ry_p<_ry_n? FRAME_W-RY_OFF-_ry_p : _ry_n+RY_OFF;
        _ry=_rx_p<_rx_n? _rx_p+RX_OFF : FRAME_H-RX_OFF-_rx_n;
        if(!_sw1 || !_sw2)
            _ry=0;//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 && jumpsX<UR_JUMPS) {
        _rx=_xya.x;
        jumpsX++;
    }
    else
        jumpsX=0;
    if(abs(_ry-_xya.y)>UR_JUMP && jumpsY<UR_JUMPS) {
        _ry=_xya.y;
        jumpsY++;
    }
    else
        jumpsY=0;
    _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;
}