Diff: main.cpp

Revision:

7:4d02d2486949

Parent:

6:df81f0a821d8

Child:

8:97589b322a4f

--- a/main.cpp	Fri Jun 12 08:15:41 2020 +0000
+++ b/main.cpp	Fri Jun 12 12:49:53 2020 +0000
@@ -3,19 +3,43 @@
 
 GYRO_DISCO_L476VG gyro;
 Serial pc(SERIAL_TX, SERIAL_RX);
+Ticker tick_mesure;
+
+volatile bool flag_mesure = 0;
+
+void mesure(void){flag_mesure = 1;}
 
 int main()
 {
+    float GyroBuffer[3], offset, GyrY;
+    offset = 0;
+    wait(0.5);
+    for(int i=0; i<100; i++){
+        gyro.GetXYZ(GyroBuffer);
+        offset = offset + GyroBuffer[1];
+        wait(0.01);
+    }
+    offset = offset/100;
+    
+    tick_mesure.attach(&mesure, 0.01);
     pc.baud(115200);
-    float GyroBuffer[3];
-  
-    printf("Gyroscope started\n");
+    float angle = 0;
+    unsigned int count = 0;
   
-    while(1) {
-        // Read Gyroscope values
-        gyro.GetXYZ(GyroBuffer);
-        // Display values      
-        pc.printf("$%f;", GyroBuffer[1]);
-        wait(0.2);
+    while(1) {  
+        if(flag_mesure){
+            gyro.GetXYZ(GyroBuffer);
+            GyrY = GyroBuffer[1]-offset;
+            angle = angle + GyrY/100;
+            
+            if(count > 9){
+                pc.printf("$%f %f;", offset/1000, angle/1000);
+                count = 0;
+            }
+            else
+                count++;
+            flag_mesure = 0;
+        }
+        //wait(0.1);
     }
 }