File content as of revision 5:0d84191fde21:

#include "mbed.h"
#include "MPU6050.h"
#include "rtos.h"
#include <math.h>

//state
#define FinAcquisition  1
#define PretPourAcquisition  0

Serial pc(USBTX, USBRX);     // USB initialization
Serial HC06(D1,D0);          // TX - RX du microcontroler
MPU6050 accelero(D4, D5);    // MPU6050 library initialization

PwmOut Mot_G(D12);
PwmOut Mot_D(D10);

DigitalOut SensMotG(D11);
DigitalOut SensMotD(D9);

struct Trame {
    float kp;
    float kd;
    float val3;
    float val4;
    float val5;
    float val6;
    float val7;
};

Trame trame;


// filtre compléméntaire
float Te_ms = 5, Te, Tau = 0.1,A,B;  
int gx, gz, OmegaY;
float AngleTan,AngleFiltre=0,AngleNonFiltre=0; 
char Flag=PretPourAcquisition;

//asservissement angulaire
float Kp=0.01,Kd=0.05;
float Alpha, Erreur;
float AngleOffset=0, AngleCons=0;

//asservissement angle consigne 
float TauA=0.1,KpA=0.1;


void rotationMoteur(float motG, float motD)
{
    Mot_D.period_us(40);
    Mot_G.period_us(40);
    
    if (motG < 0) {
        SensMotG = 1;
        motG = 1 + motG;
    } else SensMotG = 0;

    if (motD<0) {
        SensMotD = 1;
        motD = 1 + motD;
    } else SensMotD = 0;

    Mot_G.write(motG);
    Mot_D.write(motD);
}

void calculAngle(void)
{
    // acq de l'acc linéaire (G) et calcul de AngleTan
    gx = accelero.getAcceleroRawX();
    gz = accelero.getAcceleroRawZ();
    AngleTan = atan2((double)gx,(double)gz);

    // acq de vitesse angulaire
    OmegaY = accelero.getGyroRawY();

    // calcul AngleNonFiltre
    AngleNonFiltre = OmegaY*Tau/1000 + AngleTan;

    // filtre passe bas numérique
    AngleFiltre = A*AngleNonFiltre+B*AngleFiltre;

    Flag = FinAcquisition;
}

RtosTimer timer(mbed::callback(calculAngle),osTimerPeriodic); // definition of rtos timer

void asservissmentAngulaire(void)
{ 
    Erreur = AngleCons + AngleOffset - AngleFiltre; // calcul erreur
    Alpha = Kp*AngleFiltre + Kd*OmegaY;              // proportionel + derivé
    
    // saturation 
    if (Alpha < -0.35)  Alpha = -0.35;
    if (Alpha > 0.35)   Alpha = 0.35;
    
    rotationMoteur(Alpha,Alpha);
}


void receptionBluetooth(void)
{
    int i=0;
    char val[500];

    // stockage de la trame 
    val[0]= HC06.getc();
    while(val[i]!='\n') {
        i++;
        val[i]= HC06.getc();
    }

    sscanf(val,"%f:%f:%f:%f:%f:%f:%f",
                       &trame.kp,
                       &trame.kd,
                       &trame.val3,
                       &trame.val4,
                       &trame.val5,
                       &trame.val6,
                       &trame.val7);
                       
    // mise à jour des variables
    Kp = trame.kp  ;
    Kd = trame.kd ;
}

int main (void)
{
    // calcul des coefficients du filtre de l'angle
    Te = Te_ms/1000;
    A = Te/(Te+Tau);
    B = Tau/(Te+Tau);

    // Usb init
    pc.baud(115200);
    pc.printf("\r\n Debut prog\r\n");

    // bluetooth init
    HC06.baud(9600);

    // verification of connection
    if (accelero.testConnection()) {
        pc.printf("connected to MPU 6050\r\n");
    } else {
        pc.printf("No device  \r\n");
        while(1); // infinite pause
    }
    wait(0.2);

    // Scale init
    accelero.setGyroRange(MPU6050_GYRO_RANGE_2000);
    accelero.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);

    timer.start(Te_ms);

    while(1) {

        if (HC06.readable()) {
            receptionBluetooth();
            /* Affichage valeur recues
            pc.printf("\r\nkp %f   kd %f   val3 %f   val4 %f   val5 %f   val6 %f   val7 %f",
                      trame.kp,
                      trame.kd,
                      trame.val3,
                      trame.val4,
                      trame.val5,
                      trame.val6,
                      trame.val7);*/
        }

        // Nouvelle valeur d'angle
        if (Flag == FinAcquisition) {
            asservissmentAngulaire();
            // pc.printf("%f %f \r\n",AngleFiltre, AngleTan);
            Flag = PretPourAcquisition ;
        }

        rotationMoteur(0,0);

    }
}