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
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?
User | Revision | Line number | New 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 | } |