duckietownhsunina / Mbed 2 deprecated AccpiuGiroscopio

sensori

Dependencies:   X_NUCLEO_IKS01A2 mbed

Fork of HelloWorld_IKS01A2 by duckietownhsunina

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

Committer:
vidica94
Date:
Thu Feb 16 11:25:20 2017 +0000
Revision:
16:f50bbf3ae57c
Parent:
15:25999e71b22d 
no 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:f50bbf3ae57c 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;
cparata 0:69566eea0fba 13
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:f50bbf3ae57c 18 int32_t mdps[3];
vidica94 16:f50bbf3ae57c 19 int32_t acc[3];
vidica94 13:2e809b3e6ea9 20 int32_t off[3];
vidica94 13:2e809b3e6ea9 21 float parziale[3];
vidica94 16:f50bbf3ae57c 22 float angolo[3];
vidica94 16:f50bbf3ae57c 23
vidica94 16:f50bbf3ae57c 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:f50bbf3ae57c 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:f50bbf3ae57c 38 // attendo l' inzializzazione
vidica94 16:f50bbf3ae57c 39 wait(1.5);
vidica94 16:f50bbf3ae57c 40
vidica94 16:f50bbf3ae57c 41 // effettuo una prima acquisizione del giroscopio per ottenere l'offset delle velocità angolari
vidica94 16:f50bbf3ae57c 42 acc_gyro->Get_G_Axes(mdps);
vidica94 16:f50bbf3ae57c 43 printf("LSM6DSL [gyro/mdps]: %6ld, %6ld, %6ld\r\n", mdps[0], mdps[1], mdps[2]);
vidica94 16:f50bbf3ae57c 44
vidica94 15:25999e71b22d 45 for(int i=0;i<3;i++){
vidica94 16:f50bbf3ae57c 46 off[i]=mdps[i];}
vidica94 16:f50bbf3ae57c 47
vidica94 13:2e809b3e6ea9 48 printf("off [gyro/mdps]: %6ld, %6ld, %6ld\r\n", off[0], off[1], off[2]);
vidica94 16:f50bbf3ae57c 49
cparata 0:69566eea0fba 50 while(1) {
cparata 0:69566eea0fba 51 printf("\r\n");
cparata 0:69566eea0fba 52
vidica94 16:f50bbf3ae57c 53 acc_gyro->Get_X_Axes(acc);
vidica94 15:25999e71b22d 54
vidica94 16:f50bbf3ae57c 55 printf("LSM6DSL [acc/mg]: %6ld, %6ld, %6ld\r\n", acc[0], acc[1], acc[2]);
cparata 0:69566eea0fba 56
vidica94 16:f50bbf3ae57c 57 acc_gyro->Get_G_Axes(mdps);
vidica94 16:f50bbf3ae57c 58 printf("LSM6DSLrow [gyro/mdps]: %6ld, %6ld, %6ld\r\n", mdps[0], mdps[1], mdps[2]);
vidica94 16:f50bbf3ae57c 59
vidica94 16:f50bbf3ae57c 60 // sottraggo l'offset
vidica94 13:2e809b3e6ea9 61 for(int i=0;i<3;i++)
vidica94 16:f50bbf3ae57c 62 {mdps[i]=mdps[i]-off[i];}
vidica94 16:f50bbf3ae57c 63 printf("LSM6DSLfine [gyro/mdps]: %6ld, %6ld, %6ld\r\n", mdps[0], mdps[1], mdps[2]);
vidica94 16:f50bbf3ae57c 64
vidica94 15:25999e71b22d 65 wait_ms(1);
vidica94 13:2e809b3e6ea9 66
vidica94 15:25999e71b22d 67 // ricavo il parziale dalla velocità angolare
vidica94 13:2e809b3e6ea9 68
vidica94 13:2e809b3e6ea9 69 for(int i=0;i<3;i++)
vidica94 14:de30b189c5ec 70 {
vidica94 16:f50bbf3ae57c 71 // passo da mdps a dpLSB
vidica94 15:25999e71b22d 72
vidica94 16:f50bbf3ae57c 73 parziale[i]=(mdps[i]*sens)/1000;
vidica94 16:f50bbf3ae57c 74 // levo la correzione per poter sommare i dati parziali off 19 coeff 2.84
vidica94 16:f50bbf3ae57c 75 angolo[i]=(angolo[i]- 19)/2.84;
vidica94 16:f50bbf3ae57c 76 // moltiplico per il dt (1ms)
vidica94 16:f50bbf3ae57c 77 parziale[i]/= 1000;
vidica94 15:25999e71b22d 78
vidica94 16:f50bbf3ae57c 79 if (mdps[i]>140 ||mdps[i]<-140)// ci si puo mettere anche 70 come soglia non cambia molto
vidica94 16:f50bbf3ae57c 80 angolo[i] += parziale[i]; // integro
vidica94 16:f50bbf3ae57c 81
vidica94 16:f50bbf3ae57c 82 // correggo offset e guadagno che ho ricavato da una "taratura" grezza (ricavo la retta )
vidica94 15:25999e71b22d 83
vidica94 16:f50bbf3ae57c 84 angolo[i]=(angolo[i]*2.84)+19;
vidica94 15:25999e71b22d 85 }
cparata 0:69566eea0fba 86
vidica94 14:de30b189c5ec 87
vidica94 16:f50bbf3ae57c 88 printf("angolo [gyro/d]: %6f, %6f, %6f\r\n", angolo[0], angolo[1], angolo[2]);//angolo
cparata 0:69566eea0fba 89 }
cparata 0:69566eea0fba 90 }