Alejandro Giraldo Martinez / Mbed 2 deprecated BNO055_basic_copy

.

Dependencies:   BNO055 SDFileSystem mbed

main.cpp@11:4058bd1b41bb, 2018-11-15 (annotated)

Committer:
alejo5214416
Date:
Thu Nov 15 22:26:20 2018 +0000
Revision:
11:4058bd1b41bb
Parent:
9:b5f2b626bff8 
.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alejo5214416 0:b91b6f47eb98 1 #include "mbed.h"
alejo5214416 0:b91b6f47eb98 2 #include "BNO055.h"
alejo5214416 8:67c4e64bccb6 3 #include "SDFileSystem.h"
alejo5214416 8:67c4e64bccb6 4
alejo5214416 8:67c4e64bccb6 5
alejo5214416 8:67c4e64bccb6 6 #define MOSI p5
alejo5214416 8:67c4e64bccb6 7 #define MISO p6
alejo5214416 8:67c4e64bccb6 8 #define SCK p7
alejo5214416 8:67c4e64bccb6 9 #define CS p8
alejo5214416 6:122749ed4e2e 10 #define TS 0.01
alejo5214416 8:67c4e64bccb6 11
alejo5214416 0:b91b6f47eb98 12 using namespace std;
alejo5214416 9:b5f2b626bff8 13 int count=0;//cuenta hasta 4, si vel < 0.03 entonces hace vel = 0
alejo5214416 9:b5f2b626bff8 14 int count_interrupt = 0;
alejo5214416 3:a4a031655cda 15
alejo5214416 8:67c4e64bccb6 16 SDFileSystem sd(MOSI, MISO, SCK, CS, "sd");
alejo5214416 0:b91b6f47eb98 17 DigitalOut led1(LED1);
alejo5214416 6:122749ed4e2e 18 DigitalOut led4(LED4);
alejo5214416 8:67c4e64bccb6 19 DigitalOut led3(LED3);
alejo5214416 2:54bb4cee7885 20 DigitalOut green(p21);
alejo5214416 2:54bb4cee7885 21 DigitalOut blue(p22);
alejo5214416 2:54bb4cee7885 22 DigitalOut red(p23);
alejo5214416 2:54bb4cee7885 23
alejo5214416 6:122749ed4e2e 24 Ticker int_tempo;
alejo5214416 8:67c4e64bccb6 25 //Serial pc(USBTX, USBRX);
alejo5214416 8:67c4e64bccb6 26 Serial xbee(p28, p27);//p28, p27,
alejo5214416 0:b91b6f47eb98 27 BNO055 bno055(p9, p10);
alejo5214416 6:122749ed4e2e 28 InterruptIn interrupt(p15);
alejo5214416 9:b5f2b626bff8 29 InterruptIn b1(p16);
alejo5214416 6:122749ed4e2e 30
alejo5214416 9:b5f2b626bff8 31 bool flagi = false;
alejo5214416 9:b5f2b626bff8 32 int flag_calib = 0;
alejo5214416 8:67c4e64bccb6 33 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];
alejo5214416 8:67c4e64bccb6 34 float suma_x = 0, suma_y = 0, suma_z = 0, mean_x = 0, mean_y = 0, mean_z = 0;
alejo5214416 9:b5f2b626bff8 35 int muestras = 0;
alejo5214416 8:67c4e64bccb6 36 float vel_x=0, vel_y=0, vel_z=0;
alejo5214416 8:67c4e64bccb6 37 float vel_x_act=0, vel_y_act=0, vel_z_act=0;
alejo5214416 8:67c4e64bccb6 38
alejo5214416 8:67c4e64bccb6 39 float pos_x=0, pos_y=0, pos_z=0;
alejo5214416 8:67c4e64bccb6 40 float pos_x_act=0, pos_y_act=0, pos_z_act=0;
alejo5214416 8:67c4e64bccb6 41 uint32_t tant=0;
alejo5214416 8:67c4e64bccb6 42
alejo5214416 3:a4a031655cda 43
alejo5214416 3:a4a031655cda 44 void event();
alejo5214416 6:122749ed4e2e 45 void tempo();
alejo5214416 9:b5f2b626bff8 46 void calibracion();
alejo5214416 0:b91b6f47eb98 47
alejo5214416 3:a4a031655cda 48 int main()
alejo5214416 3:a4a031655cda 49 {
alejo5214416 7:27d1f57f8030 50 //float acel_x_act, acel_y_act, acel_z_act;
alejo5214416 8:67c4e64bccb6 51 led4 = 0;
alejo5214416 8:67c4e64bccb6 52 led3=0;
alejo5214416 3:a4a031655cda 53 green=0;
alejo5214416 3:a4a031655cda 54 blue=0;
alejo5214416 3:a4a031655cda 55 red=1;
alejo5214416 0:b91b6f47eb98 56 bno055.reset();
alejo5214416 3:a4a031655cda 57 interrupt.rise(&event);
alejo5214416 9:b5f2b626bff8 58 b1.rise(&calibracion);
alejo5214416 0:b91b6f47eb98 59 bno055.setmode(OPERATION_MODE_NDOF);
alejo5214416 6:122749ed4e2e 60 //bno055.write_calibration_data();
alejo5214416 0:b91b6f47eb98 61 bno055.get_calib();
alejo5214416 6:122749ed4e2e 62 //bno055.write_calibration_data();
alejo5214416 9:b5f2b626bff8 63
alejo5214416 9:b5f2b626bff8 64 xbee.printf("Para calibrar pulse B1, de lo contrario espere 5 segundos\n");
alejo5214416 9:b5f2b626bff8 65 wait(7);
alejo5214416 9:b5f2b626bff8 66 if (flag_calib == 1) {
alejo5214416 9:b5f2b626bff8 67 while(((bno055.calib>>4)&3)!= 3) {
alejo5214416 9:b5f2b626bff8 68 red=1;
alejo5214416 9:b5f2b626bff8 69 blue=0;
alejo5214416 9:b5f2b626bff8 70 green=0;
alejo5214416 9:b5f2b626bff8 71 bno055.get_calib();
alejo5214416 9:b5f2b626bff8 72 wait_ms(50);
alejo5214416 9:b5f2b626bff8 73 red = 0;
alejo5214416 9:b5f2b626bff8 74 wait_ms(50);
alejo5214416 9:b5f2b626bff8 75 //pc.printf("Calibrando giroscopio= %d\n",(bno055.calib>>4)&(3));
alejo5214416 9:b5f2b626bff8 76 xbee.printf("Calibrando giroscopio= %d\n",(bno055.calib>>4)&(3));
alejo5214416 9:b5f2b626bff8 77 }
alejo5214416 9:b5f2b626bff8 78 //pc.printf("GIROSCOPIO CALIBRADO= %d\n",(bno055.calib>>4)&(3));
alejo5214416 9:b5f2b626bff8 79 xbee.printf("GIROSCOPIO CALIBRADO= %d\n",(bno055.calib>>4)&(3));
alejo5214416 9:b5f2b626bff8 80 wait(2);
alejo5214416 3:a4a031655cda 81
alejo5214416 3:a4a031655cda 82
alejo5214416 3:a4a031655cda 83 red=0;
alejo5214416 6:122749ed4e2e 84 //bno055.write_calibration_data();
alejo5214416 3:a4a031655cda 85 bno055.get_calib();
alejo5214416 7:27d1f57f8030 86
alejo5214416 9:b5f2b626bff8 87 while((bno055.calib & 3) != 3) {
alejo5214416 9:b5f2b626bff8 88 red=0;
alejo5214416 9:b5f2b626bff8 89 blue=1;
alejo5214416 9:b5f2b626bff8 90 green=0;
alejo5214416 9:b5f2b626bff8 91 //bno055.write_calibration_data();
alejo5214416 9:b5f2b626bff8 92 bno055.get_calib();
alejo5214416 9:b5f2b626bff8 93 wait_ms(50);
alejo5214416 9:b5f2b626bff8 94 blue = 0;
alejo5214416 9:b5f2b626bff8 95 wait_ms(50);
alejo5214416 9:b5f2b626bff8 96 //pc.printf("Calibrando Magnetometro= %d\n",bno055.calib & (3));
alejo5214416 9:b5f2b626bff8 97 xbee.printf("Calibrando Magnetometro= %d\n",bno055.calib & (3));
alejo5214416 9:b5f2b626bff8 98 }
alejo5214416 3:a4a031655cda 99
alejo5214416 9:b5f2b626bff8 100 //pc.printf("MAGNETOMETRO CALIBRADO= %d\n",bno055.calib & (3));
alejo5214416 9:b5f2b626bff8 101 xbee.printf("MAGNETOMETRO CALIBRADO= %d\n",bno055.calib & (3));
alejo5214416 9:b5f2b626bff8 102 wait(2);
alejo5214416 9:b5f2b626bff8 103
alejo5214416 3:a4a031655cda 104 blue=0;
alejo5214416 6:122749ed4e2e 105 //bno055.write_calibration_data();
alejo5214416 3:a4a031655cda 106 bno055.get_calib();
alejo5214416 9:b5f2b626bff8 107 while(((bno055.calib>>2)& 3 )!= 3) {
alejo5214416 9:b5f2b626bff8 108 red=0;
alejo5214416 9:b5f2b626bff8 109 blue=0;
alejo5214416 9:b5f2b626bff8 110 green=1;
alejo5214416 9:b5f2b626bff8 111 //bno055.write_calibration_data();
alejo5214416 9:b5f2b626bff8 112 bno055.get_calib();
alejo5214416 9:b5f2b626bff8 113 wait_ms(2000);
alejo5214416 9:b5f2b626bff8 114 green = 0;
alejo5214416 9:b5f2b626bff8 115 wait_ms(2000);
alejo5214416 9:b5f2b626bff8 116 //pc.printf("Calibrando acelerometro= %d\n",(bno055.calib>>2)&(3));
alejo5214416 9:b5f2b626bff8 117 xbee.printf("Calibrando acelerometro= %d\n",(bno055.calib>>2)&(3));
alejo5214416 9:b5f2b626bff8 118 }
alejo5214416 9:b5f2b626bff8 119
alejo5214416 9:b5f2b626bff8 120
alejo5214416 9:b5f2b626bff8 121 //pc.printf("ACELEROMETRO CALIBRADO= %d\n",(bno055.calib>>2)&(3));
alejo5214416 9:b5f2b626bff8 122 xbee.printf("ACELEROMETRO CALIBRADO= %d\n",(bno055.calib>>2)&(3));
alejo5214416 9:b5f2b626bff8 123 //pc.printf("IMU CORRECTAMENTE CALIBRADA= %d\n",(bno055.calib>>2)&(3));
alejo5214416 9:b5f2b626bff8 124 xbee.printf("IMU CORRECTAMENTE CALIBRADA= %d\n",(bno055.calib>>2)&(3));
alejo5214416 9:b5f2b626bff8 125 wait(1.5);
alejo5214416 9:b5f2b626bff8 126 green=1;
alejo5214416 9:b5f2b626bff8 127
alejo5214416 3:a4a031655cda 128 }
alejo5214416 7:27d1f57f8030 129
alejo5214416 7:27d1f57f8030 130
alejo5214416 9:b5f2b626bff8 131 flag_calib = 0;
alejo5214416 3:a4a031655cda 132
alejo5214416 7:27d1f57f8030 133 if (bno055.check()) {
alejo5214416 1:74fb432755d0 134 bno055.initIntr();
alejo5214416 3:a4a031655cda 135 }
alejo5214416 3:a4a031655cda 136
alejo5214416 8:67c4e64bccb6 137 wait(2);
alejo5214416 8:67c4e64bccb6 138 int_tempo.attach(&tempo, TS); // the address of the function to be attached (tempo) and the interval (0.1)
alejo5214416 8:67c4e64bccb6 139
alejo5214416 8:67c4e64bccb6 140 while (muestras<300) {
alejo5214416 8:67c4e64bccb6 141 led3 = 1;
alejo5214416 8:67c4e64bccb6 142 }
alejo5214416 8:67c4e64bccb6 143 for (int p=0; p<300; p++) {
alejo5214416 8:67c4e64bccb6 144 suma_x = suma_x + muestra_x[p];
alejo5214416 8:67c4e64bccb6 145 suma_y = suma_y + muestra_y[p];
alejo5214416 8:67c4e64bccb6 146 suma_z = suma_z + muestra_z[p];
alejo5214416 8:67c4e64bccb6 147 }
alejo5214416 9:b5f2b626bff8 148
alejo5214416 8:67c4e64bccb6 149 mean_x = suma_x/300.0;
alejo5214416 8:67c4e64bccb6 150 mean_y = suma_y/300.0;
alejo5214416 8:67c4e64bccb6 151 mean_z = suma_z/300.0;
alejo5214416 9:b5f2b626bff8 152
alejo5214416 8:67c4e64bccb6 153 xbee.printf("PROMEDIO DE ACELERACION EN X = %f\n", mean_x);
alejo5214416 8:67c4e64bccb6 154 xbee.printf("PROMEDIO DE ACELERACION EN y = %f\n", mean_y);
alejo5214416 8:67c4e64bccb6 155 xbee.printf("PROMEDIO DE ACELERACION EN z = %f\n", mean_z);
alejo5214416 9:b5f2b626bff8 156
alejo5214416 8:67c4e64bccb6 157 wait_ms(1500);
alejo5214416 9:b5f2b626bff8 158
alejo5214416 9:b5f2b626bff8 159 mkdir("/sd/mydir", 0777);
alejo5214416 9:b5f2b626bff8 160 FILE *fp = fopen("/sd/datos.csv", "a+");
alejo5214416 9:b5f2b626bff8 161 if(fp == NULL) {
alejo5214416 9:b5f2b626bff8 162 //pc.printf("Could not open file for write\n");
alejo5214416 9:b5f2b626bff8 163 xbee.printf("Could not open file for write\n");
alejo5214416 9:b5f2b626bff8 164 }
alejo5214416 3:a4a031655cda 165
alejo5214416 9:b5f2b626bff8 166 while(1) {
alejo5214416 9:b5f2b626bff8 167 if (count_interrupt > 0) {
alejo5214416 9:b5f2b626bff8 168 xbee.printf("FIN DE ESCRITURA EN SD\n");
alejo5214416 9:b5f2b626bff8 169 fclose(fp);
alejo5214416 9:b5f2b626bff8 170 count_interrupt = 0;
alejo5214416 9:b5f2b626bff8 171 }
alejo5214416 7:27d1f57f8030 172
alejo5214416 9:b5f2b626bff8 173 if(flagi==true) {
alejo5214416 9:b5f2b626bff8 174 vel_x_act = vel_x + TS * ( ( acel_x + acel_x_ant ) ) / 2.0;
alejo5214416 9:b5f2b626bff8 175 vel_y_act = vel_y + TS * ( ( acel_y + acel_y_ant ) ) / 2.0;
alejo5214416 9:b5f2b626bff8 176 vel_z_act = vel_z + TS * ( ( acel_z + acel_y_ant ) ) / 2.0;
alejo5214416 7:27d1f57f8030 177
alejo5214416 9:b5f2b626bff8 178 acel_x_ant = acel_x;
alejo5214416 9:b5f2b626bff8 179 acel_y_ant = acel_y;
alejo5214416 9:b5f2b626bff8 180 acel_z_ant = acel_z;
alejo5214416 9:b5f2b626bff8 181
alejo5214416 9:b5f2b626bff8 182 pos_x_act = pos_x + TS * ( ( vel_x_act + vel_x ) ) / 2.0;
alejo5214416 9:b5f2b626bff8 183 pos_y_act = pos_y + TS * ( ( vel_y_act + vel_y ) ) / 2.0;
alejo5214416 9:b5f2b626bff8 184 pos_z_act = pos_z + TS * ( ( vel_z_act + vel_z ) ) / 2.0;
alejo5214416 7:27d1f57f8030 185
alejo5214416 6:122749ed4e2e 186 vel_x = vel_x_act;
alejo5214416 6:122749ed4e2e 187 vel_y = vel_y_act;
alejo5214416 6:122749ed4e2e 188 vel_z = vel_z_act;
alejo5214416 7:27d1f57f8030 189
alejo5214416 6:122749ed4e2e 190 pos_x = pos_x_act;
alejo5214416 6:122749ed4e2e 191 pos_y = pos_y_act;
alejo5214416 6:122749ed4e2e 192 pos_z = pos_z_act;
alejo5214416 9:b5f2b626bff8 193 if (flag_calib == 1){
alejo5214416 8:67c4e64bccb6 194 fprintf(fp,"%f %f %f\n",pos_x_act, pos_y_act, pos_z_act);
alejo5214416 9:b5f2b626bff8 195 wait_ms(5);
alejo5214416 8:67c4e64bccb6 196 }
alejo5214416 9:b5f2b626bff8 197
alejo5214416 7:27d1f57f8030 198 }
alejo5214416 8:67c4e64bccb6 199
alejo5214416 9:b5f2b626bff8 200
alejo5214416 7:27d1f57f8030 201 if(us_ticker_read()>(tant+1000000)) {
alejo5214416 7:27d1f57f8030 202 tant=us_ticker_read();
alejo5214416 9:b5f2b626bff8 203 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));
alejo5214416 8:67c4e64bccb6 204 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);
alejo5214416 9:b5f2b626bff8 205 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);
alejo5214416 7:27d1f57f8030 206 }
alejo5214416 9:b5f2b626bff8 207
alejo5214416 9:b5f2b626bff8 208
alejo5214416 9:b5f2b626bff8 209 }//end while(1)
alejo5214416 9:b5f2b626bff8 210 }//end main
alejo5214416 9:b5f2b626bff8 211
alejo5214416 9:b5f2b626bff8 212
alejo5214416 9:b5f2b626bff8 213 void calibracion(){
alejo5214416 9:b5f2b626bff8 214 flag_calib++;
alejo5214416 9:b5f2b626bff8 215 wait_ms(10);
alejo5214416 0:b91b6f47eb98 216 }
alejo5214416 9:b5f2b626bff8 217
alejo5214416 9:b5f2b626bff8 218
alejo5214416 0:b91b6f47eb98 219
alejo5214416 3:a4a031655cda 220 void event()
alejo5214416 3:a4a031655cda 221 {
alejo5214416 9:b5f2b626bff8 222 led4=!led4;;
alejo5214416 9:b5f2b626bff8 223 count_interrupt++;
alejo5214416 6:122749ed4e2e 224 }
alejo5214416 6:122749ed4e2e 225
alejo5214416 6:122749ed4e2e 226
alejo5214416 7:27d1f57f8030 227 void tempo()
alejo5214416 7:27d1f57f8030 228 {
alejo5214416 9:b5f2b626bff8 229 //1
alejo5214416 6:122749ed4e2e 230 bno055.get_lia(); //query the i2c device
alejo5214416 6:122749ed4e2e 231 acel_x = bno055.lia.x;
alejo5214416 6:122749ed4e2e 232 acel_y = bno055.lia.y;
alejo5214416 6:122749ed4e2e 233 acel_z = bno055.lia.z;
alejo5214416 8:67c4e64bccb6 234 if(muestras<300) {
alejo5214416 8:67c4e64bccb6 235 muestra_x[muestras] = acel_x;
alejo5214416 8:67c4e64bccb6 236 muestra_y[muestras] = acel_y;
alejo5214416 8:67c4e64bccb6 237 muestra_z[muestras] = acel_z;
alejo5214416 8:67c4e64bccb6 238 muestras++;
alejo5214416 9:b5f2b626bff8 239 flagi = false;
alejo5214416 8:67c4e64bccb6 240 } else {
alejo5214416 9:b5f2b626bff8 241 flagi = true;
alejo5214416 11:4058bd1b41bb 242 // < 0.03 && fabs(acel_y)<0.03 && fabs(acel_z)<0.03
alejo5214416 11:4058bd1b41bb 243 if ( fabs(vel_x_act) < 1 && fabs(vel_y_act) < 1 && fabs(vel_z_act) < 1 && fabs(acel_x) ) {
alejo5214416 9:b5f2b626bff8 244 count++;
alejo5214416 9:b5f2b626bff8 245 if ( count >= 2) {
alejo5214416 9:b5f2b626bff8 246 led1 = 0;
alejo5214416 9:b5f2b626bff8 247 vel_x_act = 0;
alejo5214416 9:b5f2b626bff8 248 vel_y_act = 0;
alejo5214416 9:b5f2b626bff8 249 vel_z_act = 0;
alejo5214416 9:b5f2b626bff8 250 vel_x = 0;
alejo5214416 9:b5f2b626bff8 251 vel_y = 0;
alejo5214416 9:b5f2b626bff8 252 vel_z = 0;
alejo5214416 8:67c4e64bccb6 253 acel_x_ant = 0;
alejo5214416 8:67c4e64bccb6 254 acel_y_ant = 0;
alejo5214416 8:67c4e64bccb6 255 acel_z_ant = 0;
alejo5214416 9:b5f2b626bff8 256 acel_x = 0;
alejo5214416 9:b5f2b626bff8 257 acel_y = 0;
alejo5214416 9:b5f2b626bff8 258 acel_z = 0;
alejo5214416 8:67c4e64bccb6 259 }
alejo5214416 8:67c4e64bccb6 260 } else {
alejo5214416 9:b5f2b626bff8 261 led1=1;
alejo5214416 9:b5f2b626bff8 262 count = 0;
alejo5214416 7:27d1f57f8030 263 }
alejo5214416 9:b5f2b626bff8 264
alejo5214416 7:27d1f57f8030 265 }
alejo5214416 9:b5f2b626bff8 266 }