main.cpp

00001 #include "mbed.h"
00002 #include "x_nucleo_iks01a1.h"
00003 
00004 #define TEMPO 3     //in secondi
00005 #define ACCURATEZZA 2000
00006 #define CONVERSIONE_UNITA 1000
00007 #define INTERVALLO_MISURA 100 //in ms
00008 #define TS 10                //tempo di campionamento in ms
00009 
00010 bool Inizializzato=0;
00011 
00012 //deifinisco lo shield
00013 static X_NUCLEO_IKS01A1 *sensor_shield= X_NUCLEO_IKS01A1::Instance(D14, D15); //D14 e D15 sono i pind SDA e SCL sulla nucleo
00014 
00015 //definisco il giroscopio
00016 static GyroSensor *gyro= sensor_shield->GetGyroscope();
00017 
00018 uint8_t GyroID;
00019 int32_t W_ini[3],W_ris[3];//X,Y,Z
00020 int Posizione[3];//X, Y, Z
00021 DigitalOut led(LED1);
00022 
00023 //Inizializzo i valori del giroscopio (velocità angolare e posizione)
00024 bool Inizializza_Giroscopio(int32_t W[], int Pos[]){
00025     int32_t W_temp[]={0,0,0};
00026     printf("--- Inizializzo il giroscopio ---\r\n");  
00027     led=1;
00028     for(int i=0; i<TEMPO; i++){
00029         gyro->Get_G_Axes(W_temp);
00030         
00031         for(int z=0; z<3; z++)
00032             W[i]+=W_temp[i];
00033              
00034         wait(1);
00035     }
00036     for (int i=0; i<3; i++){
00037         W[i]= (W[i]/CONVERSIONE_UNITA)/TEMPO;
00038         Pos[i]=0;
00039     }
00040         
00041     led=0;  
00042     return (Inizializzato=1);
00043 }
00044 
00045 //Leggo la velocità angolare in mdps
00046 void Leggi_Velocita(int32_t W[], int32_t W_ini[]){
00047     int32_t W_letta[3];
00048     gyro->Get_G_Axes(W_letta);
00049     for(int i=0;i<3;i++){
00050         if(W_letta[i]>=(W_ini[i]-ACCURATEZZA) && W_letta[i]<=(W_ini[i]+ACCURATEZZA))
00051             W[i]=0;
00052         else
00053             W[i]= W_letta[i]/CONVERSIONE_UNITA; 
00054     }    
00055 }
00056 //Ricavo le posizioni in gradi
00057 void Leggi_Posizioni(int Pos[], int32_t W_ini[]){
00058     int32_t W[3]={0,0,0}, temp[3]={0,0,0};
00059     for(int tempo=0; tempo<INTERVALLO_MISURA; tempo++){
00060         Leggi_Velocita(temp, W_ini);
00061         for(int i=0; i<3; i++){
00062             W[i]+=temp[i];   
00063         }
00064         wait_ms(TS);
00065     }
00066     
00067     for(int i=0; i<3; i++){
00068         Pos[i]= (Pos[i]+W[i]*TS);    //TS è il tempo di campionamento 
00069         /*if(Pos[i]>360*CONVERSIONE_UNITA)
00070             Pos[i]-=360*CONVERSIONE_UNITA;
00071         else if(Pos[i]<-360*CONVERSIONE_UNITA)
00072             Pos[i]+=360*CONVERSIONE_UNITA;*/
00073     }
00074 }
00075 int main() {
00076     //controllo l'id del giroscopio
00077     gyro->ReadID(&GyroID);
00078     printf("Il giroscopio ha id: %d \r \n",GyroID);
00079     
00080     while(!Inizializza_Giroscopio(W_ini, Posizione));
00081     
00082     while(1) {
00083         printf("-------\r\n");
00084         Leggi_Velocita(W_ris, W_ini);
00085         Leggi_Posizioni(Posizione, W_ini);
00086         printf("X: %d dps\tY: %d dps\tZ: %d dps\r\n",W_ris[0],W_ris[1],W_ris[2]);
00087         printf("Pos_X: %d\tPos_Y: %d\tPos_Z: %d\r\n",Posizione[0]/CONVERSIONE_UNITA,Posizione[1]/CONVERSIONE_UNITA,Posizione[2]/CONVERSIONE_UNITA);        
00088        // wait(1);
00089     }
00090 }