duckietownhsunina / Mbed 2 deprecated AccpiuGiroscopio_timer

con timer

Dependencies:   X_NUCLEO_IKS01A2 mbed

Fork of HelloWorld_IKS01A2 by duckietownhsunina

main.cpp@15:25999e71b22d, 2017-02-11 (annotated)

Committer:
vidica94
Date:
Sat Feb 11 15:18:54 2017 +0000
Revision:
15:25999e71b22d
Parent:
14:de30b189c5ec
Child:
16:553a1c68d606
funziona abbastanza bene ora

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 13:2e809b3e6ea9 10 #define sens 70
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;
cparata 0:69566eea0fba 18 int32_t axes[3];
vidica94 13:2e809b3e6ea9 19 int32_t off[3];
vidica94 13:2e809b3e6ea9 20 float parziale[3];
vidica94 13:2e809b3e6ea9 21 float finale[3];
vidica94 13:2e809b3e6ea9 22 int32_t k=0;
vidica94 13:2e809b3e6ea9 23 for(int i=0;i<3;i++)
vidica94 13:2e809b3e6ea9 24 {parziale[i]=0;}
vidica94 13:2e809b3e6ea9 25
vidica94 13:2e809b3e6ea9 26 for(int i=0;i<3;i++)
vidica94 13:2e809b3e6ea9 27 {finale [i]=0;}
cparata 0:69566eea0fba 28 /* Enable all sensors */
vidica94 13:2e809b3e6ea9 29
cparata 0:69566eea0fba 30 acc_gyro->Enable_X();
cparata 0:69566eea0fba 31 acc_gyro->Enable_G();
cparata 0:69566eea0fba 32
cparata 0:69566eea0fba 33 printf("\r\n--- Starting new run ---\r\n");
cparata 0:69566eea0fba 34 acc_gyro->ReadID(&id);
cparata 0:69566eea0fba 35 printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
vidica94 13:2e809b3e6ea9 36 wait(1.5);
vidica94 13:2e809b3e6ea9 37 acc_gyro->Get_G_Axes(axes);
vidica94 13:2e809b3e6ea9 38 printf("LSM6DSL [gyro/mdps]: %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
vidica94 15:25999e71b22d 39 for(int i=0;i<3;i++){
vidica94 13:2e809b3e6ea9 40 off[i]=axes[i];}
vidica94 13:2e809b3e6ea9 41 printf("off [gyro/mdps]: %6ld, %6ld, %6ld\r\n", off[0], off[1], off[2]);
cparata 0:69566eea0fba 42 while(1) {
cparata 0:69566eea0fba 43 printf("\r\n");
cparata 0:69566eea0fba 44
cparata 0:69566eea0fba 45 acc_gyro->Get_X_Axes(axes);
vidica94 15:25999e71b22d 46
cparata 0:69566eea0fba 47 printf("LSM6DSL [acc/mg]: %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
cparata 0:69566eea0fba 48
cparata 0:69566eea0fba 49 acc_gyro->Get_G_Axes(axes);
vidica94 13:2e809b3e6ea9 50 printf("LSM6DSLrow [gyro/mdps]: %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
vidica94 13:2e809b3e6ea9 51 for(int i=0;i<3;i++)
vidica94 13:2e809b3e6ea9 52 {axes[i]=axes[i]-off[i];}
vidica94 13:2e809b3e6ea9 53 printf("LSM6DSLfine [gyro/mdps]: %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
vidica94 13:2e809b3e6ea9 54 k=k+1;
vidica94 15:25999e71b22d 55 wait_ms(1);
vidica94 13:2e809b3e6ea9 56
vidica94 15:25999e71b22d 57 // ricavo il parziale dalla velocità angolare
vidica94 13:2e809b3e6ea9 58
vidica94 13:2e809b3e6ea9 59 for(int i=0;i<3;i++)
vidica94 14:de30b189c5ec 60 {
vidica94 15:25999e71b22d 61 parziale[i]=(axes[i]*sens)/1000;// passo da mdps a dpLSB
vidica94 15:25999e71b22d 62
vidica94 15:25999e71b22d 63 finale[i]=(finale[i]-19)/2.84;// levo la correzione per poter sommare i dati parziali
vidica94 15:25999e71b22d 64 parziale[i]/= 1000;// moltiplico per il dt (1ms)
vidica94 15:25999e71b22d 65 if (axes[i]>150 ||axes[i]<-150)
vidica94 15:25999e71b22d 66 finale[i] += parziale[i]; // integro
vidica94 15:25999e71b22d 67
vidica94 15:25999e71b22d 68 finale[i]=(finale[i]*2.84)+19;// correggo offset e guadagno che ho ricavato da una "taratura" grezza (ricavo la retta )
vidica94 15:25999e71b22d 69
vidica94 15:25999e71b22d 70 }
cparata 0:69566eea0fba 71
vidica94 14:de30b189c5ec 72
vidica94 13:2e809b3e6ea9 73 printf("finale [gyro/d]: %6f, %6f, %6f\r\n", finale[0], finale[1], finale[2]);//angolo
cparata 0:69566eea0fba 74 }
cparata 0:69566eea0fba 75 }