File content as of revision 2:354d20277612:

#include "mbed.h"
#include "SRF05.h"
#define PI 3.14159265358979323846264338327950288419716939937510582
#define periode 20
#define Temps 1.0 //temps sur le quel on mesure le nombre d'encodeur passé
#define rayon 0.021
#define crans 12
#define voulu 25.0
SRF05 srf(p18, p17);
//Déclaration des variables globales
DigitalOut myled1(LED1) ;
DigitalOut myled2(LED2) ;
DigitalOut myled3(LED3) ;
DigitalOut myled4(LED4) ;
DigitalIn Bp1(p30); //gauche
DigitalIn Bp2(p29);    //droit
AnalogIn capt1(p20);    //droit
AnalogIn capt2(p19);    //gauche
InterruptIn Encodeur_AG(p10);
InterruptIn Encodeur_AD(p9);
InterruptIn Encodeur_BG(p7);
InterruptIn Encodeur_BD(p6);
PwmOut mot1(p23);
PwmOut mot2(p22);
DigitalOut sens_droit(p11);
DigitalOut sens_gauche(p12);
AnalogIn Vbat(p15);
float dist;
int sens=1;
int cpt=0;
int vitesse1=30;
int vitesse2;
int mot;
int ticker;
float vitessemsAG;  //vitesse en m/s en fonction des encodeurs
float vitessemsAD;
float vitesseradsAG; // vitesse en ras/s en foction des encodeurs
float vitesseradsAD;
int cpt1=0,cpt2=0; // compteur pour la mesure de la vitessse
Ticker T1;                        // ticker pour la mesure de la vitesse
float Bat;
void sens_vitesse(int,int);   // gestin du sens et de la vitesse et du moteur 1 periode entre [-100;100] puis le moteur 1=droit 2=gauche
void gestion_cpt_AG(void);    //mesure de la vitesse sue les different encodeurs
void gestion_cpt_AD(void);    //"                                              "
void calculvitesse(void);
void position(float); // distance voulu
int main()      //Début du main
{
    Encodeur_AG.mode(PullNone);         // pas de resistance interne
    Encodeur_AD.mode(PullNone);
    Encodeur_AG.rise(&gestion_cpt_AG); // front montant
    Encodeur_AD.rise(&gestion_cpt_AD);
    Bp1.mode(PullUp);         //Déclaration des variables locales au main
    Bp2.mode(PullUp);
    mot1.period_us(100);    //periode des moteurs
    mot2.period_us(100);
    T1.attach(&calculvitesse, Temps);


    while(1) {  //Boucle infinie
        int BpG;
        /*myled1=1;
        myled2=0;
        myled3=0;
        myled4=0;
        wait(0.5);
        myled1=0;
        myled2=1;
        myled3=0;
        myled4=0;
        wait(0.5);
        myled1=0;
        myled2=0;
        myled3=1;
        myled4=0;
        wait(0.5);
        myled1=0;
        myled2=0;
        myled3=0;
        myled4=1;
        wait(0.5);*/
        BpG=Bp1.read();
        Bat=(Vbat.read()*100)/7.4; //valeur de la batterie
        /*printf("batt:%g \t",Bat);
        printf("Bg: %d \t Bd: %d \t",Bp1.read(),Bp2.read());
        printf("Cd: %.02f \t Cg:%.02f \t",capt1.read(),capt2.read());
        printf("EAg:%d \t EAd:%d \t",Encodeur_AG.read(),Encodeur_AD.read());
        printf("EBg:%d \t EBd:%d \n\r",Encodeur_BG.read(),Encodeur_BD.read());
        wait(0.5);*/
        if (ticker==1) {
            //printf("vitesse mot Gauche:%g\n\r",vitessemsAG);
            //printf("vitesse mot Droite:%g\n\r",vitessemsAD);
            ticker=0;
        }
        dist= srf.read();
    if(dist>voulu) {
        sens_vitesse(30,1);
        sens_vitesse(30,2);
    } else if(dist< voulu) {
        sens_vitesse(-30,1);
        sens_vitesse(-30,2);
    } else if((dist<23) && (dist>27)) {
        sens_vitesse(0,1);
        sens_vitesse(0,2);
    }
    }
    
    
}

void sens_vitesse(int Periode,int mot)
{
    int sens;
    if (Periode<=0) {       // determination du sens 1 si peride inf a 0
        sens=1;
        Periode=abs(Periode); // valeurs abs de la periode donc en positif
    } else {
        sens=0;
    }
    if((mot==1)&&(sens==1)) {
        sens_gauche.write(1);
        mot1.pulsewidth_us(100-Periode); // 100-periode car sinon vitesse est de la difference entre 100 et la periode

    } else if((mot==1)&& (sens==0)) {
        sens_gauche.write(0);
        mot1.pulsewidth_us(Periode);
    }

    if((mot==2)&&(sens==1)) {
        sens_droit.write(1);
        mot2.pulsewidth_us(100-Periode); // 100-periode car sinon vitesse est de la difference entre 100 et la periode
    } else if((mot==2)&& (sens==0)) {
        sens_droit.write(0);
        mot2.pulsewidth_us(Periode);
    }
}

void gestion_cpt_AG(void)
{
    cpt1++;   // incrementation de +1 a chaque passage
}
void gestion_cpt_AD(void)
{
    cpt2++;   // incrementation de +1 a chaque passage
}

void calculvitesse(void)
{
    vitessemsAG = (cpt1*((2*PI*rayon)/crans))/Temps; //(compteur * ((perimetre)/ nombre de crans))/Temps
    vitesseradsAG = vitessemsAG/rayon; // m/s div par rayon donc = rad/s
    vitessemsAD = (cpt2*((2*PI*rayon)/crans))/Temps; //(compteur * ((perimetre)/ nombre de crans))/Temps
    vitesseradsAD = vitessemsAD/rayon; // m/s div par rayon donc = rad/s
    ticker=1;
    cpt1=0;                     // remise du compteur a 0
    cpt2=0;                     // remise du compteur a 0
}

/*void position(float voulu)
{
    dist= srf.read();
    if(dist>voulu) {
        sens_vitesse(30,1);
        sens_vitesse(30,2);
    } else if(dist< voulu) {
        sens_vitesse(-30,1);
        sens_vitesse(-30,2);
    } else if((dist<23) && (dist>27)) {
        sens_vitesse(0,1);
        sens_vitesse(0,2);
    }
}*/