File content as of revision 8:67c4e64bccb6:

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


#define MOSI p5
#define MISO p6
#define SCK  p7
#define CS   p8
#define TS 0.01

using namespace std;
int cont=0;

SDFileSystem sd(MOSI, MISO, SCK, CS, "sd");
DigitalOut led1(LED1);
DigitalOut led4(LED4);
DigitalOut led3(LED3);
DigitalOut green(p21);
DigitalOut blue(p22);
DigitalOut red(p23);

Ticker int_tempo;
//Serial pc(USBTX, USBRX);
Serial xbee(p28, p27);//p28, p27,
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, muestra_x[300], muestra_y[300], muestra_z[300];
float   suma_x = 0, suma_y = 0, suma_z = 0, mean_x = 0, mean_y = 0, mean_z = 0;
int     conteo = 0, muestras = 0;
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;


void event();
void tempo();

int main()
{
    //float   acel_x_act, acel_y_act, acel_z_act;
    led4 = 0;

    mkdir("/sd/mydir", 0777);
    FILE *fp = fopen("/sd/datos.csv", "a+");
    if(fp == NULL) {
        //pc.printf("Could not open file for write\n");
        xbee.printf("Could not open file for write\n");
    }
    led3=0;
    green=0;
    blue=0;
    red=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));
        xbee.printf("Calibrando giroscopio= %d\n",(bno055.calib>>4)&(3));
    }
    //pc.printf("GIROSCOPIO CALIBRADO= %d\n",(bno055.calib>>4)&(3));
    xbee.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));
        xbee.printf("Calibrando Magnetometro= %d\n",bno055.calib & (3));
    }

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

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


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

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

    wait(2);
    int_tempo.attach(&tempo, TS); // the address of the function to be attached (tempo) and the interval (0.1)

    while (muestras<300) {
        led3 = 1;
    }
    for (int p=0; p<300; p++) {
        suma_x = suma_x + muestra_x[p];
        suma_y = suma_y + muestra_y[p];
        suma_z = suma_z + muestra_z[p];
    }
    
    mean_x = suma_x/300.0;
    mean_y = suma_y/300.0;
    mean_z = suma_z/300.0;
    
    //pc.printf("PROMEDIO DE ACELERACION EN X = %f\n", mean_x);
    //pc.printf("PROMEDIO DE ACELERACION EN y = %f\n", mean_y);
    //pc.printf("PROMEDIO DE ACELERACION EN z = %f\n", mean_z);
    
    xbee.printf("PROMEDIO DE ACELERACION EN X = %f\n", mean_x);
    xbee.printf("PROMEDIO DE ACELERACION EN y = %f\n", mean_y);
    xbee.printf("PROMEDIO DE ACELERACION EN z = %f\n", mean_z);
    wait_ms(1500);
    
    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-mean_x)+acel_x_ant)/2.0;
            vel_y_act = vel_y + TS * ((acel_y-mean_y)+acel_y_ant)/2.0;
            vel_z_act = vel_z + TS * ((acel_z-mean_z)+acel_y_ant)/2.0;

            acel_x_ant = acel_x-mean_x;
            acel_y_ant = acel_y-mean_y;
            acel_z_ant = acel_z-mean_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;
        }
        
        if(flags == true) {
            cont++;
            fprintf(fp,"%f %f %f\n",pos_x_act, pos_y_act, pos_z_act);
            if (cont == 6000) {
                //pc.printf("FIN DE ESCRITURA EN SD\n");
                xbee.printf("FIN DE ESCRITURA EN SD\n");
                fclose(fp);

            }
            flags=false;
        }

        //bno055.resetIntr();
        if(us_ticker_read()>(tant+1000000)) {
            tant=us_ticker_read();
            //pc.printf("ACELERACION  X=%2.4f m/s^2   Y=%2.4f m/s^2   Z=%2.4f m/s^2   \n", (acel_x-mean_x), (acel_y-mean_y), (acel_z-mean_z));
            //pc.printf("VELOCIDAD    X=%2.4f m/s   Y=%2.4f m/s   Z=%2.4f m/s   \n", vel_x_act, vel_y_act, vel_z_act);
            //pc.printf("POSICION     X=%2.4f m   Y=%2.4f m   Z=%2.4f m   \n\n\n\n",pos_x_act, pos_y_act, pos_z_act);
            
            xbee.printf("ACELERACION  X=%2.4f m/s^2   Y=%2.4f m/s^2   Z=%2.4f m/s^2   \n", (acel_x-mean_x), (acel_y-mean_y), (acel_z-mean_z));
            xbee.printf("VELOCIDAD    X=%2.4f m/s   Y=%2.4f m/s   Z=%2.4f m/s   \n", vel_x_act, vel_y_act, vel_z_act);
            xbee.printf("POSICION     X=%2.4f m   Y=%2.4f m   Z=%2.4f m   \n\n\n\n",pos_x_act, pos_y_act, pos_z_act);            

        }
    }
}

void event()
{
    led4 = 1;
//Interrupcion de IMU
}


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(muestras<300) {
        muestra_x[muestras] = acel_x;
        muestra_y[muestras] = acel_y;
        muestra_z[muestras] = acel_z;
        muestras++;
    } else {
        if(fabs(acel_x)<0.01 && fabs(acel_y)<0.01 && fabs(acel_z)<0.01) {
            conteo++;
            if (conteo>=4) {
                flagi=false;
                led1=0;
                bno055.resetIntr();
                acel_x=0;
                acel_y=0;
                acel_z=0;
                vel_x=0;
                vel_y=0;
                vel_z=0;
                vel_x_act=0;
                vel_y_act=0;
                vel_z_act=0;

                acel_x_ant = 0;
                acel_y_ant = 0;
                acel_z_ant = 0;
            }
        } else {
            conteo=0;
            flagi=true;
        }
    }
    flags=true;
}