File content as of revision 1:8966a48ce8d5:

#include "LOCALIZE.h"

#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);

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;

int main()
{
    pc.baud(9600);
    pc.printf("IMU INIT %d\n",loc.init());
    while(1) {
        //loc.get_angle(&xya);
        loc.get_xy(&xya);
        pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,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);
//    }
}