marie allan / Mbed 2 deprecated ticker_test_mpu6050

ticker test

Dependencies:   FastPWM MPU6050 mbed

Fork of ticker_test by sho hashimoto

main.cpp@4:a38992f8f176, 2015-02-18 (annotated)

Committer:
allanmarie
Date:
Wed Feb 18 17:07:42 2015 +0000
Revision:
4:a38992f8f176
Parent:
2:58798fc6dffc 
read MPU6050 with flipper

Who changed what in which revision?

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