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
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