Alejandro Giraldo Martinez / Mbed 2 deprecated BNO055_basic_copy

.

Dependencies:   BNO055 SDFileSystem mbed

main.cpp@6:122749ed4e2e, 2018-09-20 (annotated)

Committer:
alejo5214416
Date:
Thu Sep 20 06:30:27 2018 +0000
Revision:
6:122749ed4e2e
Parent:
3:a4a031655cda
Child:
7:27d1f57f8030
INTEGRACION CORRECTA, cambiar salida de la interrupcion y reset.

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 6:122749ed4e2e 3 #define TS 0.01
alejo5214416 0:b91b6f47eb98 4 using namespace std;
alejo5214416 3:a4a031655cda 5
alejo5214416 0:b91b6f47eb98 6 DigitalOut led1(LED1);
alejo5214416 6:122749ed4e2e 7 DigitalOut led4(LED4);
alejo5214416 2:54bb4cee7885 8 DigitalOut green(p21);
alejo5214416 2:54bb4cee7885 9 DigitalOut blue(p22);
alejo5214416 2:54bb4cee7885 10 DigitalOut red(p23);
alejo5214416 2:54bb4cee7885 11
alejo5214416 6:122749ed4e2e 12 Ticker int_tempo;
alejo5214416 3:a4a031655cda 13 Serial pc(USBTX, USBRX);
alejo5214416 0:b91b6f47eb98 14 BNO055 bno055(p9, p10);
alejo5214416 6:122749ed4e2e 15 InterruptIn interrupt(p15);
alejo5214416 6:122749ed4e2e 16
alejo5214416 6:122749ed4e2e 17 bool flag = false, flags=false;
alejo5214416 6:122749ed4e2e 18 float acel_x, acel_y, acel_z,acel_x_ant,acel_y_ant,acel_z_ant;
alejo5214416 3:a4a031655cda 19
alejo5214416 3:a4a031655cda 20 void event();
alejo5214416 6:122749ed4e2e 21 void tempo();
alejo5214416 0:b91b6f47eb98 22
alejo5214416 3:a4a031655cda 23 int main()
alejo5214416 3:a4a031655cda 24 {
alejo5214416 6:122749ed4e2e 25 //float acel_x_act, acel_y_act, acel_z_act;
alejo5214416 6:122749ed4e2e 26 float vel_x=0, vel_y=0, vel_z=0;
alejo5214416 6:122749ed4e2e 27 float vel_x_act=0, vel_y_act=0, vel_z_act=0;
alejo5214416 6:122749ed4e2e 28
alejo5214416 6:122749ed4e2e 29 float pos_x=0, pos_y=0, pos_z=0;
alejo5214416 6:122749ed4e2e 30 float pos_x_act=0, pos_y_act=0, pos_z_act=0;
alejo5214416 6:122749ed4e2e 31 uint32_t tant=0;
alejo5214416 6:122749ed4e2e 32
alejo5214416 3:a4a031655cda 33 green=0;
alejo5214416 3:a4a031655cda 34 blue=0;
alejo5214416 3:a4a031655cda 35 red=1;
alejo5214416 6:122749ed4e2e 36 int_tempo.attach(&tempo, TS); // the address of the function to be attached (tempo) and the interval (0.1)
alejo5214416 0:b91b6f47eb98 37 bno055.reset();
alejo5214416 3:a4a031655cda 38 interrupt.rise(&event);
alejo5214416 0:b91b6f47eb98 39 bno055.setmode(OPERATION_MODE_NDOF);
alejo5214416 6:122749ed4e2e 40 //bno055.write_calibration_data();
alejo5214416 0:b91b6f47eb98 41 bno055.get_calib();
alejo5214416 6:122749ed4e2e 42 //bno055.write_calibration_data();
alejo5214416 6:122749ed4e2e 43
alejo5214416 6:122749ed4e2e 44 while(((bno055.calib>>4)&3)!= 3) {
alejo5214416 3:a4a031655cda 45 red=1;
alejo5214416 3:a4a031655cda 46 blue=0;
alejo5214416 3:a4a031655cda 47 green=0;
alejo5214416 0:b91b6f47eb98 48 bno055.get_calib();
alejo5214416 3:a4a031655cda 49 wait_ms(200);
alejo5214416 3:a4a031655cda 50 red = 0;
alejo5214416 3:a4a031655cda 51 wait_ms(200);
alejo5214416 3:a4a031655cda 52 pc.printf("Calibrando giroscopio= %d\n",(bno055.calib>>4)&(3));
alejo5214416 0:b91b6f47eb98 53 }
alejo5214416 3:a4a031655cda 54 pc.printf("GIROSCOPIO CALIBRADO= %d\n",(bno055.calib>>4)&(3));
alejo5214416 3:a4a031655cda 55 wait(2);
alejo5214416 3:a4a031655cda 56
alejo5214416 3:a4a031655cda 57
alejo5214416 3:a4a031655cda 58 red=0;
alejo5214416 6:122749ed4e2e 59 //bno055.write_calibration_data();
alejo5214416 3:a4a031655cda 60 bno055.get_calib();
alejo5214416 6:122749ed4e2e 61
alejo5214416 3:a4a031655cda 62 while((bno055.calib & 3) != 3) {
alejo5214416 3:a4a031655cda 63 red=0;
alejo5214416 3:a4a031655cda 64 blue=1;
alejo5214416 3:a4a031655cda 65 green=0;
alejo5214416 6:122749ed4e2e 66 //bno055.write_calibration_data();
alejo5214416 3:a4a031655cda 67 bno055.get_calib();
alejo5214416 3:a4a031655cda 68 wait_ms(200);
alejo5214416 3:a4a031655cda 69 blue = 0;
alejo5214416 3:a4a031655cda 70 wait_ms(200);
alejo5214416 3:a4a031655cda 71 pc.printf("Calibrando Magnetometro= %d\n",bno055.calib & (3));
alejo5214416 3:a4a031655cda 72 }
alejo5214416 6:122749ed4e2e 73
alejo5214416 3:a4a031655cda 74 pc.printf("MAGNETOMETRO CALIBRADO= %d\n",bno055.calib & (3));
alejo5214416 3:a4a031655cda 75 wait(2);
alejo5214416 3:a4a031655cda 76
alejo5214416 3:a4a031655cda 77 blue=0;
alejo5214416 6:122749ed4e2e 78 //bno055.write_calibration_data();
alejo5214416 3:a4a031655cda 79 bno055.get_calib();
alejo5214416 3:a4a031655cda 80 while(((bno055.calib>>2)& 3 )!= 3) {
alejo5214416 3:a4a031655cda 81 red=0;
alejo5214416 3:a4a031655cda 82 blue=0;
alejo5214416 3:a4a031655cda 83 green=1;
alejo5214416 6:122749ed4e2e 84 //bno055.write_calibration_data();
alejo5214416 3:a4a031655cda 85 bno055.get_calib();
alejo5214416 3:a4a031655cda 86 wait_ms(200);
alejo5214416 3:a4a031655cda 87 green = 0;
alejo5214416 3:a4a031655cda 88 wait_ms(200);
alejo5214416 3:a4a031655cda 89 pc.printf("Calibrando acelerometro= %d\n",(bno055.calib>>2)&(3));
alejo5214416 3:a4a031655cda 90 }
alejo5214416 6:122749ed4e2e 91
alejo5214416 6:122749ed4e2e 92
alejo5214416 3:a4a031655cda 93 pc.printf("ACELEROMETRO CALIBRADO= %d\n",(bno055.calib>>2)&(3));
alejo5214416 3:a4a031655cda 94 pc.printf("IMU CORRECTAMENTE CALIBRADA= %d\n",(bno055.calib>>2)&(3));
alejo5214416 3:a4a031655cda 95 wait(2);
alejo5214416 3:a4a031655cda 96 green=1;
alejo5214416 3:a4a031655cda 97
alejo5214416 6:122749ed4e2e 98 if (bno055.check()){
alejo5214416 1:74fb432755d0 99 bno055.initIntr();
alejo5214416 3:a4a031655cda 100 }
alejo5214416 3:a4a031655cda 101
alejo5214416 0:b91b6f47eb98 102 while(1) {
alejo5214416 3:a4a031655cda 103
alejo5214416 6:122749ed4e2e 104 /*
alejo5214416 0:b91b6f47eb98 105 bno055.get_angles(); //query the i2c device
alejo5214416 1:74fb432755d0 106 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 107 bno055.get_lia(); //query the i2c device
alejo5214416 0:b91b6f47eb98 108 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 109 wait_ms(TS);
alejo5214416 6:122749ed4e2e 110 */
alejo5214416 6:122749ed4e2e 111
alejo5214416 6:122749ed4e2e 112 if(flag==true && flags==true){
alejo5214416 6:122749ed4e2e 113 vel_x_act = vel_x + TS * (acel_x+acel_x_ant)/2.0;
alejo5214416 6:122749ed4e2e 114 vel_y_act = vel_y + TS * (acel_y+acel_y_ant)/2.0;
alejo5214416 6:122749ed4e2e 115 vel_z_act = vel_z + TS * (acel_z+acel_y_ant)/2.0;
alejo5214416 6:122749ed4e2e 116
alejo5214416 6:122749ed4e2e 117 acel_x_ant = acel_x;
alejo5214416 6:122749ed4e2e 118 acel_y_ant = acel_y;
alejo5214416 6:122749ed4e2e 119 acel_z_ant = acel_z;
alejo5214416 6:122749ed4e2e 120
alejo5214416 6:122749ed4e2e 121 pos_x_act = pos_x + TS * (vel_x_act+vel_x)/2.0;
alejo5214416 6:122749ed4e2e 122 pos_y_act = pos_y + TS * (vel_y_act+vel_y)/2.0;
alejo5214416 6:122749ed4e2e 123 pos_z_act = pos_z + TS * (vel_z_act+vel_z)/2.0;
alejo5214416 6:122749ed4e2e 124
alejo5214416 6:122749ed4e2e 125 vel_x = vel_x_act;
alejo5214416 6:122749ed4e2e 126 vel_y = vel_y_act;
alejo5214416 6:122749ed4e2e 127 vel_z = vel_z_act;
alejo5214416 6:122749ed4e2e 128
alejo5214416 6:122749ed4e2e 129 pos_x = pos_x_act;
alejo5214416 6:122749ed4e2e 130 pos_y = pos_y_act;
alejo5214416 6:122749ed4e2e 131 pos_z = pos_z_act;
alejo5214416 6:122749ed4e2e 132 flags=false;
alejo5214416 6:122749ed4e2e 133 }
alejo5214416 6:122749ed4e2e 134 //bno055.resetIntr();
alejo5214416 6:122749ed4e2e 135 if(us_ticker_read()>(tant+1000000)){
alejo5214416 6:122749ed4e2e 136 tant=us_ticker_read();
alejo5214416 6:122749ed4e2e 137 pc.printf("Cambios\n\n\n");
alejo5214416 6:122749ed4e2e 138 pc.printf("ACEL %1.8f %1.8f %1.8f \n", acel_x, acel_y, acel_z);
alejo5214416 6:122749ed4e2e 139 pc.printf("VELOCIDAD actual %1.8f %1.8f %1.8f \n", vel_x_act, vel_y_act, vel_z_act);
alejo5214416 6:122749ed4e2e 140 pc.printf("POSICION actual %1.8f %1.8f %1.8f \n\n\n", pos_x_act, pos_y_act, pos_z_act);
alejo5214416 6:122749ed4e2e 141 pc.printf("Posicion actual %1.8f %1.8f %1.8f \n",pos_x_act, pos_y_act, pos_z_act);
alejo5214416 6:122749ed4e2e 142 }
alejo5214416 0:b91b6f47eb98 143 }
alejo5214416 0:b91b6f47eb98 144 }
alejo5214416 0:b91b6f47eb98 145
alejo5214416 0:b91b6f47eb98 146
alejo5214416 3:a4a031655cda 147 void event()
alejo5214416 3:a4a031655cda 148 {
alejo5214416 6:122749ed4e2e 149 flag=true;
alejo5214416 6:122749ed4e2e 150 led1 = 1;
alejo5214416 2:54bb4cee7885 151 //wait(10);
alejo5214416 2:54bb4cee7885 152 /*
alejo5214416 2:54bb4cee7885 153 float v_act=0;
alejo5214416 2:54bb4cee7885 154 float x_act=0;
alejo5214416 2:54bb4cee7885 155 float v_ant=0;
alejo5214416 2:54bb4cee7885 156 float x_ant=0;
alejo5214416 2:54bb4cee7885 157 bno055.get_angles(); //query the i2c device
alejo5214416 2:54bb4cee7885 158 bno055.get_lia(); //query the i2c device
alejo5214416 2:54bb4cee7885 159 v_act = v_ant + TS*float(bno055.lia.x);
alejo5214416 2:54bb4cee7885 160 wait(TS);
alejo5214416 2:54bb4cee7885 161 pc.printf("V_act = %f",v_act);
alejo5214416 2:54bb4cee7885 162 */
alejo5214416 6:122749ed4e2e 163 //bno055.resetIntr();
alejo5214416 6:122749ed4e2e 164 }
alejo5214416 6:122749ed4e2e 165
alejo5214416 6:122749ed4e2e 166
alejo5214416 6:122749ed4e2e 167 void tempo(){
alejo5214416 6:122749ed4e2e 168 led4 = !led4;
alejo5214416 6:122749ed4e2e 169 bno055.get_lia(); //query the i2c device
alejo5214416 6:122749ed4e2e 170 acel_x = bno055.lia.x;
alejo5214416 6:122749ed4e2e 171 acel_y = bno055.lia.y;
alejo5214416 6:122749ed4e2e 172 acel_z = bno055.lia.z;
alejo5214416 6:122749ed4e2e 173 flags=true;
alejo5214416 3:a4a031655cda 174 }