DuckieTownHS_UniNA / Mbed 2 deprecated Giroscopio_timer_fast

solo z veloce

Dependencies:   X_NUCLEO_IKS01A2 mbed

Fork of Giroscopio_timer_fast by duckietownhsunina

main.cpp@19:ba121767de70, 2017-03-07 (annotated)

Committer:
vidica94
Date:
Tue Mar 07 14:45:59 2017 +0000
Revision:
19:ba121767de70
Parent:
18:da0744a1b128 
versione veloce

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cparata 0:69566eea0fba 1
cparata 0:69566eea0fba 2 /* Includes */
cparata 0:69566eea0fba 3 #include "mbed.h"
cparata 0:69566eea0fba 4 #include "x_nucleo_iks01a2.h"
cparata 0:69566eea0fba 5
cparata 0:69566eea0fba 6 /* Instantiate the expansion board */
cparata 8:8f495e604424 7 static X_NUCLEO_IKS01A2 *mems_expansion_board = X_NUCLEO_IKS01A2::Instance(D14, D15, D4, D5);
cparata 0:69566eea0fba 8
vidica94 17:a8d332cc6cad 9 Serial pc(SERIAL_TX, SERIAL_RX);
cparata 0:69566eea0fba 10
vidica94 16:553a1c68d606 11 #define sens 70 // sensibilità presa dal datasheet per il fondo scala utilizzato
cparata 0:69566eea0fba 12
vidica94 13:2e809b3e6ea9 13 static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
vidica94 16:553a1c68d606 14 Timer t;
cparata 0:69566eea0fba 15
cparata 0:69566eea0fba 16 /* Simple main function */
vidica94 19:ba121767de70 17 int main()
vidica94 19:ba121767de70 18 {
vidica94 19:ba121767de70 19 uint8_t id;
vidica94 19:ba121767de70 20 int32_t mdps[3];
vidica94 19:ba121767de70 21 int32_t acc[3];
vidica94 19:ba121767de70 22 int32_t off[3];
vidica94 19:ba121767de70 23 float parziale[3];
vidica94 19:ba121767de70 24 float angolo[3];
vidica94 19:ba121767de70 25 float dt=0;
vidica94 19:ba121767de70 26
vidica94 19:ba121767de70 27
vidica94 19:ba121767de70 28 parziale[2]=0;
vidica94 19:ba121767de70 29
vidica94 19:ba121767de70 30
vidica94 19:ba121767de70 31 angolo [2]=0;
vidica94 19:ba121767de70 32 /* Enable sensors */
vidica94 19:ba121767de70 33
vidica94 19:ba121767de70 34
vidica94 19:ba121767de70 35 acc_gyro->Enable_G();
vidica94 19:ba121767de70 36
vidica94 19:ba121767de70 37 pc.printf("\r\n--- Starting new run ---\r\n");
vidica94 19:ba121767de70 38 acc_gyro->ReadID(&id);
vidica94 19:ba121767de70 39 pc.printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
vidica94 19:ba121767de70 40 // attendo l' inzializzazione
vidica94 19:ba121767de70 41 wait(1.5);
vidica94 19:ba121767de70 42
vidica94 19:ba121767de70 43 // effettuo una prima acquisizione del giroscopio per ottenere l'offset delle velocità angolari
vidica94 19:ba121767de70 44 acc_gyro->Get_G_Axes(mdps);
vidica94 19:ba121767de70 45 pc.printf("LSM6DSL [gyro/mdps]: %6ld\r\n", mdps[2]);
vidica94 19:ba121767de70 46
vidica94 13:2e809b3e6ea9 47
vidica94 19:ba121767de70 48 off[2]=mdps[2];
vidica94 19:ba121767de70 49
vidica94 19:ba121767de70 50 pc.printf("off [gyro/mdps]: %6ld\r\n", off[2]);
vidica94 19:ba121767de70 51
vidica94 19:ba121767de70 52 while(1) {
vidica94 19:ba121767de70 53
vidica94 19:ba121767de70 54 pc.printf("\r\n");
vidica94 19:ba121767de70 55
vidica94 19:ba121767de70 56
vidica94 19:ba121767de70 57 t.stop();
vidica94 19:ba121767de70 58 dt= t.read();
vidica94 19:ba121767de70 59 pc.printf("The time taken was %d milliseconds\n", t.read_ms());
vidica94 19:ba121767de70 60 t.reset();
vidica94 19:ba121767de70 61 acc_gyro->Get_G_Axes(mdps);
vidica94 16:553a1c68d606 62
vidica94 16:553a1c68d606 63
vidica94 19:ba121767de70 64 t.start();
vidica94 19:ba121767de70 65
vidica94 19:ba121767de70 66 //pc.printf("LSM6DSLrow [gyro/mdps]: %6ld\r\n", mdps[2]);
vidica94 19:ba121767de70 67
vidica94 19:ba121767de70 68 // sottraggo l'offset
vidica94 17:a8d332cc6cad 69
vidica94 19:ba121767de70 70 mdps[2]=mdps[2]-off[2];
vidica94 19:ba121767de70 71 pc.printf("LSM6DSLfine [gyro/mdps]: %6ld\r\n",mdps[2]);
vidica94 19:ba121767de70 72
vidica94 19:ba121767de70 73 //wait_ms(1);
cparata 0:69566eea0fba 74
vidica94 19:ba121767de70 75
vidica94 19:ba121767de70 76 // ricavo il parziale dalla velocità angolare
vidica94 19:ba121767de70 77
vidica94 19:ba121767de70 78
vidica94 16:553a1c68d606 79 // passo da mdps a dpLSB
vidica94 15:25999e71b22d 80
vidica94 19:ba121767de70 81 parziale[2]=(mdps[2]*sens)/1000;
vidica94 19:ba121767de70 82
vidica94 16:553a1c68d606 83 // levo la correzione per poter sommare i dati parziali off 19 coeff 2.84
vidica94 19:ba121767de70 84 angolo[2]=(angolo[2]-(0.724005))/0.0226195;
vidica94 19:ba121767de70 85 // moltiplico per il dt
vidica94 19:ba121767de70 86 parziale[2]*=(dt);
vidica94 19:ba121767de70 87
vidica94 19:ba121767de70 88 if (mdps[2]>70 ||mdps[2]<-70)// ci si puo mettere anche 70 come soglia non cambia molto
vidica94 19:ba121767de70 89 angolo[2] += parziale[2]; // integro
vidica94 19:ba121767de70 90
vidica94 16:553a1c68d606 91 // correggo offset e guadagno che ho ricavato da una "taratura" (ricavo la best fit line )
vidica94 19:ba121767de70 92
vidica94 19:ba121767de70 93 angolo[2]=(angolo[2]*0.0226195)+(0.724005);
vidica94 17:a8d332cc6cad 94
cparata 0:69566eea0fba 95
vidica94 14:de30b189c5ec 96
vidica94 19:ba121767de70 97 pc.printf("angolo [gyro/d]: %6f\r\n", angolo[2]);//angolo
vidica94 19:ba121767de70 98 }
cparata 0:69566eea0fba 99 }