File content as of revision 11:4058bd1b41bb:

#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 count=0;//cuenta hasta 4, si vel < 0.03 entonces hace vel = 0
int count_interrupt = 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);
InterruptIn b1(p16);

bool    flagi = false;
int    flag_calib = 0;
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     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();
void calibracion();

int main()
{
    //float   acel_x_act, acel_y_act, acel_z_act;
    led4 = 0;
    led3=0;
    green=0;
    blue=0;
    red=1;
    bno055.reset();
    interrupt.rise(&event);
    b1.rise(&calibracion);
    bno055.setmode(OPERATION_MODE_NDOF);
    //bno055.write_calibration_data();
    bno055.get_calib();
    //bno055.write_calibration_data();
    
    xbee.printf("Para calibrar pulse B1, de lo contrario espere 5 segundos\n");
    wait(7);
    if (flag_calib == 1) {
        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;

    }


    flag_calib = 0;

    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;

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

    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");
    }

    while(1) {
        if (count_interrupt > 0) {
            xbee.printf("FIN DE ESCRITURA EN SD\n");
            fclose(fp);
            count_interrupt = 0;
        }

        if(flagi==true) {
            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;
            if (flag_calib == 1){
            fprintf(fp,"%f %f %f\n",pos_x_act, pos_y_act, pos_z_act);
            wait_ms(5);
            }
            
        }


        if(us_ticker_read()>(tant+1000000)) {
            tant=us_ticker_read();
            xbee.printf("ACELERACION  X=%2.4f m/s^2   Y=%2.4f m/s^2   Z=%2.4f m/s^2   \n", (acel_x), (acel_y), (acel_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);
        }
    
    
    }//end while(1)
}//end main


void calibracion(){
    flag_calib++;
    wait_ms(10);
    }



void event()
{
    led4=!led4;;
    count_interrupt++;
}


void tempo()
{
    //1
    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++;
        flagi = false;
    } else {
        flagi = true;
         // < 0.03 && fabs(acel_y)<0.03 && fabs(acel_z)<0.03
        if ( fabs(vel_x_act) < 1 && fabs(vel_y_act) < 1 && fabs(vel_z_act) < 1 && fabs(acel_x) ) {
            count++;
            if ( count >= 2) {
                led1 = 0;
                vel_x_act = 0;
                vel_y_act = 0;
                vel_z_act = 0;
                vel_x = 0;
                vel_y = 0;
                vel_z = 0;
                acel_x_ant = 0;
                acel_y_ant = 0;
                acel_z_ant = 0;
                acel_x = 0;
                acel_y = 0;
                acel_z = 0;
            }
        } else {
            led1=1;
            count = 0;
        }

    }
}