Hiruka Hana / Mbed 2 deprecated Gyro_Accelerometre2

Filtre complémentaire

Dependencies:   FastPWM MPU6050 Gyro_Accelerometre mbed

Gyro_Accelerometre_XYZ.cpp@2:41e642ed448d, 2018-03-09 (annotated)

Committer:
SandrineO
Date:
Fri Mar 09 08:31:26 2018 +0000
Revision:
2:41e642ed448d
Parent:
1:aca3c4b9fcfe 
Programme avec l'acquisition de acc?l?ro+calcul t?ta+filtre compl?mentaire;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
SandrineO 0:1096fcacf950 1 #include "mbed.h"
SandrineO 0:1096fcacf950 2 #include "MPU6050.h"
SandrineO 0:1096fcacf950 3 #include "rtos.h"
SandrineO 1:aca3c4b9fcfe 4 #include "FastPWM.h"
SandrineO 0:1096fcacf950 5
SandrineO 0:1096fcacf950 6 DigitalOut myled(LED1);
SandrineO 0:1096fcacf950 7
SandrineO 0:1096fcacf950 8 //****************Init Hardware*****************
SandrineO 2:41e642ed448d 9 Serial pc(USBTX, USBRX,115200); // Initialisation de pc tx, rx -> hyperterminal
SandrineO 1:aca3c4b9fcfe 10 Serial Bluetooth(PA_9, PA_10); // Initialisation Bluetooth (D1 et D0)
SandrineO 0:1096fcacf950 11 MPU6050 mpu6050(D4, D5); // Initialisation accéléromètre/ liaison I2C (SDA,SCL)
SandrineO 2:41e642ed448d 12 //Initialisation des 4 PWM
SandrineO 2:41e642ed448d 13 FastPWM motG1 (D9);
SandrineO 1:aca3c4b9fcfe 14 FastPWM motG2 (D10);
SandrineO 1:aca3c4b9fcfe 15 FastPWM motD1 (D11);
SandrineO 1:aca3c4b9fcfe 16 FastPWM motD2 (D12);
SandrineO 0:1096fcacf950 17
SandrineO 2:41e642ed448d 18 //****************Constante********************
SandrineO 2:41e642ed448d 19 int Te=5; // période d'échantillonnage en ms
SandrineO 0:1096fcacf950 20
SandrineO 1:aca3c4b9fcfe 21
SandrineO 2:41e642ed448d 22 bool test_accelero;// tester la connextion accéléro
SandrineO 2:41e642ed448d 23
SandrineO 2:41e642ed448d 24 //******Init variables acquisition accelero-gyro**************
SandrineO 2:41e642ed448d 25 float accelero[3]; // init tableau xyz accéléro
SandrineO 2:41e642ed448d 26 float gyro[3]; // init tableau xyz gyro
SandrineO 2:41e642ed448d 27
SandrineO 2:41e642ed448d 28 //****************Init constantes acquisition teta *****************
SandrineO 2:41e642ed448d 29 float TauFC=1.0/15; // tau du filtre complémentaire
SandrineO 2:41e642ed448d 30 float AFC= 1.0/(1+1000*TauFC/Te); // coefficient du filtre complémentaire *1000-> TauFC en s
SandrineO 2:41e642ed448d 31 float BFC=(1000.0*TauFC/Te)/(1+1000*TauFC/Te); // coefficient du filtre complémentaire
SandrineO 2:41e642ed448d 32
SandrineO 2:41e642ed448d 33 //************Init variables acquisition teta*********************
SandrineO 2:41e642ed448d 34 float teta_accelero; // téta acceleromètre en entrée
SandrineO 2:41e642ed448d 35 float teta_final; // valeur du téta final
SandrineO 2:41e642ed448d 36 float teta_final_p; // valeur du téta final précédant
SandrineO 0:1096fcacf950 37
SandrineO 0:1096fcacf950 38
SandrineO 2:41e642ed448d 39 int flag_affichage=0; // drapeau pour l'affichage des valeurs de l'accéléromètre en dehors de la fonction gyro_thread
SandrineO 2:41e642ed448d 40
SandrineO 2:41e642ed448d 41 //*******************fonction timer***************
SandrineO 2:41e642ed448d 42 void gyro_thread()
SandrineO 2:41e642ed448d 43 {
SandrineO 0:1096fcacf950 44 //***************Lecture des données de l'inclinaison de l'accelerometre************
SandrineO 1:aca3c4b9fcfe 45 mpu6050.getAccelero(accelero); //donne l'acceleration en m/s2
SandrineO 1:aca3c4b9fcfe 46 mpu6050.getGyro(gyro);
SandrineO 2:41e642ed448d 47
SandrineO 2:41e642ed448d 48 //*************Calcul du téta de l'accéléromètre************
SandrineO 2:41e642ed448d 49 teta_accelero=atan2(accelero[1],accelero[2]);
SandrineO 2:41e642ed448d 50 teta_final=AFC*(teta_accelero+gyro[0]*TauFC)+BFC*teta_final_p;
SandrineO 2:41e642ed448d 51 teta_final_p=teta_final;
SandrineO 2:41e642ed448d 52
SandrineO 2:41e642ed448d 53
SandrineO 2:41e642ed448d 54 //*************************Variation des moteurs en fonction de l'angle téta final************************
SandrineO 1:aca3c4b9fcfe 55 motG1.period_us(50);
SandrineO 2:41e642ed448d 56 motG1.write(0.5+0.4/1.5*teta_final); //moteur gauche marche avant
SandrineO 1:aca3c4b9fcfe 57 motG2.period_us(50);
SandrineO 2:41e642ed448d 58 motG2.write(0.5-0.4/1.5*teta_final); //moteur gauche marche arrière
SandrineO 1:aca3c4b9fcfe 59 motD1.period_us(50);
SandrineO 2:41e642ed448d 60 motD1.write(0.5+0.4/1.5*teta_final); //moteur droit marche avant
SandrineO 1:aca3c4b9fcfe 61 motD2.period_us(50);
SandrineO 2:41e642ed448d 62 motD2.write(0.5-0.4/1.5*teta_final); //moteur droit marche arrière
SandrineO 2:41e642ed448d 63
SandrineO 0:1096fcacf950 64 flag_affichage=1;// activation du drapeau
SandrineO 0:1096fcacf950 65
SandrineO 0:1096fcacf950 66 }
SandrineO 0:1096fcacf950 67
SandrineO 0:1096fcacf950 68 // ***************Declaration d'un objet RtosTimer (temps réel) ******************
SandrineO 0:1096fcacf950 69 RtosTimer timer(mbed::callback(gyro_thread),osTimerPeriodic); // mbed::callback(gyro_thread)-> excècute la fonction gyro_thread osTimerPeriodic -> de manière répétitive
SandrineO 0:1096fcacf950 70
SandrineO 0:1096fcacf950 71 int main()
SandrineO 0:1096fcacf950 72 {
SandrineO 2:41e642ed448d 73 //***************Test connection accelero******************
SandrineO 2:41e642ed448d 74 test_accelero = mpu6050.testConnection();
SandrineO 2:41e642ed448d 75 if (test_accelero ==1) {
SandrineO 2:41e642ed448d 76 pc.printf("test accelero ok\r\n");
SandrineO 2:41e642ed448d 77 } else {
SandrineO 2:41e642ed448d 78 pc.printf("test accelero non ok\r\n");
SandrineO 2:41e642ed448d 79 while(1);// arrête le programme
SandrineO 2:41e642ed448d 80 }
SandrineO 2:41e642ed448d 81
SandrineO 2:41e642ed448d 82 timer.start(Te); // en ms objet RtosTimer timer executé sur les secondes
SandrineO 0:1096fcacf950 83
SandrineO 0:1096fcacf950 84 while(1) {
SandrineO 0:1096fcacf950 85
SandrineO 0:1096fcacf950 86 if(flag_affichage==1) {
SandrineO 2:41e642ed448d 87 // pc.printf("Xaccelero = %lf ; Yaccelero = %lf ; Zaccelero = %lf \r\n",accelero[0],accelero[1],accelero[2]);
SandrineO 2:41e642ed448d 88 pc.printf("%lf %lf \r\n",teta_accelero,teta_final);
SandrineO 0:1096fcacf950 89 flag_affichage=0;
SandrineO 0:1096fcacf950 90 }// fin if flag_affichage
SandrineO 0:1096fcacf950 91
SandrineO 0:1096fcacf950 92 }//fin while(1)
SandrineO 1:aca3c4b9fcfe 93 }//fin main
SandrineO 1:aca3c4b9fcfe 94
SandrineO 1:aca3c4b9fcfe 95