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?
User | Revision | Line number | New 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 | } |