Alejandro Giraldo Martinez / Mbed 2 deprecated BNO055_basic_copy

.

Dependencies:   BNO055 SDFileSystem mbed

main.cpp@7:27d1f57f8030, 2018-10-03 (annotated)

Committer:
alejo5214416
Date:
Wed Oct 03 03:18:53 2018 +0000
Revision:
7:27d1f57f8030
Parent:
6:122749ed4e2e
Child:
8:67c4e64bccb6
Integracion correcta

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 7:27d1f57f8030 17 bool flagi = false, flags=false;
alejo5214416 6:122749ed4e2e 18 float acel_x, acel_y, acel_z,acel_x_ant,acel_y_ant,acel_z_ant;
alejo5214416 7:27d1f57f8030 19 int conteo=0;
alejo5214416 3:a4a031655cda 20
alejo5214416 3:a4a031655cda 21 void event();
alejo5214416 6:122749ed4e2e 22 void tempo();
alejo5214416 0:b91b6f47eb98 23
alejo5214416 3:a4a031655cda 24 int main()
alejo5214416 3:a4a031655cda 25 {
alejo5214416 7:27d1f57f8030 26 //float acel_x_act, acel_y_act, acel_z_act;
alejo5214416 6:122749ed4e2e 27 float vel_x=0, vel_y=0, vel_z=0;
alejo5214416 6:122749ed4e2e 28 float vel_x_act=0, vel_y_act=0, vel_z_act=0;
alejo5214416 6:122749ed4e2e 29
alejo5214416 6:122749ed4e2e 30 float pos_x=0, pos_y=0, pos_z=0;
alejo5214416 6:122749ed4e2e 31 float pos_x_act=0, pos_y_act=0, pos_z_act=0;
alejo5214416 6:122749ed4e2e 32 uint32_t tant=0;
alejo5214416 6:122749ed4e2e 33
alejo5214416 3:a4a031655cda 34 green=0;
alejo5214416 3:a4a031655cda 35 blue=0;
alejo5214416 3:a4a031655cda 36 red=1;
alejo5214416 6:122749ed4e2e 37 int_tempo.attach(&tempo, TS); // the address of the function to be attached (tempo) and the interval (0.1)
alejo5214416 0:b91b6f47eb98 38 bno055.reset();
alejo5214416 3:a4a031655cda 39 interrupt.rise(&event);
alejo5214416 0:b91b6f47eb98 40 bno055.setmode(OPERATION_MODE_NDOF);
alejo5214416 6:122749ed4e2e 41 //bno055.write_calibration_data();
alejo5214416 0:b91b6f47eb98 42 bno055.get_calib();
alejo5214416 6:122749ed4e2e 43 //bno055.write_calibration_data();
alejo5214416 7:27d1f57f8030 44
alejo5214416 6:122749ed4e2e 45 while(((bno055.calib>>4)&3)!= 3) {
alejo5214416 3:a4a031655cda 46 red=1;
alejo5214416 3:a4a031655cda 47 blue=0;
alejo5214416 3:a4a031655cda 48 green=0;
alejo5214416 0:b91b6f47eb98 49 bno055.get_calib();
alejo5214416 7:27d1f57f8030 50 wait_ms(50);
alejo5214416 3:a4a031655cda 51 red = 0;
alejo5214416 7:27d1f57f8030 52 wait_ms(50);
alejo5214416 3:a4a031655cda 53 pc.printf("Calibrando giroscopio= %d\n",(bno055.calib>>4)&(3));
alejo5214416 0:b91b6f47eb98 54 }
alejo5214416 3:a4a031655cda 55 pc.printf("GIROSCOPIO CALIBRADO= %d\n",(bno055.calib>>4)&(3));
alejo5214416 3:a4a031655cda 56 wait(2);
alejo5214416 3:a4a031655cda 57
alejo5214416 3:a4a031655cda 58
alejo5214416 3:a4a031655cda 59 red=0;
alejo5214416 6:122749ed4e2e 60 //bno055.write_calibration_data();
alejo5214416 3:a4a031655cda 61 bno055.get_calib();
alejo5214416 7:27d1f57f8030 62
alejo5214416 3:a4a031655cda 63 while((bno055.calib & 3) != 3) {
alejo5214416 3:a4a031655cda 64 red=0;
alejo5214416 3:a4a031655cda 65 blue=1;
alejo5214416 3:a4a031655cda 66 green=0;
alejo5214416 6:122749ed4e2e 67 //bno055.write_calibration_data();
alejo5214416 3:a4a031655cda 68 bno055.get_calib();
alejo5214416 7:27d1f57f8030 69 wait_ms(50);
alejo5214416 3:a4a031655cda 70 blue = 0;
alejo5214416 7:27d1f57f8030 71 wait_ms(50);
alejo5214416 3:a4a031655cda 72 pc.printf("Calibrando Magnetometro= %d\n",bno055.calib & (3));
alejo5214416 3:a4a031655cda 73 }
alejo5214416 7:27d1f57f8030 74
alejo5214416 3:a4a031655cda 75 pc.printf("MAGNETOMETRO CALIBRADO= %d\n",bno055.calib & (3));
alejo5214416 3:a4a031655cda 76 wait(2);
alejo5214416 3:a4a031655cda 77
alejo5214416 3:a4a031655cda 78 blue=0;
alejo5214416 6:122749ed4e2e 79 //bno055.write_calibration_data();
alejo5214416 3:a4a031655cda 80 bno055.get_calib();
alejo5214416 7:27d1f57f8030 81 while(((bno055.calib>>2)& 3 )!= 1) {
alejo5214416 3:a4a031655cda 82 red=0;
alejo5214416 3:a4a031655cda 83 blue=0;
alejo5214416 3:a4a031655cda 84 green=1;
alejo5214416 6:122749ed4e2e 85 //bno055.write_calibration_data();
alejo5214416 3:a4a031655cda 86 bno055.get_calib();
alejo5214416 7:27d1f57f8030 87 wait_ms(500);
alejo5214416 3:a4a031655cda 88 green = 0;
alejo5214416 7:27d1f57f8030 89 wait_ms(500);
alejo5214416 3:a4a031655cda 90 pc.printf("Calibrando acelerometro= %d\n",(bno055.calib>>2)&(3));
alejo5214416 3:a4a031655cda 91 }
alejo5214416 7:27d1f57f8030 92
alejo5214416 7:27d1f57f8030 93
alejo5214416 3:a4a031655cda 94 pc.printf("ACELEROMETRO CALIBRADO= %d\n",(bno055.calib>>2)&(3));
alejo5214416 3:a4a031655cda 95 pc.printf("IMU CORRECTAMENTE CALIBRADA= %d\n",(bno055.calib>>2)&(3));
alejo5214416 3:a4a031655cda 96 wait(2);
alejo5214416 3:a4a031655cda 97 green=1;
alejo5214416 3:a4a031655cda 98
alejo5214416 7:27d1f57f8030 99 if (bno055.check()) {
alejo5214416 1:74fb432755d0 100 bno055.initIntr();
alejo5214416 3:a4a031655cda 101 }
alejo5214416 3:a4a031655cda 102
alejo5214416 0:b91b6f47eb98 103 while(1) {
alejo5214416 3:a4a031655cda 104
alejo5214416 6:122749ed4e2e 105 /*
alejo5214416 0:b91b6f47eb98 106 bno055.get_angles(); //query the i2c device
alejo5214416 1:74fb432755d0 107 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 108 bno055.get_lia(); //query the i2c device
alejo5214416 0:b91b6f47eb98 109 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 110 wait_ms(TS);
alejo5214416 6:122749ed4e2e 111 */
alejo5214416 7:27d1f57f8030 112
alejo5214416 7:27d1f57f8030 113 if(flagi==true && flags==true) {
alejo5214416 7:27d1f57f8030 114 led1 = 1;
alejo5214416 6:122749ed4e2e 115 vel_x_act = vel_x + TS * (acel_x+acel_x_ant)/2.0;
alejo5214416 6:122749ed4e2e 116 vel_y_act = vel_y + TS * (acel_y+acel_y_ant)/2.0;
alejo5214416 6:122749ed4e2e 117 vel_z_act = vel_z + TS * (acel_z+acel_y_ant)/2.0;
alejo5214416 7:27d1f57f8030 118
alejo5214416 6:122749ed4e2e 119 acel_x_ant = acel_x;
alejo5214416 6:122749ed4e2e 120 acel_y_ant = acel_y;
alejo5214416 6:122749ed4e2e 121 acel_z_ant = acel_z;
alejo5214416 7:27d1f57f8030 122
alejo5214416 6:122749ed4e2e 123 pos_x_act = pos_x + TS * (vel_x_act+vel_x)/2.0;
alejo5214416 6:122749ed4e2e 124 pos_y_act = pos_y + TS * (vel_y_act+vel_y)/2.0;
alejo5214416 6:122749ed4e2e 125 pos_z_act = pos_z + TS * (vel_z_act+vel_z)/2.0;
alejo5214416 7:27d1f57f8030 126
alejo5214416 6:122749ed4e2e 127 vel_x = vel_x_act;
alejo5214416 6:122749ed4e2e 128 vel_y = vel_y_act;
alejo5214416 6:122749ed4e2e 129 vel_z = vel_z_act;
alejo5214416 7:27d1f57f8030 130
alejo5214416 6:122749ed4e2e 131 pos_x = pos_x_act;
alejo5214416 6:122749ed4e2e 132 pos_y = pos_y_act;
alejo5214416 6:122749ed4e2e 133 pos_z = pos_z_act;
alejo5214416 6:122749ed4e2e 134 flags=false;
alejo5214416 7:27d1f57f8030 135 }
alejo5214416 7:27d1f57f8030 136 //bno055.resetIntr();
alejo5214416 7:27d1f57f8030 137 if(us_ticker_read()>(tant+1000000)) {
alejo5214416 7:27d1f57f8030 138 tant=us_ticker_read();
alejo5214416 7:27d1f57f8030 139 pc.printf("ACEL %2.4f %2.4f %2.4f \n", acel_x, acel_y, acel_z);
alejo5214416 7:27d1f57f8030 140 pc.printf("VELOCIDAD actual %2.4f %2.4f %2.4f \n", vel_x_act, vel_y_act, vel_z_act);
alejo5214416 7:27d1f57f8030 141 pc.printf("Posicion actual %2.4f %2.4f %2.4f \n\n\n",pos_x_act, pos_y_act, pos_z_act);
alejo5214416 7:27d1f57f8030 142
alejo5214416 7:27d1f57f8030 143 }
alejo5214416 0:b91b6f47eb98 144 }
alejo5214416 0:b91b6f47eb98 145 }
alejo5214416 0:b91b6f47eb98 146
alejo5214416 0:b91b6f47eb98 147
alejo5214416 3:a4a031655cda 148 void event()
alejo5214416 3:a4a031655cda 149 {
alejo5214416 7:27d1f57f8030 150
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 7:27d1f57f8030 167 void tempo()
alejo5214416 7:27d1f57f8030 168 {
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 7:27d1f57f8030 173 if(acel_x<0.03 && acel_y<0.03 && acel_z<0.03) {
alejo5214416 7:27d1f57f8030 174 conteo++;
alejo5214416 7:27d1f57f8030 175 if (conteo>=4) {
alejo5214416 7:27d1f57f8030 176 flagi=false;
alejo5214416 7:27d1f57f8030 177 led1==0;
alejo5214416 7:27d1f57f8030 178 bno055.resetIntr();
alejo5214416 7:27d1f57f8030 179 }
alejo5214416 7:27d1f57f8030 180 } else {
alejo5214416 7:27d1f57f8030 181 conteo=0;
alejo5214416 7:27d1f57f8030 182 flagi=true;
alejo5214416 7:27d1f57f8030 183 }
alejo5214416 6:122749ed4e2e 184 flags=true;
alejo5214416 3:a4a031655cda 185 }