File content as of revision 7:27d1f57f8030:

#include "mbed.h"
#include "BNO055.h"
#define TS 0.01
using namespace std;

DigitalOut led1(LED1);
DigitalOut led4(LED4);
DigitalOut green(p21);
DigitalOut blue(p22);
DigitalOut red(p23);

Ticker int_tempo;
Serial pc(USBTX, USBRX);
BNO055 bno055(p9, p10);
InterruptIn interrupt(p15);

bool    flagi = false, flags=false;
float   acel_x, acel_y, acel_z,acel_x_ant,acel_y_ant,acel_z_ant;
int     conteo=0;

void event();
void tempo();

int main()
{
    //float   acel_x_act, acel_y_act, acel_z_act;
    float   vel_x=0, vel_y=0, vel_z=0;
    float   vel_x_act=0, vel_y_act=0, vel_z_act=0;

    float   pos_x=0, pos_y=0, pos_z=0;
    float   pos_x_act=0, pos_y_act=0, pos_z_act=0;
    uint32_t tant=0;

    green=0;
    blue=0;
    red=1;
    int_tempo.attach(&tempo, TS); // the address of the function to be attached (tempo) and the interval (0.1)
    bno055.reset();
    interrupt.rise(&event);
    bno055.setmode(OPERATION_MODE_NDOF);
    //bno055.write_calibration_data();
    bno055.get_calib();
    //bno055.write_calibration_data();

    while(((bno055.calib>>4)&3)!= 3) {
        red=1;
        blue=0;
        green=0;
        bno055.get_calib();
        wait_ms(50);
        red = 0;
        wait_ms(50);
        pc.printf("Calibrando giroscopio= %d\n",(bno055.calib>>4)&(3));
    }
    pc.printf("GIROSCOPIO CALIBRADO= %d\n",(bno055.calib>>4)&(3));
    wait(2);


    red=0;
    //bno055.write_calibration_data();
    bno055.get_calib();

    while((bno055.calib & 3) != 3) {
        red=0;
        blue=1;
        green=0;
        //bno055.write_calibration_data();
        bno055.get_calib();
        wait_ms(50);
        blue = 0;
        wait_ms(50);
        pc.printf("Calibrando Magnetometro= %d\n",bno055.calib & (3));
    }

    pc.printf("MAGNETOMETRO CALIBRADO= %d\n",bno055.calib & (3));
    wait(2);

    blue=0;
    //bno055.write_calibration_data();
    bno055.get_calib();
    while(((bno055.calib>>2)& 3 )!= 1) {
        red=0;
        blue=0;
        green=1;
        //bno055.write_calibration_data();
        bno055.get_calib();
        wait_ms(500);
        green = 0;
        wait_ms(500);
        pc.printf("Calibrando acelerometro= %d\n",(bno055.calib>>2)&(3));
    }


    pc.printf("ACELEROMETRO CALIBRADO= %d\n",(bno055.calib>>2)&(3));
    pc.printf("IMU CORRECTAMENTE CALIBRADA= %d\n",(bno055.calib>>2)&(3));
    wait(2);
    green=1;

    if (bno055.check()) {
        bno055.initIntr();
    }

    while(1) {

        /*
        bno055.get_angles(); //query the i2c device
        pc.printf("yaw:%6.2f pitch:%6.2f roll:%6.2f\r\n",bno055.euler.yaw, bno055.euler.pitch, bno055.euler.roll);
        bno055.get_lia(); //query the i2c device
        pc.printf("X: %3.2f, Y: %3.2f, Z: %3.2f\r\n",bno055.lia.x,bno055.lia.y,bno055.lia.z);
        wait_ms(TS);
        */

        if(flagi==true && flags==true) {
            led1 = 1;
            vel_x_act = vel_x + TS * (acel_x+acel_x_ant)/2.0;
            vel_y_act = vel_y + TS * (acel_y+acel_y_ant)/2.0;
            vel_z_act = vel_z + TS * (acel_z+acel_y_ant)/2.0;

            acel_x_ant = acel_x;
            acel_y_ant = acel_y;
            acel_z_ant = acel_z;

            pos_x_act = pos_x + TS * (vel_x_act+vel_x)/2.0;
            pos_y_act = pos_y + TS * (vel_y_act+vel_y)/2.0;
            pos_z_act = pos_z + TS * (vel_z_act+vel_z)/2.0;

            vel_x = vel_x_act;
            vel_y = vel_y_act;
            vel_z = vel_z_act;

            pos_x = pos_x_act;
            pos_y = pos_y_act;
            pos_z = pos_z_act;
            flags=false;
        }
        //bno055.resetIntr();
        if(us_ticker_read()>(tant+1000000)) {
            tant=us_ticker_read();
            pc.printf("ACEL %2.4f   %2.4f   %2.4f   \n", acel_x, acel_y, acel_z);
            pc.printf("VELOCIDAD actual %2.4f   %2.4f   %2.4f   \n", vel_x_act, vel_y_act, vel_z_act);
            pc.printf("Posicion actual %2.4f   %2.4f   %2.4f   \n\n\n",pos_x_act, pos_y_act, pos_z_act);

        }
    }
}


void event()
{

    //wait(10);
    /*
    float v_act=0;
    float x_act=0;
    float v_ant=0;
    float x_ant=0;
        bno055.get_angles(); //query the i2c device
        bno055.get_lia(); //query the i2c device
        v_act = v_ant + TS*float(bno055.lia.x);
        wait(TS);
        pc.printf("V_act = %f",v_act);
        */
    //bno055.resetIntr();
}


void tempo()
{
    bno055.get_lia(); //query the i2c device
    acel_x = bno055.lia.x;
    acel_y = bno055.lia.y;
    acel_z = bno055.lia.z;
    if(acel_x<0.03 && acel_y<0.03 && acel_z<0.03) {
        conteo++;
        if (conteo>=4) {
            flagi=false;
            led1==0;
            bno055.resetIntr();
        }
    } else {
        conteo=0;
        flagi=true;
    }
    flags=true;
}