lecture capteurs accéléromètre et gyromètre et traitement capteur pour récupérer l'angle estimé en fonction de ces 2 informations

Dependencies:   MPU6050 mbed

Files at this revision

API Documentation at this revision

Comitter:
allanmarie
Date:
Wed Feb 11 19:45:44 2015 +0000
Commit message:
read teta and omega from MPU6050

Changed in this revision

MPU6050.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r 6e5bca5b4c06 MPU6050.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050.lib	Wed Feb 11 19:45:44 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/Sissors/code/MPU6050/#5c63e20c50f3
diff -r 000000000000 -r 6e5bca5b4c06 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Feb 11 19:45:44 2015 +0000
@@ -0,0 +1,90 @@
+#include "mbed.h"
+#include "MPU6050.h"
+#include "I2Cdev.h"
+#include <math.h>
+
+#define NB_MEASURE 5
+#define KP 10
+#define T_MEASURE 2000
+#define TE T_MEASURE*NB_MEASURE*1.0e-6
+
+#ifndef M_PI
+#define M_PI           3.14159265358979323846
+#endif
+ 
+  
+
+Ticker T_measure;
+DigitalOut led1(D2);
+DigitalOut led2(D3);
+Serial pc(USBTX, USBRX);
+MPU6050 mpu;
+
+
+int gOmega,gaccY,gAccZ;
+bool gEndMeasure=false;
+
+ 
+void measure() {
+    static int cpt=0;
+    static int accY,accZ,omegaM; //retour capteurs
+    //mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+    //mesure angle et vitesse ang
+    led2=1;
+    if(cpt<NB_MEASURE){
+
+        accY+= mpu.getAccelerationY();
+        accZ+= mpu.getAccelerationZ();
+        omegaM += mpu.getRotationY();
+        cpt++;
+        led2=0;
+    }else
+    {
+        cpt=0;
+        gaccY =accY/NB_MEASURE;
+        gAccZ=accZ/NB_MEASURE; 
+        gOmega=omegaM/NB_MEASURE; 
+        accY=accZ=omegaM=0;
+        gEndMeasure=true;
+    }
+    led2=0;
+
+}
+ 
+int main() {
+    float tetaM,teta,tetaAv=0;
+    mpu.initialize();
+    pc.baud(115200);
+    pc.printf("MPU6050 testConnection \n\r");
+ 
+    bool mpu6050TestResult = mpu.testConnection();
+    if(mpu6050TestResult) {
+        pc.printf("\r\nMPU6050 test passed Te=%.4f\n\r",TE);
+    } else {
+        pc.printf("\r\nMPU6050 test failed \n\r");
+    }
+    led2 = 1;
+    led1 = 1;
+    T_measure.attach_us(&measure, T_MEASURE); // the address of the function to be attached (flip) and the interval (2 seconds)
+   // temporisation.attach_us(&tempo, 10000); // spin in a main loop. flipper will interrupt it to call flip
+    while(1) {
+        if(gEndMeasure){
+            led1=1;
+            gEndMeasure=false;
+            tetaM=atan2(static_cast<double>(gaccY),static_cast<double>(gAccZ)); //calcul de l'angle
+            float tetaM_deg = tetaM * 180 / M_PI;
+            float K=exp(static_cast<double>(-KP*TE));
+            pc.printf("K = %6.4f  ",K);
+            teta=K*tetaAv +(1-K)/KP*(KP*tetaM+gOmega); //angle estimé
+            pc.printf("gaccY = %6d  gAccZ= %6d   gyro=%6d  angleM=%6.2f  angle=%.2f\r\n",gaccY, gAccZ, gOmega,tetaM_deg,teta);
+            tetaAv=teta;
+            led1=0;
+
+           /* if(tetaM < 0) 
+                tetaM += 2*PI;
+            if(tetaM > 2*PI) 
+                tetaM -= 2*PI;
+            tetaM = tetaM * 180 / PI;*/
+        }
+    }
+}
\ No newline at end of file
diff -r 000000000000 -r 6e5bca5b4c06 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Feb 11 19:45:44 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/4fc01daae5a5
\ No newline at end of file