duckietownhsunina / Mbed 2 deprecated HelloWorld_IKS01A2

sensori

Dependencies:   X_NUCLEO_IKS01A2 mbed

Fork of HelloWorld_IKS01A2 by niente

main.cpp@16:553a1c68d606, 2017-02-16 (annotated)

Committer:
vidica94
Date:
Thu Feb 16 11:23:33 2017 +0000
Revision:
16:553a1c68d606
Parent:
15:25999e71b22d 
stesso programma di prima con i timer

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