File content as of revision 34:083073e54dbd:

#ifndef LOCALIZE_H
#define LOCALIZE_H

#include "mbed.h"
#include "BNO055.h"

//Comment if not testing on ground
//#define IMU_GROUND      1

#define R_P_ADDR    0x04    //0x0B^0x0F=0x04
#define R_N_ADDR    0x0B

#define FRAME_OFF   0
#define FRAME_H     180
#define FRAME_W     150
#define R_WAIT      0.025
#define R_ERROR     14
#define RX_OFF      15
#define RY_OFF      13
#define UR_JUMP     25
#define UR_JUMPS    7
#define UR_DEADZONE 3

#define ERROR_IMU   -1
#define ERROR_RX_P  -2
#define ERROR_RX_N  -3
#define ERROR_RY_P  -4
#define ERROR_RY_N  -5

typedef struct {
    int x;
    int y;
    int a;
}LOCALIZE_xya;

class LOCALIZE
{
public:
    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);
    int init(void);
    void get_raw_xy(void);
    void get_angle(LOCALIZE_xya *xya);
    void get_xy(LOCALIZE_xya *xya);
    int _angle, _rx_p, _rx_n, _ry_p, _ry_n;//
    BNO055_EULER_TypeDef _euler;
    inline void ledSw(void);
    
protected:
    I2C _y_i2c;
    I2C _x_imu_i2c;
    BNO055 _imu;
    DigitalIn _sw1;
    DigitalIn _sw2;
    DigitalIn _sw3;
    DigitalIn _sw4;
    DigitalOut _led1;
    DigitalOut _led2;
    DigitalOut _led3;
    DigitalOut _led4;
    uint8_t jumpsX;
    uint8_t jumpsY;
    
    BNO055_GRAVITY_TypeDef _gravity;
    BNO055_LIN_ACC_TypeDef _accel;
    LOCALIZE_xya _xya;
    
};

#endif  //LOCALIZE_H