Revision 3:a15c86292e5e, committed 2018-03-09

Comitter:

SandrineO

Date:

Fri Mar 09 08:43:36 2018 +0000

Parent:

2:41e642ed448d

Commit message:

Filtre compl?mentaire

Changed in this revision

diff -r 41e642ed448d -r a15c86292e5e Gyro_Accelerometre.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Gyro_Accelerometre.cpp	Fri Mar 09 08:43:36 2018 +0000
@@ -0,0 +1,95 @@
+#include "mbed.h"
+#include "MPU6050.h"
+#include "rtos.h"
+#include "FastPWM.h"
+
+DigitalOut myled(LED1);
+
+//****************Init Hardware*****************
+Serial pc(USBTX, USBRX,115200); // Initialisation de pc tx, rx -> hyperterminal
+Serial Bluetooth(PA_9, PA_10); // Initialisation Bluetooth (D1 et D0)
+MPU6050 mpu6050(D4, D5); // Initialisation accéléromètre/ liaison I2C (SDA,SCL)
+//Initialisation des 4 PWM
+FastPWM motG1 (D9);
+FastPWM motG2 (D10);
+FastPWM motD1 (D11);
+FastPWM motD2 (D12);
+
+//****************Constante********************
+int Te=5; // période d'échantillonnage en ms
+
+
+bool test_accelero;// tester la connextion accéléro
+
+//******Init variables acquisition accelero-gyro**************
+float accelero[3]; // init tableau xyz accéléro
+float gyro[3]; // init tableau xyz gyro
+
+//****************Init constantes acquisition teta *****************
+float TauFC=1.0/15; // tau du filtre complémentaire
+float AFC= 1.0/(1+1000*TauFC/Te);  // coefficient du filtre complémentaire *1000-> TauFC en s
+float BFC=(1000.0*TauFC/Te)/(1+1000*TauFC/Te); // coefficient du filtre complémentaire
+
+//************Init variables acquisition teta*********************
+float teta_accelero; // téta acceleromètre en entrée
+float teta_final; // valeur du téta final
+float teta_final_p; // valeur du téta final précédant
+
+
+int flag_affichage=0; // drapeau pour l'affichage des valeurs de l'accéléromètre en dehors de la fonction gyro_thread
+
+//*******************fonction timer***************
+void gyro_thread() 
+{
+    //***************Lecture des données de l'inclinaison de l'accelerometre************
+    mpu6050.getAccelero(accelero); //donne l'acceleration en m/s2
+    mpu6050.getGyro(gyro);
+
+    //*************Calcul du téta de l'accéléromètre************
+    teta_accelero=atan2(accelero[1],accelero[2]);
+    teta_final=AFC*(teta_accelero+gyro[0]*TauFC)+BFC*teta_final_p;
+    teta_final_p=teta_final;
+
+
+//*************************Variation des moteurs en fonction de l'angle téta final************************
+    motG1.period_us(50);
+    motG1.write(0.5+0.4/1.5*teta_final); //moteur gauche marche avant
+    motG2.period_us(50);
+    motG2.write(0.5-0.4/1.5*teta_final); //moteur gauche marche arrière
+    motD1.period_us(50);
+    motD1.write(0.5+0.4/1.5*teta_final); //moteur droit marche avant
+    motD2.period_us(50);
+    motD2.write(0.5-0.4/1.5*teta_final); //moteur droit marche arrière
+
+    flag_affichage=1;// activation du drapeau
+
+}
+
+// ***************Declaration d'un objet RtosTimer (temps réel) ******************
+RtosTimer timer(mbed::callback(gyro_thread),osTimerPeriodic);  //  mbed::callback(gyro_thread)-> excècute la fonction gyro_thread     osTimerPeriodic -> de manière répétitive
+
+int main()
+{
+    //***************Test connection accelero******************
+    test_accelero = mpu6050.testConnection();
+    if (test_accelero ==1) {
+        pc.printf("test accelero ok\r\n");
+    } else {
+        pc.printf("test accelero non ok\r\n");
+        while(1);// arrête le programme
+    }
+
+    timer.start(Te); // en ms   objet RtosTimer timer executé sur les secondes
+
+    while(1) {
+
+        if(flag_affichage==1) {
+            //    pc.printf("Xaccelero = %lf ; Yaccelero = %lf ; Zaccelero = %lf \r\n",accelero[0],accelero[1],accelero[2]);
+            pc.printf("%lf %lf \r\n",teta_accelero,teta_final);
+            flag_affichage=0;
+        }// fin if flag_affichage
+
+    }//fin while(1)
+}//fin main
+
+

diff -r 41e642ed448d -r a15c86292e5e Gyro_Accelerometre_XYZ.cpp
--- a/Gyro_Accelerometre_XYZ.cpp	Fri Mar 09 08:31:26 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,95 +0,0 @@
-#include "mbed.h"
-#include "MPU6050.h"
-#include "rtos.h"
-#include "FastPWM.h"
-
-DigitalOut myled(LED1);
-
-//****************Init Hardware*****************
-Serial pc(USBTX, USBRX,115200); // Initialisation de pc tx, rx -> hyperterminal
-Serial Bluetooth(PA_9, PA_10); // Initialisation Bluetooth (D1 et D0)
-MPU6050 mpu6050(D4, D5); // Initialisation accéléromètre/ liaison I2C (SDA,SCL)
-//Initialisation des 4 PWM
-FastPWM motG1 (D9);
-FastPWM motG2 (D10);
-FastPWM motD1 (D11);
-FastPWM motD2 (D12);
-
-//****************Constante********************
-int Te=5; // période d'échantillonnage en ms
-
-
-bool test_accelero;// tester la connextion accéléro
-
-//******Init variables acquisition accelero-gyro**************
-float accelero[3]; // init tableau xyz accéléro
-float gyro[3]; // init tableau xyz gyro
-
-//****************Init constantes acquisition teta *****************
-float TauFC=1.0/15; // tau du filtre complémentaire
-float AFC= 1.0/(1+1000*TauFC/Te);  // coefficient du filtre complémentaire *1000-> TauFC en s
-float BFC=(1000.0*TauFC/Te)/(1+1000*TauFC/Te); // coefficient du filtre complémentaire
-
-//************Init variables acquisition teta*********************
-float teta_accelero; // téta acceleromètre en entrée
-float teta_final; // valeur du téta final
-float teta_final_p; // valeur du téta final précédant
-
-
-int flag_affichage=0; // drapeau pour l'affichage des valeurs de l'accéléromètre en dehors de la fonction gyro_thread
-
-//*******************fonction timer***************
-void gyro_thread() 
-{
-    //***************Lecture des données de l'inclinaison de l'accelerometre************
-    mpu6050.getAccelero(accelero); //donne l'acceleration en m/s2
-    mpu6050.getGyro(gyro);
-
-    //*************Calcul du téta de l'accéléromètre************
-    teta_accelero=atan2(accelero[1],accelero[2]);
-    teta_final=AFC*(teta_accelero+gyro[0]*TauFC)+BFC*teta_final_p;
-    teta_final_p=teta_final;
-
-
-//*************************Variation des moteurs en fonction de l'angle téta final************************
-    motG1.period_us(50);
-    motG1.write(0.5+0.4/1.5*teta_final); //moteur gauche marche avant
-    motG2.period_us(50);
-    motG2.write(0.5-0.4/1.5*teta_final); //moteur gauche marche arrière
-    motD1.period_us(50);
-    motD1.write(0.5+0.4/1.5*teta_final); //moteur droit marche avant
-    motD2.period_us(50);
-    motD2.write(0.5-0.4/1.5*teta_final); //moteur droit marche arrière
-
-    flag_affichage=1;// activation du drapeau
-
-}
-
-// ***************Declaration d'un objet RtosTimer (temps réel) ******************
-RtosTimer timer(mbed::callback(gyro_thread),osTimerPeriodic);  //  mbed::callback(gyro_thread)-> excècute la fonction gyro_thread     osTimerPeriodic -> de manière répétitive
-
-int main()
-{
-    //***************Test connection accelero******************
-    test_accelero = mpu6050.testConnection();
-    if (test_accelero ==1) {
-        pc.printf("test accelero ok\r\n");
-    } else {
-        pc.printf("test accelero non ok\r\n");
-        while(1);// arrête le programme
-    }
-
-    timer.start(Te); // en ms   objet RtosTimer timer executé sur les secondes
-
-    while(1) {
-
-        if(flag_affichage==1) {
-            //    pc.printf("Xaccelero = %lf ; Yaccelero = %lf ; Zaccelero = %lf \r\n",accelero[0],accelero[1],accelero[2]);
-            pc.printf("%lf %lf \r\n",teta_accelero,teta_final);
-            flag_affichage=0;
-        }// fin if flag_affichage
-
-    }//fin while(1)
-}//fin main
-
-