File content as of revision 0:96d6eb224379:

#include "LOCALIZE.h"

#define RANGE_ADDR  0x04
#define FRAME_H     180
#define FRAME_W     150
#define RANGE_X_OFF 7
#define RANGE_Y_OFF 6
#define RANGE_ERROR 14
#define RANGE_DIFF  10
#define RANGE_JMPS  4

//Serial pc(p13, p14);
Serial pc(USBTX, USBRX);

I2C i2c1(p28, p27);
I2C i2c2(p9, p10);
LOCALIZE loc(i2c1, i2c2, p26);
LOCALIZE_xya xya;

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

//BNO055_ID_INF_TypeDef bno055_id_inf;
//BNO055_EULER_TypeDef  euler;
//BNO055_GRAVITY_TypeDef gravity;
//BNO055_LIN_ACC_TypeDef accel;
int range_px=-1;
int range_py=-1;
int range_x=-1;
int range_y=-1;
int p_x=-1;
int p_y=-1;
int p_angle=0;
int err=-1;
int range_jmps_y=RANGE_JMPS;
int range_jmps_x=RANGE_JMPS;

//void x_y()
//{
//    err=0;
//    if(p_angle<RANGE_ERROR || p_angle>360-RANGE_ERROR) {
//        p_x=range_x;
//        p_y=range_y;
//    } else if(abs(p_angle-90)<RANGE_ERROR) {
//        p_x=range_y;
//        p_y=FRAME_H-range_x-RANGE_X_OFF;
//    } else if(abs(p_angle-180)<RANGE_ERROR) {
//        p_x=FRAME_W-range_x-RANGE_X_OFF;
//        p_y=FRAME_H-range_y-RANGE_Y_OFF;
//    } else if(abs(p_angle-270)<RANGE_ERROR) {
//        p_x=FRAME_W-range_y-RANGE_Y_OFF;
//        p_y=range_x;
//    } else {
//        err=-1;
//        p_x=range_x;
//        p_y=range_y;
//    }
//    if(p_x>=FRAME_W)
//        p_x=FRAME_W;
//    if(p_y>=FRAME_H)
//        p_y=FRAME_H;
//}
//
//void jump()
//{
//    if(abs(range_x-range_px)<RANGE_DIFF || range_jmps_x>RANGE_JMPS) {
//        if(range_x>0) {
//            range_px=range_x;
//        } else {
//            range_x=range_px;
//        }
//        range_jmps_x=0;
//    } else {
//        range_jmps_x++;
//    }
//
//    if(abs(range_y-range_py)<RANGE_DIFF || range_jmps_y>RANGE_JMPS) {
//        if(range_y>0) {
//            range_py=range_y;
//        } else {
//            range_y=range_py;
//        }
//        range_jmps_y=0;
//    } else {
//        range_jmps_y++;
//    }
//}

int main()
{
    pc.baud(9600);
    pc.printf("IMU INIT %d\n",loc.init());
    while(1) {
        loc.get_angle(&xya);
        pc.printf("Angle: %d\n",xya.a);
    }
    /*if (imu.chip_ready()==0) {
        //pc.printf("BNO055 is NOT available!\n");
    }*/
    //imu.set_mounting_position(MT_P0);
    //imu.change_fusion_mode(MODE_IMU);
    //p_x=location.x;
//    while(1) {
//        char data[]= {0,0,0};
//        i2c1.read(RANGE_ADDR<<1, data, 3);
//        wait(0.05);
//        range_y=data[1];
//        i2c2.read(RANGE_ADDR<<1, data, 3);
//        wait(0.05);
//        //imu.get_linear_accel(&accel);
//        range_x=data[1];
//        //p_angle=get_angle();
//        //jump();
//        //x_y();
//        //pc.printf("X: %3d\tY: %3d\tP: %3d\tERROR:%3d\n",p_x,p_y,p_angle,err);
//        pc.printf("%c%c%c%c\n",p_x,p_y,p_angle/10,p_angle%10);
//        wait(0.5);
//    }
}