Diff: main.cpp

Revision:

3:2cd58c219b89

Child:

5:0d84191fde21

diff -r 8c562a8fed36 -r 2cd58c219b89 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Feb 03 13:41:29 2020 +0000
@@ -0,0 +1,79 @@
+#include "mbed.h"
+#include "MPU6050.h"
+#include "rtos.h"
+#include <math.h>
+
+//state
+#define FinAcquisition  1
+#define PretPourAcquisition  0
+
+Serial pc(USBTX, USBRX);     // USB initialization
+MPU6050 accelero(D4, D5);    // MPU6050 library initialization
+
+// Variables globales
+float AngleTan,AngleFiltre=0,AngleNonFiltre=0,A,B;
+float Te_ms = 5, Te, Tau = 0.1;   // Valeurs periode d'enchantillonage et tau
+
+char Flag=PretPourAcquisition;
+
+void calculAngle(void)
+{
+    int gx, gz, OmegaY;
+    
+    // acq de gx et gz et calcul de AngleTan
+    gx = accelero.getAcceleroRawX();
+    gz = accelero.getAcceleroRawZ();
+    AngleTan = atan2((double)gx,(double)gz);
+    
+    // acq de OmegaY
+    OmegaY = accelero.getGyroRawY()*Tau/1000;
+
+    // calcul AngleNonFiltre
+    AngleNonFiltre = OmegaY + AngleTan;
+
+    // filtre passe bas numérique
+    AngleFiltre = A*AngleNonFiltre+B*AngleFiltre;
+    
+    Flag = FinAcquisition;
+}
+
+RtosTimer timer(mbed::callback(calculAngle),osTimerPeriodic); // definition of rtos timer 
+
+
+int main (void)
+{
+    float AngleDeg;
+    // calcul des coefficients du filtre de l'angle
+    Te = Te_ms/1000;
+    A = Te/(Te+Tau);
+    B = Tau/(Te+Tau);
+    
+    // Usb init
+    pc.baud(115200);
+    pc.printf("\r\n Debut prog\r\n");
+
+    // verification of connection
+    if (accelero.testConnection()) {
+        pc.printf("connected to MPU 6050\r\n");
+    } else {
+        pc.printf("No device  \r\n");
+        while(1); // infinite pause
+    }
+    wait(0.2);
+    
+    // Scale init
+    accelero.setGyroRange(MPU6050_GYRO_RANGE_2000);
+    accelero.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
+    
+    timer.start(Te_ms);
+    
+    while(1) {
+        //display of angles calculated
+        if (Flag == FinAcquisition) {
+            pc.printf("%f %f \r\n",AngleFiltre, AngleTan);
+            Flag = PretPourAcquisition ;
+        }
+       
+    }
+}
+/*