Alejandro Giraldo Martinez / Mbed 2 deprecated BNO055_basic_copy

.

Dependencies:   BNO055 SDFileSystem mbed

main.cpp@8:67c4e64bccb6, 2018-11-08 (annotated)

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