duckietownhsunina / Mbed 2 deprecated AccpiuGiroscopio_timer_copy

soloz

Dependencies:   X_NUCLEO_IKS01A2 mbed

Fork of AccpiuGiroscopio_timer by duckietownhsunina

main.cpp@17:a8d332cc6cad, 2017-02-17 (annotated)

Committer:
vidica94
Date:
Fri Feb 17 10:03:58 2017 +0000
Revision:
17:a8d332cc6cad
Parent:
16:553a1c68d606 
solo z

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