Diff: Gyro_Accelerometre_XYZ.cpp

Revision:

1:aca3c4b9fcfe

Parent:

0:1096fcacf950

Child:

2:41e642ed448d

--- a/Gyro_Accelerometre_XYZ.cpp	Tue Feb 20 15:58:55 2018 +0000
+++ b/Gyro_Accelerometre_XYZ.cpp	Tue Mar 06 14:31:22 2018 +0000
@@ -1,21 +1,28 @@
 #include "mbed.h"
 #include "MPU6050.h"
 #include "rtos.h"
+#include "FastPWM.h"
 
 DigitalOut myled(LED1);
 
 //****************Init Hardware*****************
 Serial pc(USBTX, USBRX); // 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)
-Serial Bluetooth(PA_9, PA_10); // Initialisation Bluetooth (D1 et D0)
+FastPWM motG1 (D9); //Initialisation PWM
+FastPWM motG2 (D10);
+FastPWM motD1 (D11);
+FastPWM motD2 (D12);
 
 //****************Init constantes*****************
 bool test_accelero;// tester la connextion accéléro
-float accelerometre[3]; // init tableau xyz accéléro
+float accelero[3]; // init tableau xyz accéléro
+float gyro[3]; // init tableau xyz gyro
 int Te=1000; // période d'échantillonnage en ms
 int flag_affichage=0; // drapeau pour l'affichage des valeurs de l'accéléromètre en dehors de la fonction gyro_thread
 
-void gyro_thread()
+
+void gyro_thread() //fct timer
 {
     // **********Test connexion accelerometre********
     test_accelero = mpu6050.testConnection();
@@ -27,7 +34,18 @@
 
 
     //***************Lecture des données de l'inclinaison de l'accelerometre************
-    mpu6050.getAccelero(accelerometre); //donne l'acceleration en m/s2
+    mpu6050.getAccelero(accelero); //donne l'acceleration en m/s2
+    mpu6050.getGyro(gyro);
+    
+    motG1.period_us(50);
+    motG1.write(0.5+0.05*accelero[0]); //moteur gauche marche avant
+    motG2.period_us(50);
+    motG2.write(0.5-0.05*accelero[0]); //moteur gauche marche arrière
+    motD1.period_us(50);
+    motD1.write(0.5+0.05*accelero[0]); //moteur droit marche avant
+    motD2.period_us(50);
+    motD2.write(0.5-0.05*accelero[0]); //moteur droit marche arrière
+    
     flag_affichage=1;// activation du drapeau
 
 }
@@ -37,15 +55,17 @@
 
 int main()
 {
-
     timer.start(Te); // en ms   RtosTimer timer executé sur les secondes
 
     while(1) {
 
         if(flag_affichage==1) {
-            pc.printf("Xaccelero = %lf ; Yaccelero = %lf ; Zaccelero = %lf \r\n",accelerometre[0],accelerometre[1],accelerometre[2]);
+            pc.printf("Xaccelero = %lf ; Yaccelero = %lf ; Zaccelero = %lf \r\n",accelero[0],accelero[1],accelero[2]);
+            pc.printf("Xgyro = %lf ; Ygyro = %lf ; Zgyro = %lf \r\n",gyro[0],gyro[1],gyro[2]);
             flag_affichage=0;
         }// fin if flag_affichage
 
     }//fin while(1)
-}//fin main
\ No newline at end of file
+}//fin main
+
+