File content as of revision 0:0d257bbf5c05:

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

// déclaration des objets
Serial pc(USBTX, USBRX);
DigitalOut led(LED1);
MPU6050 module(I2C_SDA, I2C_SCL);

// valeur de Te
float tems = 10;

// varailble necessire à la mesure de la position angulaire du gyropode
float accelero[3]= {0};
float gyro[3] = {0};
float tauFC = 1;        // constante de tyemps en seconde du filtre passe bas du filtre complémentaire
float aFC, bFC;         // les coefficient du filter passe bas du filtre complémentaire
float angleATG ,angleNF, angleGyro ,omegarGyro, angleGyrop;



int nbInc=0;
char flagInterupt=0;

void reception(char ch)
{
    static  int i=0;                // inice du dernier caratère recu
    static  char chaine[10];        // chaine de stockage des caratères recus
    char commande[3];               // chaine contenant la commande
    char valeur[6];                 // chaine contenant la valeur

    if ((ch==13) or (ch==10)) {

        chaine[i]='\0';

        // séparation de la commande et de la valeur
        strcpy(commande, strtok(chaine, " "));
        strcpy(valeur,  strtok(NULL, " "));

        // affichage de commande et valeur
        pc.printf("Commande %s \n\r",commande);
        pc.printf("Valeur   %s \n\r",valeur);

        // reinitialisation de l indice de chaine
        i = 0;

    } else {
        chaine[i]=ch;
        i++;
    }
}

void interupt()
{
    nbInc++;
//lecture des données de l'accéléro et du gyro
    module.getAccelero(accelero);
    module.getGyro(gyro);  
//  
    angleATG = atan2(accelero[0],accelero[1]);
    omegarGyro = gyro[3];
    
    angleNF = angleATG + tauFC * omegarGyro;
    angleGyro = aFC*( angleNF + bFC * angleGyrop);
    
    
    
    
// memorisation des valeurs precedante pour les filtres recursifs
    angleGyrop = angleGyro ;
    
    
    flagInterupt=1;
}

RtosTimer timer(mbed::callback(interupt),osTimerPeriodic);

int main()
{
    pc.printf("Test unitaire mecatro \n\r");
    



// initialisation et test de connection du MPU6050
    while (module.testConnection()==0) {
        pc.printf("not connected to mpu\n\r");
        wait(1);
    }
    //changement du l'echelle du module
    module.setGyroRange(MPU6050_GYRO_RANGE_2000);
    module.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);

// calcul des coefficients des filtres
    aFC=(float)1/(1+tauFC/(tems/1000));
    bFC=tauFC/(tems/1000);


    
//   initialisation de la latache periodique du noyau multitache
    timer.start(tems);

    while(1) {
        if (pc.readable()!=0) {
            reception(pc.getc());
        }
        if (flagInterupt==1) {
            pc.printf("Ax = %3.2f, Ay = %3.2f, Az = %3.2f Wx = %3.2f, wy = %3.2f, wz = %3.2f \n\r",accelero[0],accelero[1],accelero[2],gyro[0],gyro[1],gyro[2]);
            flagInterupt=0;
        }
    }
}