Diff: main.cpp

Revision:

8:97589b322a4f

Parent:

7:4d02d2486949

Child:

9:187f56fa6db5

--- a/main.cpp	Fri Jun 12 12:49:53 2020 +0000
+++ b/main.cpp	Fri Jun 12 13:48:55 2020 +0000
@@ -1,39 +1,48 @@
 #include "mbed.h"
 #include "GYRO_DISCO_L476VG.h"
+#include "COMPASS_DISCO_L476VG.h"
+#define PI 3.14159265358979323846
 
+COMPASS_DISCO_L476VG compass;
 GYRO_DISCO_L476VG gyro;
+
 Serial pc(SERIAL_TX, SERIAL_RX);
 Ticker tick_mesure;
 
 volatile bool flag_mesure = 0;
 
-void mesure(void){flag_mesure = 1;}
+void mesure(void){flag_mesure = 1;}//   ISR mesure
 
 int main()
 {
     float GyroBuffer[3], offset, GyrY;
     offset = 0;
     wait(0.5);
-    for(int i=0; i<100; i++){
+    for(int i=0; i<100; i++){//     séquence de mise à 0
         gyro.GetXYZ(GyroBuffer);
         offset = offset + GyroBuffer[1];
         wait(0.01);
     }
-    offset = offset/100;
+    offset = offset/100;//-----------------fin mise à 0
     
     tick_mesure.attach(&mesure, 0.01);
     pc.baud(115200);
-    float angle = 0;
-    unsigned int count = 0;
+    
+    unsigned int count = 0;   
+    float Gyr_angle = 0;
+    float Acc_angle;
+    int16_t AccBuffer[3];
   
     while(1) {  
-        if(flag_mesure){
+        if(flag_mesure){//  mesure Gyro
             gyro.GetXYZ(GyroBuffer);
             GyrY = GyroBuffer[1]-offset;
-            angle = angle + GyrY/100;
+            Gyr_angle = Gyr_angle + GyrY/100;
+            Acc_angle = atan2((float)AccBuffer[0], (float)AccBuffer[2])*180.0/PI;
             
             if(count > 9){
-                pc.printf("$%f %f;", offset/1000, angle/1000);
+                compass.AccGetXYZ(AccBuffer);       
+                pc.printf("$%f %f;", Acc_angle, Gyr_angle/1000);
                 count = 0;
             }
             else