File content as of revision 2:41e642ed448d:

#include "mbed.h"
#include "MPU6050.h"
#include "rtos.h"
#include "FastPWM.h"

DigitalOut myled(LED1);

//****************Init Hardware*****************
Serial pc(USBTX, USBRX,115200); // Initialisation de pc tx, rx -> hyperterminal
Serial Bluetooth(PA_9, PA_10); // Initialisation Bluetooth (D1 et D0)
MPU6050 mpu6050(D4, D5); // Initialisation accéléromètre/ liaison I2C (SDA,SCL)
//Initialisation des 4 PWM
FastPWM motG1 (D9);
FastPWM motG2 (D10);
FastPWM motD1 (D11);
FastPWM motD2 (D12);

//****************Constante********************
int Te=5; // période d'échantillonnage en ms


bool test_accelero;// tester la connextion accéléro

//******Init variables acquisition accelero-gyro**************
float accelero[3]; // init tableau xyz accéléro
float gyro[3]; // init tableau xyz gyro

//****************Init constantes acquisition teta *****************
float TauFC=1.0/15; // tau du filtre complémentaire
float AFC= 1.0/(1+1000*TauFC/Te);  // coefficient du filtre complémentaire *1000-> TauFC en s
float BFC=(1000.0*TauFC/Te)/(1+1000*TauFC/Te); // coefficient du filtre complémentaire

//************Init variables acquisition teta*********************
float teta_accelero; // téta acceleromètre en entrée
float teta_final; // valeur du téta final
float teta_final_p; // valeur du téta final précédant


int flag_affichage=0; // drapeau pour l'affichage des valeurs de l'accéléromètre en dehors de la fonction gyro_thread

//*******************fonction timer***************
void gyro_thread() 
{
    //***************Lecture des données de l'inclinaison de l'accelerometre************
    mpu6050.getAccelero(accelero); //donne l'acceleration en m/s2
    mpu6050.getGyro(gyro);

    //*************Calcul du téta de l'accéléromètre************
    teta_accelero=atan2(accelero[1],accelero[2]);
    teta_final=AFC*(teta_accelero+gyro[0]*TauFC)+BFC*teta_final_p;
    teta_final_p=teta_final;


//*************************Variation des moteurs en fonction de l'angle téta final************************
    motG1.period_us(50);
    motG1.write(0.5+0.4/1.5*teta_final); //moteur gauche marche avant
    motG2.period_us(50);
    motG2.write(0.5-0.4/1.5*teta_final); //moteur gauche marche arrière
    motD1.period_us(50);
    motD1.write(0.5+0.4/1.5*teta_final); //moteur droit marche avant
    motD2.period_us(50);
    motD2.write(0.5-0.4/1.5*teta_final); //moteur droit marche arrière

    flag_affichage=1;// activation du drapeau

}

// ***************Declaration d'un objet RtosTimer (temps réel) ******************
RtosTimer timer(mbed::callback(gyro_thread),osTimerPeriodic);  //  mbed::callback(gyro_thread)-> excècute la fonction gyro_thread     osTimerPeriodic -> de manière répétitive

int main()
{
    //***************Test connection accelero******************
    test_accelero = mpu6050.testConnection();
    if (test_accelero ==1) {
        pc.printf("test accelero ok\r\n");
    } else {
        pc.printf("test accelero non ok\r\n");
        while(1);// arrête le programme
    }

    timer.start(Te); // en ms   objet RtosTimer timer executé sur les secondes

    while(1) {

        if(flag_affichage==1) {
            //    pc.printf("Xaccelero = %lf ; Yaccelero = %lf ; Zaccelero = %lf \r\n",accelero[0],accelero[1],accelero[2]);
            pc.printf("%lf %lf \r\n",teta_accelero,teta_final);
            flag_affichage=0;
        }// fin if flag_affichage

    }//fin while(1)
}//fin main