File content as of revision 6:a4e49784b533:

//Bibliothéque
#include "QEI.h"
#include "mbed.h"
#include "Gestion_Moteur.h"
#include "Servo.h"

DigitalOut led_red(LED_RED);
DigitalOut led_green(LED_GREEN);
//DigitalOut FLAG(PTA5);


AnalogOut FLAG(DAC0_OUT);
Timeout timeout;
PwmOut Pwm_moteur_1(PTD4);  // Moteur gauche
PwmOut Pwm_moteur_2(PTA1);  // Moteur droit
Ticker ticker_asserv;
Ticker ticker_servo;
//  Declaration des differents objets, variables necessaires au bon fonctionnement de la regulation des moteurs dediés à la propulsion
double Periode_echantillonage=0.0001;
// QEI moteur gauche (QEI pinA, QEI pinB, NC, pulses/tour)
QEI decodeur_1 (PTB0,PTB1, NC, 30);
// QEI moteur droit (QEI pinA, QEI pinB, NC, pulses/tour)
QEI decodeur_2 (PTB18,PTB19, NC, 30);


// Variables d'asserv moteur gauche
// Mesure brute QEI 1, Mesure brute QEI 1 précédente, Écart mesuré, Écart de consigne
int mesure1=0,mesure_precedente1=0,mesure_moteur_1;
extern int consigne_moteur_1=0; 
// Coeff proportionnel, Intégral, Valeur intégrale, valeur sortie de l'asserv, consigne PWM envoyée au moteur gauche
float Kp_moteur_1=2,Ki_moteur_1=2,I_x_moteur_1=0,sortie_asserv1,consigne_PWM1;
// Variables d'asserv moteur droit
int mesure2=0,mesure_precedente2=0,mesure_moteur_2;
extern int consigne_moteur_2=0; 
// Coeff proportionnel, Intégral, Valeur intégrale, valeur sortie de l'asserv, consigne PWM envoyée au moteur droit
float Kp_moteur_2=2,Ki_moteur_2=2,I_x_moteur_2=0,sortie_asserv2,consigne_PWM2;

// Pins de sortie PWM


double i=0.4;
/*
void move_servo()
{
    
    float mem;
    mem=FLAG.read();
    FLAG.write(0.5);

    if (i<0.7) i+=0.001;
    else i=0.4;
    servo = i; 

    FLAG.write(mem);
}
*/
void mesure_interruption(void)
{
    float mem;
    mem=FLAG.read();
    FLAG.write(1);
    // Calcul de l'écart pour le moteur gauche
    mesure1=decodeur_1.getPulses();
    if (mesure1 >= mesure_precedente1)
        mesure_moteur_1=mesure1-mesure_precedente1;
    else
        mesure_moteur_1=mesure1-mesure_precedente1+32767;
    mesure_precedente1=mesure1;
    
    // Calcul de l'écart pour le moteur droit
      mesure2=decodeur_2.getPulses();
    if (mesure2 >= mesure_precedente2)
        mesure_moteur_2=mesure2-mesure_precedente2;
    else
        mesure_moteur_2=mesure2-mesure_precedente2+32767;
    mesure_precedente2=mesure2;
        
        
    Bloc_propulsion();   
    FLAG.write(mem);
}
void init_led(void)
{
        // Ticker asserv périodique :
        //timeout.attach_us(&retard_servo,10);
        //ticker_servo.attach_us(&move_servo,0.05*1000000);
        ticker_asserv.attach_us(&mesure_interruption,Periode_echantillonage*1000000); 
        led_red=1;
        led_green=1;
        Pwm_moteur_1.period_us(100);
        Pwm_moteur_2.period_us(100);
        //
}
    

double Regulateur_PI_decodeur(double consigne,double mesure, double Kp, double Ki ,double Periode_echantillonage,double I_x )
{
    // Kp Ki sont des parametres Globals modifiable via la transmission série 
    // Periode_echantillonage est une constante à tous le système
    // I_x est une valeur interne du regulateur
    // la consigne est la vitesse calculé par le calculateur moteur
    // la mesure  est la vitesse mesuré par les qei
    
    //Consigne et Mesure doivent être dans les mêm unité : rad/s
    double ecart, P_x,commande;
    /////// Régulation PI ///////
    // Ecart entre la consigne et la mesure
    ecart = consigne - mesure;
    
    // Terme proportionnel
    P_x = Kp * ecart;
     
    // Calcul de la commande
    commande = P_x + I_x;
     
    // Terme intégral (sera utilisé lors du pas d'échantillonnage suivant)
    I_x = I_x + Ki * Periode_echantillonage * ecart;
    /////// Fin régulation PID ///////
    return commande;
    }

void Bloc_propulsion()
{
    //
    sortie_asserv1 = (float)Regulateur_PI_decodeur(consigne_moteur_1, mesure_moteur_1, Kp_moteur_1, Ki_moteur_1, Periode_echantillonage,I_x_moteur_1 );
    //
    sortie_asserv2 = (float)Regulateur_PI_decodeur(consigne_moteur_2, mesure_moteur_2, Kp_moteur_2, Ki_moteur_2, Periode_echantillonage,I_x_moteur_2 );
    
    //Conversion en PWM
    consigne_PWM1=sortie_asserv1/20;
    consigne_PWM2=sortie_asserv2/20; 
    
    // Saturation
        // Moteur gauche
    if (consigne_PWM1 > 1)
        consigne_PWM1=1;
    else if  (consigne_PWM1 <0)   
        consigne_PWM1=0;
        // Moteur droit
    if (consigne_PWM2 > 1)
        consigne_PWM2=1;
    else if  (consigne_PWM2 <0)   
        consigne_PWM2=0;   
        
    // Envoi PWM aux moteurs
        Pwm_moteur_1.write(consigne_PWM1);
        Pwm_moteur_2.write(consigne_PWM2);
    }