Alejandro Giraldo Martinez / Mbed 2 deprecated BNO055_basic_copy

Dependencies:   BNO055 SDFileSystem mbed

Diff: main.cpp

Revision:
8:67c4e64bccb6
Parent:
7:27d1f57f8030
Child:
9:b5f2b626bff8
--- a/main.cpp	Wed Oct 03 03:18:53 2018 +0000
+++ b/main.cpp	Thu Nov 08 03:22:44 2018 +0000
@@ -1,22 +1,42 @@
 #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 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;
-int     conteo=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     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();
@@ -24,17 +44,18 @@
 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;
+    led4 = 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;
-
+    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;
-    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);
@@ -50,9 +71,11 @@
         wait_ms(50);
         red = 0;
         wait_ms(50);
-        pc.printf("Calibrando giroscopio= %d\n",(bno055.calib>>4)&(3));
+        //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));
+    //pc.printf("GIROSCOPIO CALIBRADO= %d\n",(bno055.calib>>4)&(3));
+    xbee.printf("GIROSCOPIO CALIBRADO= %d\n",(bno055.calib>>4)&(3));
     wait(2);
 
 
@@ -69,37 +92,67 @@
         wait_ms(50);
         blue = 0;
         wait_ms(50);
-        pc.printf("Calibrando Magnetometro= %d\n",bno055.calib & (3));
+        //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));
+    //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 )!= 1) {
+    while(((bno055.calib>>2)& 3 )!= 3) {
         red=0;
         blue=0;
         green=1;
         //bno055.write_calibration_data();
         bno055.get_calib();
-        wait_ms(500);
+        wait_ms(2000);
         green = 0;
-        wait_ms(500);
-        pc.printf("Calibrando acelerometro= %d\n",(bno055.calib>>2)&(3));
+        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));
-    pc.printf("IMU CORRECTAMENTE CALIBRADA= %d\n",(bno055.calib>>2)&(3));
-    wait(2);
+    //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) {
 
         /*
@@ -109,16 +162,15 @@
         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;
+            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;
-            acel_y_ant = acel_y;
-            acel_z_ant = acel_z;
+            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;
@@ -131,36 +183,39 @@
             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("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);
+            //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()
 {
-
-    //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();
+    led4 = 1;
+//Interrupcion de IMU
 }
 
 
@@ -170,16 +225,40 @@
     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();
+    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;
         }
-    } else {
-        conteo=0;
-        flagi=true;
     }
     flags=true;
-}
\ No newline at end of file
+}
+
+
+
+