File content as of revision 0:3d72a547faad:

#include "mbed.h"
#include "x_nucleo_iks01a1.h"

#define TEMPO 3     //in secondi
#define ACCURATEZZA 2000
#define CONVERSIONE_UNITA 1000
#define INTERVALLO_MISURA 100 //in ms
#define TS 10                //tempo di campionamento in ms

bool Inizializzato=0;

//deifinisco lo shield
static X_NUCLEO_IKS01A1 *sensor_shield= X_NUCLEO_IKS01A1::Instance(D14, D15); //D14 e D15 sono i pind SDA e SCL sulla nucleo

//definisco il giroscopio
static GyroSensor *gyro= sensor_shield->GetGyroscope();

uint8_t GyroID;
int32_t W_ini[3],W_ris[3];//X,Y,Z
int Posizione[3];//X, Y, Z
DigitalOut led(LED1);

//Inizializzo i valori del giroscopio (velocità angolare e posizione)
bool Inizializza_Giroscopio(int32_t W[], int Pos[]){
    int32_t W_temp[]={0,0,0};
    printf("--- Inizializzo il giroscopio ---\r\n");  
    led=1;
    for(int i=0; i<TEMPO; i++){
        gyro->Get_G_Axes(W_temp);
        
        for(int z=0; z<3; z++)
            W[i]+=W_temp[i];
             
        wait(1);
    }
    for (int i=0; i<3; i++){
        W[i]= (W[i]/CONVERSIONE_UNITA)/TEMPO;
        Pos[i]=0;
    }
        
    led=0;  
    return (Inizializzato=1);
}

//Leggo la velocità angolare in mdps
void Leggi_Velocita(int32_t W[], int32_t W_ini[]){
    int32_t W_letta[3];
    gyro->Get_G_Axes(W_letta);
    for(int i=0;i<3;i++){
        if(W_letta[i]>=(W_ini[i]-ACCURATEZZA) && W_letta[i]<=(W_ini[i]+ACCURATEZZA))
            W[i]=0;
        else
            W[i]= W_letta[i]/CONVERSIONE_UNITA; 
    }    
}
//Ricavo le posizioni in gradi
void Leggi_Posizioni(int Pos[], int32_t W_ini[]){
    int32_t W[3]={0,0,0}, temp[3]={0,0,0};
    for(int tempo=0; tempo<INTERVALLO_MISURA; tempo++){
        Leggi_Velocita(temp, W_ini);
        for(int i=0; i<3; i++){
            W[i]+=temp[i];   
        }
        wait_ms(TS);
    }
    
    for(int i=0; i<3; i++){
        Pos[i]= (Pos[i]+W[i]*TS);    //TS è il tempo di campionamento 
        /*if(Pos[i]>360*CONVERSIONE_UNITA)
            Pos[i]-=360*CONVERSIONE_UNITA;
        else if(Pos[i]<-360*CONVERSIONE_UNITA)
            Pos[i]+=360*CONVERSIONE_UNITA;*/
    }
}
int main() {
    //controllo l'id del giroscopio
    gyro->ReadID(&GyroID);
    printf("Il giroscopio ha id: %d \r \n",GyroID);
    
    while(!Inizializza_Giroscopio(W_ini, Posizione));
    
    while(1) {
        printf("-------\r\n");
        Leggi_Velocita(W_ris, W_ini);
        Leggi_Posizioni(Posizione, W_ini);
        printf("X: %d dps\tY: %d dps\tZ: %d dps\r\n",W_ris[0],W_ris[1],W_ris[2]);
        printf("Pos_X: %d\tPos_Y: %d\tPos_Z: %d\r\n",Posizione[0]/CONVERSIONE_UNITA,Posizione[1]/CONVERSIONE_UNITA,Posizione[2]/CONVERSIONE_UNITA);        
       // wait(1);
    }
}