main.cpp

00001 
00002 /* Includes */
00003 #include "mbed.h"
00004 #include "x_nucleo_iks01a2.h"
00005 
00006 /* Instantiate the expansion board */
00007 static X_NUCLEO_IKS01A2 *mems_expansion_board = X_NUCLEO_IKS01A2::Instance(D14, D15, D4, D5);
00008 
00009 Serial pc(SERIAL_TX, SERIAL_RX);
00010 
00011 #define  sens 70 // sensibilità presa dal datasheet per il fondo scala utilizzato 
00012 
00013 static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
00014 Timer t;
00015 
00016 /* Simple main function */
00017 int main() {
00018   uint8_t id;
00019   int32_t mdps[3];
00020   int32_t acc[3];
00021   int32_t off[3];
00022   float parziale[3];
00023   float angolo[3];
00024   float dt=0;
00025   
00026   
00027 parziale[2]=0;
00028 
00029  
00030 angolo [2]=0;
00031   /* Enable all sensors */
00032  
00033   acc_gyro->Enable_X();
00034   acc_gyro->Enable_G();
00035   
00036   pc.printf("\r\n--- Starting new run ---\r\n");
00037   acc_gyro->ReadID(&id);
00038   pc.printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
00039   // attendo l' inzializzazione
00040   wait(1.5);    
00041   
00042   // effettuo una prima acquisizione del giroscopio per ottenere l'offset delle velocità angolari
00043  acc_gyro->Get_G_Axes(mdps);   
00044  pc.printf("LSM6DSL [gyro/mdps]:    %6ld\r\n",  mdps[2]);
00045  
00046  
00047 off[2]=mdps[2];
00048 
00049   pc.printf("off [gyro/mdps]:  %6ld\r\n",  off[2]);
00050   
00051   while(1) {
00052     
00053     pc.printf("\r\n");
00054   
00055     
00056  t.stop();
00057    dt= t.read();
00058        pc.printf("The time taken was %d milliseconds\n", t.read_ms());
00059         
00060         
00061     acc_gyro->Get_G_Axes(mdps);
00062     
00063     t.reset();
00064     t.start();
00065     
00066     //pc.printf("LSM6DSLrow [gyro/mdps]:    %6ld\r\n", mdps[2]);
00067     
00068     // sottraggo l'offset
00069 
00070 mdps[2]=mdps[2]-off[2];
00071        pc.printf("LSM6DSLfine [gyro/mdps]:   %6ld\r\n",mdps[2]);
00072 
00073     //wait_ms(1);
00074     
00075     
00076     // ricavo il parziale dalla velocità angolare
00077     
00078  
00079 // passo da mdps a dpLSB
00080 
00081     parziale[2]=(mdps[2]*sens)/1000;
00082     
00083 // levo la correzione per poter sommare i dati parziali off 19 coeff 2.84
00084 angolo[2]=(angolo[2]-(0.724005))/0.0226195;
00085 // moltiplico per il dt 
00086     parziale[2]*=(dt);
00087     
00088    if (mdps[2]>70 ||mdps[2]<-70)// ci si puo mettere anche 70 come soglia non cambia molto
00089     angolo[2] += parziale[2];     // integro 
00090     
00091 // correggo offset e guadagno che ho ricavato da una "taratura" (ricavo la best fit line  )
00092   
00093    angolo[2]=(angolo[2]*0.0226195)+(0.724005);
00094 
00095 
00096 
00097     pc.printf("angolo  [gyro/d]:  %6f\r\n", angolo[2]);//angolo
00098   }
00099 }