main.cpp

00001 #include "mbed.h"
00002 #include "BNO055.h"
00003 #include "SDFileSystem.h"
00004 
00005 
00006 #define MOSI p5
00007 #define MISO p6
00008 #define SCK  p7
00009 #define CS   p8
00010 #define TS 0.01
00011 
00012 using namespace std;
00013 int count=0;//cuenta hasta 4, si vel < 0.03 entonces hace vel = 0
00014 int count_interrupt = 0;
00015 
00016 SDFileSystem sd(MOSI, MISO, SCK, CS, "sd");
00017 DigitalOut led1(LED1);
00018 DigitalOut led4(LED4);
00019 DigitalOut led3(LED3);
00020 DigitalOut green(p21);
00021 DigitalOut blue(p22);
00022 DigitalOut red(p23);
00023 
00024 Ticker int_tempo;
00025 //Serial pc(USBTX, USBRX);
00026 Serial xbee(p28, p27);//p28, p27,
00027 BNO055 bno055(p9, p10);
00028 InterruptIn interrupt(p15);
00029 InterruptIn b1(p16);
00030 
00031 bool    flagi = false;
00032 int    flag_calib = 0;
00033 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];
00034 float   suma_x = 0, suma_y = 0, suma_z = 0, mean_x = 0, mean_y = 0, mean_z = 0;
00035 int     muestras = 0;
00036 float   vel_x=0, vel_y=0, vel_z=0;
00037 float   vel_x_act=0, vel_y_act=0, vel_z_act=0;
00038 
00039 float   pos_x=0, pos_y=0, pos_z=0;
00040 float   pos_x_act=0, pos_y_act=0, pos_z_act=0;
00041 uint32_t tant=0;
00042 
00043 
00044 void event();
00045 void tempo();
00046 void calibracion();
00047 
00048 int main()
00049 {
00050     //float   acel_x_act, acel_y_act, acel_z_act;
00051     led4 = 0;
00052     led3=0;
00053     green=0;
00054     blue=0;
00055     red=1;
00056     bno055.reset();
00057     interrupt.rise(&event);
00058     b1.rise(&calibracion);
00059     bno055.setmode(OPERATION_MODE_NDOF);
00060     //bno055.write_calibration_data();
00061     bno055.get_calib();
00062     //bno055.write_calibration_data();
00063     
00064     xbee.printf("Para calibrar pulse B1, de lo contrario espere 5 segundos\n");
00065     wait(7);
00066     if (flag_calib == 1) {
00067         while(((bno055.calib>>4)&3)!= 3) {
00068             red=1;
00069             blue=0;
00070             green=0;
00071             bno055.get_calib();
00072             wait_ms(50);
00073             red = 0;
00074             wait_ms(50);
00075             //pc.printf("Calibrando giroscopio= %d\n",(bno055.calib>>4)&(3));
00076             xbee.printf("Calibrando giroscopio= %d\n",(bno055.calib>>4)&(3));
00077         }
00078         //pc.printf("GIROSCOPIO CALIBRADO= %d\n",(bno055.calib>>4)&(3));
00079         xbee.printf("GIROSCOPIO CALIBRADO= %d\n",(bno055.calib>>4)&(3));
00080         wait(2);
00081 
00082 
00083         red=0;
00084         //bno055.write_calibration_data();
00085         bno055.get_calib();
00086 
00087         while((bno055.calib & 3) != 3) {
00088             red=0;
00089             blue=1;
00090             green=0;
00091             //bno055.write_calibration_data();
00092             bno055.get_calib();
00093             wait_ms(50);
00094             blue = 0;
00095             wait_ms(50);
00096             //pc.printf("Calibrando Magnetometro= %d\n",bno055.calib & (3));
00097             xbee.printf("Calibrando Magnetometro= %d\n",bno055.calib & (3));
00098         }
00099 
00100         //pc.printf("MAGNETOMETRO CALIBRADO= %d\n",bno055.calib & (3));
00101         xbee.printf("MAGNETOMETRO CALIBRADO= %d\n",bno055.calib & (3));
00102         wait(2);
00103 
00104         blue=0;
00105         //bno055.write_calibration_data();
00106         bno055.get_calib();
00107         while(((bno055.calib>>2)& 3 )!= 3) {
00108             red=0;
00109             blue=0;
00110             green=1;
00111             //bno055.write_calibration_data();
00112             bno055.get_calib();
00113             wait_ms(2000);
00114             green = 0;
00115             wait_ms(2000);
00116             //pc.printf("Calibrando acelerometro= %d\n",(bno055.calib>>2)&(3));
00117             xbee.printf("Calibrando acelerometro= %d\n",(bno055.calib>>2)&(3));
00118         }
00119 
00120 
00121         //pc.printf("ACELEROMETRO CALIBRADO= %d\n",(bno055.calib>>2)&(3));
00122         xbee.printf("ACELEROMETRO CALIBRADO= %d\n",(bno055.calib>>2)&(3));
00123         //pc.printf("IMU CORRECTAMENTE CALIBRADA= %d\n",(bno055.calib>>2)&(3));
00124         xbee.printf("IMU CORRECTAMENTE CALIBRADA= %d\n",(bno055.calib>>2)&(3));
00125         wait(1.5);
00126         green=1;
00127 
00128     }
00129 
00130 
00131     flag_calib = 0;
00132 
00133     if (bno055.check()) {
00134         bno055.initIntr();
00135     }
00136 
00137     wait(2);
00138     int_tempo.attach(&tempo, TS); // the address of the function to be attached (tempo) and the interval (0.1)
00139 
00140     while (muestras<300) {
00141         led3 = 1;
00142     }
00143     for (int p=0; p<300; p++) {
00144         suma_x = suma_x + muestra_x[p];
00145         suma_y = suma_y + muestra_y[p];
00146         suma_z = suma_z + muestra_z[p];
00147     }
00148 
00149     mean_x = suma_x/300.0;
00150     mean_y = suma_y/300.0;
00151     mean_z = suma_z/300.0;
00152 
00153     xbee.printf("PROMEDIO DE ACELERACION EN X = %f\n", mean_x);
00154     xbee.printf("PROMEDIO DE ACELERACION EN y = %f\n", mean_y);
00155     xbee.printf("PROMEDIO DE ACELERACION EN z = %f\n", mean_z);
00156 
00157     wait_ms(1500);
00158 
00159     mkdir("/sd/mydir", 0777);
00160     FILE *fp = fopen("/sd/datos.csv", "a+");
00161     if(fp == NULL) {
00162         //pc.printf("Could not open file for write\n");
00163         xbee.printf("Could not open file for write\n");
00164     }
00165 
00166     while(1) {
00167         if (count_interrupt > 0) {
00168             xbee.printf("FIN DE ESCRITURA EN SD\n");
00169             fclose(fp);
00170             count_interrupt = 0;
00171         }
00172 
00173         if(flagi==true) {
00174             vel_x_act = vel_x + TS * ( ( acel_x + acel_x_ant ) ) / 2.0;
00175             vel_y_act = vel_y + TS * ( ( acel_y + acel_y_ant ) ) / 2.0;
00176             vel_z_act = vel_z + TS * ( ( acel_z + acel_y_ant ) ) / 2.0;
00177 
00178             acel_x_ant = acel_x;
00179             acel_y_ant = acel_y;
00180             acel_z_ant = acel_z;
00181 
00182             pos_x_act = pos_x + TS * ( ( vel_x_act + vel_x ) ) / 2.0;
00183             pos_y_act = pos_y + TS * ( ( vel_y_act + vel_y ) ) / 2.0;
00184             pos_z_act = pos_z + TS * ( ( vel_z_act + vel_z ) ) / 2.0;
00185 
00186             vel_x = vel_x_act;
00187             vel_y = vel_y_act;
00188             vel_z = vel_z_act;
00189 
00190             pos_x = pos_x_act;
00191             pos_y = pos_y_act;
00192             pos_z = pos_z_act;
00193             if (flag_calib == 1){
00194             fprintf(fp,"%f %f %f\n",pos_x_act, pos_y_act, pos_z_act);
00195             wait_ms(5);
00196             }
00197             
00198         }
00199 
00200 
00201         if(us_ticker_read()>(tant+1000000)) {
00202             tant=us_ticker_read();
00203             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));
00204             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);
00205             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);
00206         }
00207     
00208     
00209     }//end while(1)
00210 }//end main
00211 
00212 
00213 void calibracion(){
00214     flag_calib++;
00215     wait_ms(10);
00216     }
00217 
00218 
00219 
00220 void event()
00221 {
00222     led4=!led4;;
00223     count_interrupt++;
00224 }
00225 
00226 
00227 void tempo()
00228 {
00229     //1
00230     bno055.get_lia(); //query the i2c device
00231     acel_x = bno055.lia.x;
00232     acel_y = bno055.lia.y;
00233     acel_z = bno055.lia.z;
00234     if(muestras<300) {
00235         muestra_x[muestras] = acel_x;
00236         muestra_y[muestras] = acel_y;
00237         muestra_z[muestras] = acel_z;
00238         muestras++;
00239         flagi = false;
00240     } else {
00241         flagi = true;
00242          // < 0.03 && fabs(acel_y)<0.03 && fabs(acel_z)<0.03
00243         if ( fabs(vel_x_act) < 1 && fabs(vel_y_act) < 1 && fabs(vel_z_act) < 1 && fabs(acel_x) ) {
00244             count++;
00245             if ( count >= 2) {
00246                 led1 = 0;
00247                 vel_x_act = 0;
00248                 vel_y_act = 0;
00249                 vel_z_act = 0;
00250                 vel_x = 0;
00251                 vel_y = 0;
00252                 vel_z = 0;
00253                 acel_x_ant = 0;
00254                 acel_y_ant = 0;
00255                 acel_z_ant = 0;
00256                 acel_x = 0;
00257                 acel_y = 0;
00258                 acel_z = 0;
00259             }
00260         } else {
00261             led1=1;
00262             count = 0;
00263         }
00264 
00265     }
00266 }