File content as of revision 2:41e56a06c580:

#include "mbed.h"
#include "QEI.h"
#include "L298.h"
#include "math.h"

Serial pc(SERIAL_TX,SERIAL_RX);

QEI encodeur_a (D2, D3, NC, 1200, QEI::X4_ENCODING);
QEI encodeur_b (D4, D5, NC, 1200, QEI::X4_ENCODING);


/*pour 8v*/
static float Pl = 0.01; //0.03
static float Il = 0.0001; //0.001
static float Dl = 0;

static float Pa = 0.01; //0.01
static float Ia = 0.0001; //0.0001
static float Da = 0;

#define LINEIQUE 0
#define ANGULAIRE 1

#define FREQ_CORRECTION 0.01
#define ERR_STATIQUE 100
#define LIMITE_TEMPS 1/FREQ_CORRECTION /*3 secondes*/

#define DISTANCE_ENTRE_ROUES 270 /*en mm*/
#define DIMENSION_ROUE_A 70 /*en mm*/
#define DIMENSION_ROUE_B 70 /*en mm*/
#define TIC_PAR_TOUR_A 1200
#define TIC_PAR_TOUR_B 1200

#define LIMITE_VITESSE 0.5

void pos(int commande_a, int commande_b);
void pos_a(int commande_a);
void pos_b(int commande_b);
void deplacement(int theta, int r);
float PID(float erreur,float type);
 
int main() {
    
    L298 pontH();

    float distance = 0;
    float orientation_deg = 90;
    
    float pulse_a;
    float pulse_b;
    
    float pid_a;
    float pid_b;
    
    float pid_dist;
    float pid_ori;
    
    float err_dist;
    float err_ori;
    
    float n_commande_a;
    float n_commande_b;
    
    float res_a = (float)DIMENSION_ROUE_A*3.14/(float)TIC_PAR_TOUR_A;
    float res_b = (float)DIMENSION_ROUE_B*3.14/(float)TIC_PAR_TOUR_B;
    
    float orientation_dist = orientation_deg * (float)DISTANCE_ENTRE_ROUES * 3.14 / 360;
    
    //moteur_init();
    
    while(1)
    {
        pulse_a = (float)encodeur_a.getPulses() * res_a;
        pulse_b = (float)encodeur_b.getPulses()* res_b;
        
        err_dist = distance - (pulse_a + pulse_b) /2;
        err_ori = orientation_dist - (pulse_a - pulse_b) /2;   
        
        printf("%f\t%f\t%f\t%f\n\r",err_dist,err_ori,pulse_a,pulse_b);
        
        pid_dist = PID(err_dist,LINEIQUE);
        pid_ori = PID(err_ori,ANGULAIRE);
        
        pid_a = - pid_dist + pid_ori;
        pid_b = - pid_dist - pid_ori;
        
        
        //printf("%f\n\r",pulse_a);
        
        n_commande_a = pid_a;
        n_commande_b = pid_b;
        
        
        if (n_commande_a>1)
            n_commande_a=LIMITE_VITESSE;
        else if (n_commande_a<-LIMITE_VITESSE)
            n_commande_a = -LIMITE_VITESSE;
        
        if (n_commande_b>LIMITE_VITESSE)
            n_commande_b=LIMITE_VITESSE;
        else if (n_commande_b<-LIMITE_VITESSE)
            n_commande_b = -LIMITE_VITESSE;
            
        
        //moteur_a(n_commande_a);
        //moteur_b(n_commande_b);
        wait(FREQ_CORRECTION);     
        
    }
    
    return 0;    
}




float PID(float erreur,float type)
{ 
    static float errPrev = 0;
    static float errSum = 0;
    static float errDif = 0;
 
    float P,I,D;
 
    errSum += erreur;                 
 
    errDif = erreur - errPrev;      
    errPrev = erreur;
 
    if (type){
        P = erreur * Pl;                  
        I = errSum * Il;                 
        D = errDif * Dl;                 
    } else {
        P = erreur * Pa;                  
        I = errSum * Ia;                 
        D = errDif * Da; 
    }
 
    return P + I + D;                //Le résultat est la somme des trois
                                     //composantes calculées précédemment
}




/*asservissement des deux roues individuellement*/
    /*pas très utile pour le déplacement mais intéressant*/
    /*de garder comme un exemple*/
    
    /*

void pos(int commande_a, int commande_b)
{

    
    
    int erreur_a = 0;
    int somme_erreur_a = 0;
    
    int derniere_erreur_a = 0;
    int derivee_a = 0;    
    
    float n_commande_a;
    
    int validation_position_a = 0;
    
    int erreur_b = 0;
    int somme_erreur_b = 0;
    
    int derniere_erreur_b = 0;
    int derivee_b = 0;    
    
    float n_commande_b;
    
    int validation_position_b = 0;
    
    do
    {
        
        erreur_a = encodeur_a.getPulses()-commande_a;
        erreur_b = encodeur_b.getPulses()-commande_b;
        
        somme_erreur_a = somme_erreur_a + erreur_a;
        somme_erreur_b = somme_erreur_b + erreur_b;
        
        derivee_a = erreur_a - derniere_erreur_a;
        derivee_b = erreur_b - derniere_erreur_b;
        
        n_commande_a = erreur_a * P + somme_erreur_a * I + derivee_a * D;
        n_commande_b = erreur_b * P + somme_erreur_b * I + derivee_b * D;
        
        derniere_erreur_a = erreur_a;
        derniere_erreur_b = erreur_b;
        
        if (n_commande_a>1)
            n_commande_a=1;
        else if (n_commande_a<-1)
            n_commande_a = -1;
            
        if (n_commande_b>1)
            n_commande_b=1;
        else if (n_commande_b<-1)
            n_commande_b = -1;
            
        moteur_a(-n_commande_a);
        moteur_b(-n_commande_b);
        
        if (abs(erreur_a)<ERR_STATIQUE)
            validation_position_a++;
        else
            validation_position_a = 0;
            
        if (abs(erreur_b)<ERR_STATIQUE)
            validation_position_b++;
        else
            validation_position_b = 0;
        
        wait(FREQ_CORRECTION);
                
    } while (validation_position_a<LIMITE_TEMPS || validation_position_b<LIMITE_TEMPS);
    moteur_a(0);
    moteur_b(0);
    
    encodeur_a.reset();
    encodeur_b.reset();
}





*/