File content as of revision 0:b435eadf76b4:

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

//definition des sorties PWM
FastPWM PWM1Droit(D11);                 //IN1 mot Droit
FastPWM PWM2Droit(D12);                 //IN2 mot Droit
FastPWM PWM1Gauche(D9);          //IN1 mot Gauche
FastPWM PWM2Gauche(D10);         //IN2 mot Gauche

// on déclare un "flag" en globale
char flag;


//definition des connexion serie (RS232 en 115200 bauds)
Serial pc(USBTX, USBRX,115200);

DigitalOut EnableGauche(D6);
DigitalOut EnableDroite(D7);


//création des tableaux qui contiennent les valeurs sur x,y et z de l'accéléromètre et du gyroscope
float accelero[3]= {0};
float gyro[3] = {0};

// création de la variable "angleMesureFiltre" qui contient l'inclinaison filtré du gyropode
float angleMesureFiltre;

//création de variable
double angleErreur,alpha;

double erreurUmot, Umot=0;

//création de la constante d'échantillonage "Te" en [s] et Tems en [ms]
float Te = 20e-3,Tems=Te*1000;

//création de la constante de temps "TauFC" de nos filtre
float TauFC=0.25;

//caractéristique de la boucle d'asservissement
double kp=2, kd=0, k2=0, offset=0;

//création du coef "AFC" et "BFC" que l'on applique respectivement sur l'entrée et la sortie
float AFC = 1/(1+TauFC/Te),
      BFC = TauFC/Te/(1+TauFC/Te);

//création de la vaiable qui stocke la valeur de l'angle non filtré grâce et fitré à l'accéléromètre
float angle_nf=0;
float angle_f=0;

//Cr&ation de la variable qui stocke la valeurs de l'angle filtré en grâce au gyroscope (angle filtré haute fréquence)
float angle_f_HF=0;

//definition de la connnection en "I2C" entre le nucleo et le MPU6050
MPU6050 module(I2C_SDA, I2C_SCL);


//Création de la fonction permettant l'acquisition des valeurs de l'accéléro et du gyro
void acquisition()
{

    //récupération des données
    module.getAccelero(accelero);
    module.getGyro(gyro);

    //calcul de l'angle non filtré avec l'accéléro
    angle_nf = atan2(accelero[1],accelero[0]); //arctan(y/x)

    //filtrage de l'angle non filtré avec l'accéléro
    angle_f = AFC * angle_nf + BFC * angle_f;

    //calcul et filtrage de l'angle mesuré avec le gyroscope
    angle_f_HF = AFC *TauFC*(-gyro[2]) + BFC * angle_f_HF;

    //calcul finale de l'angle d'inclinaison du gyropode
    angleMesureFiltre =angle_f+(angle_f_HF);
    
    
    
    //asservissment angle
    angleErreur= 0 - (angleMesureFiltre + offset);
    alpha = kp*angleErreur + kd*gyro[2] +k2*erreurUmot;    //gyro[2] = vitesse angulaire y

    if(alpha>0.5) alpha=0.5;
    if(alpha<-0.5) alpha=-0.5;

    PWM1Droit.write(0.5-alpha);
    PWM2Droit.write(0.5+alpha);       //motDroit

    PWM1Gauche.write(0.5-alpha);      //motGauche
    PWM2Gauche.write(0.5+alpha);

    //calcul fltrage vitesse Umot(attention fréquence de coupure à modifier)
    Umot = AFC * alpha + BFC * Umot;
    
    //asservssment en translation
    erreurUmot = 0 - Umot;
    
    //on leve le drapeau
    flag = 1;
}

//declaration d'un objet RtosTimmer
RtosTimer timer(mbed::callback(acquisition),osTimerPeriodic);


//Création de la fonction qui permet la réception de commande
void reception(char ch)
{
    static  int i=0;
    static char chaine[10];
    char commande[3];
    char valeur[6];

    //Scrutation des caractères "Carriage Return" et "Line Feed"
    if ((ch==13) or (ch==10)) {

        //on vide la chaine de caractère
        chaine[i]='\0';

        //on sépare la chaine de caractère en une variable commande et valeur
        strcpy(commande, strtok(chaine, " "));
        strcpy(valeur,  strtok(NULL, " "));

        //pc.printf("commande *%s*\r\n",commande);
        //pc.printf("valeur   *%s*\r\n",valeur);

        //Si la commande "TC" est reconnu alors on change la valeur de la constante de temps "TauFC" et on met à jour la valeur des coef AFC et BFC
        if (strcmp(commande,"TC")==0) {
            TauFC= atof(valeur);
            AFC = 1/(1+TauFC/Te),
            BFC = TauFC/Te/(1+TauFC/Te);
            flag = 1;
        }

        if (strcmp(commande,"of")==0) {
            offset = atof(valeur);
        }
        if (strcmp(commande,"kd")==0) {
            kd = atof(valeur);
        }
        if (strcmp(commande,"kp")==0) {
            kp = atof(valeur);
        } 
        if (strcmp(commande,"k2")==0) {
            k2 = atof(valeur);
        }else {
            //la commande n'est pas reconnu
            pc.printf("commande inconnue \r\n");
        }
        i = 0;


    } else {

        //on incrémente la chaine de caractère si "Carriage Return" ou "Line Feed" n'est pas détecté
        chaine[i]=ch;
        i++;
    }
}

int main()
{

    

    pc.printf("Le Gyro est allume\n\r");

    //vérification de la communication entre le MPU6050 et le nucleo
    while (!module.testConnection()) {
        pc.printf("not connected to mpu\n\r");
        wait(1);
    }

    pc.printf("connected to MPU\n\r");

    //Initialisation PWM
    PWM1Droit.period_us(50);
    PWM2Droit.period_us(50);
    PWM1Gauche.period_us(50);
    PWM2Gauche.period_us(50);

    /*Initialisation rapport cyclique
    PWM1Droit.write(0.3);
    PWM2Droit.write(0.7);       //motDroit

    PWM1Gauche.write(0.3);      //motGauche
    PWM2Gauche.write(0.7); */

    //Mise e marche des moteurs
    EnableGauche = 1;
    EnableDroite = 1;

    //Initialisation de la période d'échantillonnage "Tems"[s] (soit 20ms)
    timer.start((int)Tems);
    pc.printf("TimerStart %d \n\r",(int)Tems);

    //changement de l'echelle du module
    module.setGyroRange(MPU6050_GYRO_RANGE_2000);
    module.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);

    while(1) {
        if (pc.readable()!=0) {
            reception(pc.getc());
        }

        if (flag==1) {

            //pc.printf("%3.2f %3.2f %3.2f %3.2f\n\r",accelero[0],accelero[1],accelero[2],TauFC);
            //pc.printf("%3.2f %3.2f \n\r",angle_f,angle_nf);
            pc.printf("%3.2f %3.2f %3.2f\n\r",angleMesureFiltre,alpha,offset);  //gyro[2] = vitesse angulaire y
            //pc.printf("%3.2f %3.2f %3.2f\n\r",gyro[0],gyro[1],gyro[2]);

            flag = 0;
        }

    }
}