File content as of revision 2:7de884ffc9d9:

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

int dato=0;

// déclaration des objets
Serial pc(USBTX, USBRX,115200);
Serial bluetooth(PA_9, PA_10,38400); //PINS TO CONECT. PA_9 WITH RX PIN HC-06
DigitalOut led(LED1);
MPU6050 module(I2C_SDA, I2C_SCL);

// valeur de Te
float tems = 10;

// variable asserv position
float erreurPosition, erreurPosition_p, consignePosition = 0;
float KPP = 0.001;

// varailble necessire à la mesure de la position angulaire du gyropode
float accelero[3]= {0};
float gyro[3] = {0};
float tauFC = 0.2;                 // 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,angleGyroConsigne;
float consigneAngle = -0.001;
float CommandeVitesse = 0;

// varailble necessire à l'asserv angulaire et vitesse
float offsetConsigneAngle = 0;
float KPV = 0.026;
float KDA = 0.08;
float KPA =2.3;
float KDV = 0.01;
float erreurVitesse, offsetConsigneAngle_p;

// valeur du filtre passe bas
float ec_f, ec_fp;
float Te = 0.0002;
float ec,ecCorrige;                       // grandeur de commande
float ecVir;

// Codeurs
Codeurs codeurs;
int g, d, vitesseCodeur1, g_p, vitesseCodeur2, d_p;
float  moyVitesse_p, moyVitesse;

//sorties moteur
FastPWM M1_1(D9);
FastPWM M1_2(D10);
FastPWM M2_1(D11);
FastPWM M2_2(D12);

int nbInc=0;
char flagInterupt=0;


//reception

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, " "));

        
        if (strcmp( commande, "KPV" ) == 0) {
            KPV=atof(valeur);
        }
        if (strcmp( commande, "CA" ) == 0) {
            consigneAngle=atof(valeur);
        }
        if (strcmp( commande, "KDA" ) == 0) {
            KDA=atof(valeur);
        }
        if (strcmp( commande, "KPA" ) == 0) {
            KPA=atof(valeur);
        }
        if (strcmp( commande, "CV" ) == 0) {
            CommandeVitesse=atof(valeur);
        }
        if (strcmp( commande, "KDV" ) == 0) {
            KDV=atof(valeur);
        }

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

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

void interupt()
{
    codeurs.read(g, d);

    nbInc++;




    /**********************************************************************************************/
    /*                     Estimation position angulaire                                          */
    /**********************************************************************************************/

    // Lecture des données de l'accéléro et du gyro
    module.getAccelero(accelero);
    module.getGyro(gyro);

    // calcul pos angulaire du gyropode à partir de l'arc tangeante des accelerations
    angleATG = atan2(accelero[1],accelero[0]);


    // Calcul de la sortie du filtre complementaire
    omegarGyro = -gyro[2];
    angleNF = angleATG + tauFC * omegarGyro;
    angleGyro =  aFC*( angleNF + bFC * angleGyrop);

    // Memorisation des valeurs precedante pour les filtres recursifs
    angleGyrop = angleGyro;

    
    /**********************************************************************************************/
    /*                     Asserv postion angulaire gyropode                                      */
    /**********************************************************************************************/
    

    // Definir la consigne de l'asservissement de la pos angulaire
    angleGyroConsigne = consigneAngle+offsetConsigneAngle;              // consigneAngle est la grandeur de commande issue de l'asservissement de l image de la vitesse

 
    // Calcul de la grandeur de commande de l'asservissement des pos angulaire
    ec= KPA*(angleGyroConsigne-angleGyro)-KDA*omegarGyro;

    

    /**********************************************************************************************/
    /*                     Asserv Position                                                        */
    /**********************************************************************************************/
    
    erreurPosition = consignePosition - (g-d);
    CommandeVitesse = KPP * (erreurPosition-erreurPosition_p);
    erreurPosition_p = erreurPosition;
    
    if (CommandeVitesse>3) CommandeVitesse = 3;
    else if (CommandeVitesse < -3) CommandeVitesse = -3;
    
    
    
    /**********************************************************************************************/
    /*                     Asserv Vitesse                                                         */
    /**********************************************************************************************/


    // Correcteur proportionel KPV
    erreurVitesse = CommandeVitesse-moyVitesse;

    // Asservissement de vitesse
    offsetConsigneAngle = (KPV*erreurVitesse)+(KDV*(moyVitesse - moyVitesse_p));
    moyVitesse_p = moyVitesse;

    // Calcul vitesse codeur
    vitesseCodeur1 = -g_p+g;
    g_p = g;
    vitesseCodeur2 = d_p-d;
    d_p = d;
    moyVitesse = (vitesseCodeur1+vitesseCodeur2)/2;
    moyVitesse = moyVitesse/10;


    /**********************************************************************************************/
    /*                     Commande Moteurs                                                       */
    /**********************************************************************************************/


    // Compenssation frottement sec
    if (ec<0)ecCorrige=ec-0.15;
    else if (ec>0)ecCorrige=ec+0.15;
    else ecCorrige = 0;

    // Saturation
    if (ecCorrige<-0.5)ecCorrige=-0.5;
    if (ecCorrige>0.5)ecCorrige=0.5;

    // Sorties PWM Moteurs
    M1_1.write(0.5+(ecCorrige+ecVir));
    M1_2.write(0.5-(ecCorrige+ecVir));
    M2_1.write(0.5-(ecCorrige-ecVir));
    M2_2.write(0.5+(ecCorrige-ecVir));

    flagInterupt=1;
}

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

int main()
{

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

    M1_1.period_us(50);
    M1_2.period_us(50);
    M2_1.period_us(50);
    M2_2.period_us(50);
    M1_1.write(0.5);
    M1_2.write(0.5);
    M2_1.write(0.5);
    M2_2.write(0.5);

    // Test connection codeurs
    pc.printf("Test des codeurs\r\n");
    while (!codeurs.test()) {
        pc.printf("Codeurs non connectes\r\n");
        wait(1);
    }

    pc.printf("Codeurs ok\r\n");
    codeurs.reset();


    // 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 (flagInterupt==1) {
            pc.printf("%4.3f %4.3f %4.3f %4.3f \n\r",angleGyro,moyVitesse,offsetConsigneAngle,ec);
            flagInterupt=0;
        }
        
        if(bluetooth.readable()) {
            dato=bluetooth.getc();
            pc.putc(dato);
        }
        if(pc.readable()) {
            dato=pc.getc();
            bluetooth.putc(dato);
        }
        
        // Controle valeur de la commande via HC06
        if(dato == 'A') {
            CommandeVitesse = 3;
        }
        else if(dato == 'R') {
            CommandeVitesse = -3;
        }
        else if((dato == 'a') || (dato == 'r')) {
            CommandeVitesse = 0;
        }
        
        else if((dato == 'g') || (dato == 'd')) {
            ecVir = 0;
        }
        else if(dato == 'G') {
            ecVir = -0.1;
        }
        else if(dato == 'D') {
            ecVir = 0.1;
        }
        else if(dato == 'o') {
            KPV =0;
            KDA =0;
            KPA =0;
            KDV =0;
            consigneAngle =0;
        }
        else if(dato == 'O') {
            KPV = 0.026;
            KDA = 0.08;
            KPA =2.3;
            KDV = 0.01;
            consigneAngle = -0.001;
        }

    }
}