Due Ingegneri Ed Un Controllore / Mbed 2 deprecated Giroscopio_main

Prova

Dependencies:   Giroscopio X_NUCLEO_IKS01A2 mbed

main.cpp@0:3d72a547faad, 2017-02-09 (annotated)

Committer:
Salvatore94
Date:
Thu Feb 09 16:42:59 2017 +0000
Revision:
0:3d72a547faad
Peppe;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Salvatore94 0:3d72a547faad 1 #include "mbed.h"
Salvatore94 0:3d72a547faad 2 #include "x_nucleo_iks01a1.h"
Salvatore94 0:3d72a547faad 3
Salvatore94 0:3d72a547faad 4 #define TEMPO 3 //in secondi
Salvatore94 0:3d72a547faad 5 #define ACCURATEZZA 2000
Salvatore94 0:3d72a547faad 6 #define CONVERSIONE_UNITA 1000
Salvatore94 0:3d72a547faad 7 #define INTERVALLO_MISURA 100 //in ms
Salvatore94 0:3d72a547faad 8 #define TS 10 //tempo di campionamento in ms
Salvatore94 0:3d72a547faad 9
Salvatore94 0:3d72a547faad 10 bool Inizializzato=0;
Salvatore94 0:3d72a547faad 11
Salvatore94 0:3d72a547faad 12 //deifinisco lo shield
Salvatore94 0:3d72a547faad 13 static X_NUCLEO_IKS01A1 *sensor_shield= X_NUCLEO_IKS01A1::Instance(D14, D15); //D14 e D15 sono i pind SDA e SCL sulla nucleo
Salvatore94 0:3d72a547faad 14
Salvatore94 0:3d72a547faad 15 //definisco il giroscopio
Salvatore94 0:3d72a547faad 16 static GyroSensor *gyro= sensor_shield->GetGyroscope();
Salvatore94 0:3d72a547faad 17
Salvatore94 0:3d72a547faad 18 uint8_t GyroID;
Salvatore94 0:3d72a547faad 19 int32_t W_ini[3],W_ris[3];//X,Y,Z
Salvatore94 0:3d72a547faad 20 int Posizione[3];//X, Y, Z
Salvatore94 0:3d72a547faad 21 DigitalOut led(LED1);
Salvatore94 0:3d72a547faad 22
Salvatore94 0:3d72a547faad 23 //Inizializzo i valori del giroscopio (velocità angolare e posizione)
Salvatore94 0:3d72a547faad 24 bool Inizializza_Giroscopio(int32_t W[], int Pos[]){
Salvatore94 0:3d72a547faad 25 int32_t W_temp[]={0,0,0};
Salvatore94 0:3d72a547faad 26 printf("--- Inizializzo il giroscopio ---\r\n");
Salvatore94 0:3d72a547faad 27 led=1;
Salvatore94 0:3d72a547faad 28 for(int i=0; i<TEMPO; i++){
Salvatore94 0:3d72a547faad 29 gyro->Get_G_Axes(W_temp);
Salvatore94 0:3d72a547faad 30
Salvatore94 0:3d72a547faad 31 for(int z=0; z<3; z++)
Salvatore94 0:3d72a547faad 32 W[i]+=W_temp[i];
Salvatore94 0:3d72a547faad 33
Salvatore94 0:3d72a547faad 34 wait(1);
Salvatore94 0:3d72a547faad 35 }
Salvatore94 0:3d72a547faad 36 for (int i=0; i<3; i++){
Salvatore94 0:3d72a547faad 37 W[i]= (W[i]/CONVERSIONE_UNITA)/TEMPO;
Salvatore94 0:3d72a547faad 38 Pos[i]=0;
Salvatore94 0:3d72a547faad 39 }
Salvatore94 0:3d72a547faad 40
Salvatore94 0:3d72a547faad 41 led=0;
Salvatore94 0:3d72a547faad 42 return (Inizializzato=1);
Salvatore94 0:3d72a547faad 43 }
Salvatore94 0:3d72a547faad 44
Salvatore94 0:3d72a547faad 45 //Leggo la velocità angolare in mdps
Salvatore94 0:3d72a547faad 46 void Leggi_Velocita(int32_t W[], int32_t W_ini[]){
Salvatore94 0:3d72a547faad 47 int32_t W_letta[3];
Salvatore94 0:3d72a547faad 48 gyro->Get_G_Axes(W_letta);
Salvatore94 0:3d72a547faad 49 for(int i=0;i<3;i++){
Salvatore94 0:3d72a547faad 50 if(W_letta[i]>=(W_ini[i]-ACCURATEZZA) && W_letta[i]<=(W_ini[i]+ACCURATEZZA))
Salvatore94 0:3d72a547faad 51 W[i]=0;
Salvatore94 0:3d72a547faad 52 else
Salvatore94 0:3d72a547faad 53 W[i]= W_letta[i]/CONVERSIONE_UNITA;
Salvatore94 0:3d72a547faad 54 }
Salvatore94 0:3d72a547faad 55 }
Salvatore94 0:3d72a547faad 56 //Ricavo le posizioni in gradi
Salvatore94 0:3d72a547faad 57 void Leggi_Posizioni(int Pos[], int32_t W_ini[]){
Salvatore94 0:3d72a547faad 58 int32_t W[3]={0,0,0}, temp[3]={0,0,0};
Salvatore94 0:3d72a547faad 59 for(int tempo=0; tempo<INTERVALLO_MISURA; tempo++){
Salvatore94 0:3d72a547faad 60 Leggi_Velocita(temp, W_ini);
Salvatore94 0:3d72a547faad 61 for(int i=0; i<3; i++){
Salvatore94 0:3d72a547faad 62 W[i]+=temp[i];
Salvatore94 0:3d72a547faad 63 }
Salvatore94 0:3d72a547faad 64 wait_ms(TS);
Salvatore94 0:3d72a547faad 65 }
Salvatore94 0:3d72a547faad 66
Salvatore94 0:3d72a547faad 67 for(int i=0; i<3; i++){
Salvatore94 0:3d72a547faad 68 Pos[i]= (Pos[i]+W[i]*TS); //TS è il tempo di campionamento
Salvatore94 0:3d72a547faad 69 /*if(Pos[i]>360*CONVERSIONE_UNITA)
Salvatore94 0:3d72a547faad 70 Pos[i]-=360*CONVERSIONE_UNITA;
Salvatore94 0:3d72a547faad 71 else if(Pos[i]<-360*CONVERSIONE_UNITA)
Salvatore94 0:3d72a547faad 72 Pos[i]+=360*CONVERSIONE_UNITA;*/
Salvatore94 0:3d72a547faad 73 }
Salvatore94 0:3d72a547faad 74 }
Salvatore94 0:3d72a547faad 75 int main() {
Salvatore94 0:3d72a547faad 76 //controllo l'id del giroscopio
Salvatore94 0:3d72a547faad 77 gyro->ReadID(&GyroID);
Salvatore94 0:3d72a547faad 78 printf("Il giroscopio ha id: %d \r \n",GyroID);
Salvatore94 0:3d72a547faad 79
Salvatore94 0:3d72a547faad 80 while(!Inizializza_Giroscopio(W_ini, Posizione));
Salvatore94 0:3d72a547faad 81
Salvatore94 0:3d72a547faad 82 while(1) {
Salvatore94 0:3d72a547faad 83 printf("-------\r\n");
Salvatore94 0:3d72a547faad 84 Leggi_Velocita(W_ris, W_ini);
Salvatore94 0:3d72a547faad 85 Leggi_Posizioni(Posizione, W_ini);
Salvatore94 0:3d72a547faad 86 printf("X: %d dps\tY: %d dps\tZ: %d dps\r\n",W_ris[0],W_ris[1],W_ris[2]);
Salvatore94 0:3d72a547faad 87 printf("Pos_X: %d\tPos_Y: %d\tPos_Z: %d\r\n",Posizione[0]/CONVERSIONE_UNITA,Posizione[1]/CONVERSIONE_UNITA,Posizione[2]/CONVERSIONE_UNITA);
Salvatore94 0:3d72a547faad 88 // wait(1);
Salvatore94 0:3d72a547faad 89 }
Salvatore94 0:3d72a547faad 90 }