ticker test
Dependencies: FastPWM MPU6050 mbed
Fork of ticker_test by
main.cpp
- Committer:
- allanmarie
- Date:
- 2015-02-17
- Revision:
- 1:38e70a9b9a0f
- Parent:
- 0:e26c1a395072
- Child:
- 2:58798fc6dffc
File content as of revision 1:38e70a9b9a0f:
#include "mbed.h" #include "MPU6050.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(I2C_SDA,I2C_SCL); 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.getAcceleroRawY(); accZ+= mpu.getAcceleroRawZ(); omegaM += mpu.getGyroRawY(); 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; 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"); pc.printf("il est possible qu'il faille rebooter le capteur...\r\n"); while(true); } 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 K=exp(static_cast<double>(-KP*TE)); pc.printf("K = %6.4f ",K); teta=K*tetaAv +(1-K)/KP*(KP*tetaM+((float)gOmega/7505.7)); //angle estimé //on passe en degre float tetaM_deg = tetaM * 180 / M_PI; //angle mesuré float teta_deg=teta* 180 / M_PI; //angle estimé pc.printf("gaccY = %6d gAccZ= %6d gyro=%6d angleM=%6.2f angle=%.2f\r\n",gaccY, gAccZ, gOmega,tetaM_deg,teta_deg); tetaAv=teta; led1=0; /* if(tetaM < 0) tetaM += 2*PI; if(tetaM > 2*PI) tetaM -= 2*PI; tetaM = tetaM * 180 / PI;*/ } } }