LOCALIZE.h

00001 #ifndef LOCALIZE_H
00002 #define LOCALIZE_H
00003 
00004 #include "mbed.h"
00005 #include "BNO055.h"
00006 
00007 //Comment if not testing on ground
00008 #define IMU_GROUND      1
00009 
00010 #define R_P_ADDR    0x04    //0x0B^0x0F=0x04
00011 #define R_N_ADDR    0x0B
00012 
00013 #define FRAME_OFF   0
00014 #define FRAME_H     150
00015 #define FRAME_W     104
00016 #define R_WAIT      0.025
00017 #define R_ERROR     14
00018 #define RX_OFF      7
00019 #define RY_OFF      6
00020 #define UR_JUMP     25
00021 #define UR_JUMPS    7
00022 #define UR_DEADZONE 3
00023 
00024 #define ERROR_IMU   -1
00025 #define ERROR_RX_P  -2
00026 #define ERROR_RX_N  -3
00027 #define ERROR_RY_P  -4
00028 #define ERROR_RY_N  -5
00029 
00030 typedef struct {
00031     int x;
00032     int y;
00033     int a;
00034 }LOCALIZE_xya;
00035 
00036 class LOCALIZE
00037 {
00038 public:
00039     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);
00040     int init(void);
00041     void get_raw_xy(void);
00042     void get_angle(LOCALIZE_xya *xya);
00043     void get_xy(LOCALIZE_xya *xya);
00044     int _angle, _rx_p, _rx_n, _ry_p, _ry_n;//
00045     BNO055_EULER_TypeDef _euler;
00046     inline void ledSw(void);
00047     
00048 protected:
00049     I2C _y_i2c;
00050     I2C _x_imu_i2c;
00051     BNO055 _imu;
00052     DigitalIn _sw1;
00053     DigitalIn _sw2;
00054     DigitalIn _sw3;
00055     DigitalIn _sw4;
00056     DigitalOut _led1;
00057     DigitalOut _led2;
00058     DigitalOut _led3;
00059     DigitalOut _led4;
00060     uint8_t jumpsX;
00061     uint8_t jumpsY;
00062     
00063     BNO055_GRAVITY_TypeDef _gravity;
00064     BNO055_LIN_ACC_TypeDef _accel;
00065     LOCALIZE_xya _xya;
00066     
00067 };
00068 
00069 #endif  //LOCALIZE_H