marie allan / Mbed 2 deprecated accelero_giro

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

main.cpp@0:6e5bca5b4c06, 2015-02-11 (annotated)

Committer:
allanmarie
Date:
Wed Feb 11 19:45:44 2015 +0000
Revision:
0:6e5bca5b4c06
read teta and omega from MPU6050

Who changed what in which revision?

UserRevisionLine numberNew contents of line
allanmarie 0:6e5bca5b4c06 1 #include "mbed.h"
allanmarie 0:6e5bca5b4c06 2 #include "MPU6050.h"
allanmarie 0:6e5bca5b4c06 3 #include "I2Cdev.h"
allanmarie 0:6e5bca5b4c06 4 #include <math.h>
allanmarie 0:6e5bca5b4c06 5
allanmarie 0:6e5bca5b4c06 6 #define NB_MEASURE 5
allanmarie 0:6e5bca5b4c06 7 #define KP 10
allanmarie 0:6e5bca5b4c06 8 #define T_MEASURE 2000
allanmarie 0:6e5bca5b4c06 9 #define TE T_MEASURE*NB_MEASURE*1.0e-6
allanmarie 0:6e5bca5b4c06 10
allanmarie 0:6e5bca5b4c06 11 #ifndef M_PI
allanmarie 0:6e5bca5b4c06 12 #define M_PI 3.14159265358979323846
allanmarie 0:6e5bca5b4c06 13 #endif
allanmarie 0:6e5bca5b4c06 14
allanmarie 0:6e5bca5b4c06 15
allanmarie 0:6e5bca5b4c06 16
allanmarie 0:6e5bca5b4c06 17 Ticker T_measure;
allanmarie 0:6e5bca5b4c06 18 DigitalOut led1(D2);
allanmarie 0:6e5bca5b4c06 19 DigitalOut led2(D3);
allanmarie 0:6e5bca5b4c06 20 Serial pc(USBTX, USBRX);
allanmarie 0:6e5bca5b4c06 21 MPU6050 mpu;
allanmarie 0:6e5bca5b4c06 22
allanmarie 0:6e5bca5b4c06 23
allanmarie 0:6e5bca5b4c06 24 int gOmega,gaccY,gAccZ;
allanmarie 0:6e5bca5b4c06 25 bool gEndMeasure=false;
allanmarie 0:6e5bca5b4c06 26
allanmarie 0:6e5bca5b4c06 27
allanmarie 0:6e5bca5b4c06 28 void measure() {
allanmarie 0:6e5bca5b4c06 29 static int cpt=0;
allanmarie 0:6e5bca5b4c06 30 static int accY,accZ,omegaM; //retour capteurs
allanmarie 0:6e5bca5b4c06 31 //mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
allanmarie 0:6e5bca5b4c06 32 //mesure angle et vitesse ang
allanmarie 0:6e5bca5b4c06 33 led2=1;
allanmarie 0:6e5bca5b4c06 34 if(cpt<NB_MEASURE){
allanmarie 0:6e5bca5b4c06 35
allanmarie 0:6e5bca5b4c06 36 accY+= mpu.getAccelerationY();
allanmarie 0:6e5bca5b4c06 37 accZ+= mpu.getAccelerationZ();
allanmarie 0:6e5bca5b4c06 38 omegaM += mpu.getRotationY();
allanmarie 0:6e5bca5b4c06 39 cpt++;
allanmarie 0:6e5bca5b4c06 40 led2=0;
allanmarie 0:6e5bca5b4c06 41 }else
allanmarie 0:6e5bca5b4c06 42 {
allanmarie 0:6e5bca5b4c06 43 cpt=0;
allanmarie 0:6e5bca5b4c06 44 gaccY =accY/NB_MEASURE;
allanmarie 0:6e5bca5b4c06 45 gAccZ=accZ/NB_MEASURE;
allanmarie 0:6e5bca5b4c06 46 gOmega=omegaM/NB_MEASURE;
allanmarie 0:6e5bca5b4c06 47 accY=accZ=omegaM=0;
allanmarie 0:6e5bca5b4c06 48 gEndMeasure=true;
allanmarie 0:6e5bca5b4c06 49 }
allanmarie 0:6e5bca5b4c06 50 led2=0;
allanmarie 0:6e5bca5b4c06 51
allanmarie 0:6e5bca5b4c06 52 }
allanmarie 0:6e5bca5b4c06 53
allanmarie 0:6e5bca5b4c06 54 int main() {
allanmarie 0:6e5bca5b4c06 55 float tetaM,teta,tetaAv=0;
allanmarie 0:6e5bca5b4c06 56 mpu.initialize();
allanmarie 0:6e5bca5b4c06 57 pc.baud(115200);
allanmarie 0:6e5bca5b4c06 58 pc.printf("MPU6050 testConnection \n\r");
allanmarie 0:6e5bca5b4c06 59
allanmarie 0:6e5bca5b4c06 60 bool mpu6050TestResult = mpu.testConnection();
allanmarie 0:6e5bca5b4c06 61 if(mpu6050TestResult) {
allanmarie 0:6e5bca5b4c06 62 pc.printf("\r\nMPU6050 test passed Te=%.4f\n\r",TE);
allanmarie 0:6e5bca5b4c06 63 } else {
allanmarie 0:6e5bca5b4c06 64 pc.printf("\r\nMPU6050 test failed \n\r");
allanmarie 0:6e5bca5b4c06 65 }
allanmarie 0:6e5bca5b4c06 66 led2 = 1;
allanmarie 0:6e5bca5b4c06 67 led1 = 1;
allanmarie 0:6e5bca5b4c06 68 T_measure.attach_us(&measure, T_MEASURE); // the address of the function to be attached (flip) and the interval (2 seconds)
allanmarie 0:6e5bca5b4c06 69 // temporisation.attach_us(&tempo, 10000); // spin in a main loop. flipper will interrupt it to call flip
allanmarie 0:6e5bca5b4c06 70 while(1) {
allanmarie 0:6e5bca5b4c06 71 if(gEndMeasure){
allanmarie 0:6e5bca5b4c06 72 led1=1;
allanmarie 0:6e5bca5b4c06 73 gEndMeasure=false;
allanmarie 0:6e5bca5b4c06 74 tetaM=atan2(static_cast<double>(gaccY),static_cast<double>(gAccZ)); //calcul de l'angle
allanmarie 0:6e5bca5b4c06 75 float tetaM_deg = tetaM * 180 / M_PI;
allanmarie 0:6e5bca5b4c06 76 float K=exp(static_cast<double>(-KP*TE));
allanmarie 0:6e5bca5b4c06 77 pc.printf("K = %6.4f ",K);
allanmarie 0:6e5bca5b4c06 78 teta=K*tetaAv +(1-K)/KP*(KP*tetaM+gOmega); //angle estimé
allanmarie 0:6e5bca5b4c06 79 pc.printf("gaccY = %6d gAccZ= %6d gyro=%6d angleM=%6.2f angle=%.2f\r\n",gaccY, gAccZ, gOmega,tetaM_deg,teta);
allanmarie 0:6e5bca5b4c06 80 tetaAv=teta;
allanmarie 0:6e5bca5b4c06 81 led1=0;
allanmarie 0:6e5bca5b4c06 82
allanmarie 0:6e5bca5b4c06 83 /* if(tetaM < 0)
allanmarie 0:6e5bca5b4c06 84 tetaM += 2*PI;
allanmarie 0:6e5bca5b4c06 85 if(tetaM > 2*PI)
allanmarie 0:6e5bca5b4c06 86 tetaM -= 2*PI;
allanmarie 0:6e5bca5b4c06 87 tetaM = tetaM * 180 / PI;*/
allanmarie 0:6e5bca5b4c06 88 }
allanmarie 0:6e5bca5b4c06 89 }
allanmarie 0:6e5bca5b4c06 90 }