marie allan
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
Revision 0:6e5bca5b4c06, committed 2015-02-11
- Comitter:
- allanmarie
- Date:
- Wed Feb 11 19:45:44 2015 +0000
- Commit message:
- read teta and omega from MPU6050
Changed in this revision
--- /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
--- /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
--- /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