File content as of revision 1:47b9204516a0:

#include "mbed.h"
#include "HCSR04.h"

#define ECHO_FRONT  D8
#define TRIG_FRONT  D9

#define ECHO_BACK  D10
#define TRIG_BACK  D11



Serial pc(USBTX, USBRX);    //UART
HCSR04 sensorFront(TRIG_FRONT, ECHO_FRONT);
HCSR04 sensorBack(TRIG_BACK, ECHO_BACK);

PwmOut PWM_R(PB_3);
PwmOut PWM_L(PB_10);

DigitalOut B_R(PB_5);
DigitalOut F_R(PA_10);

DigitalOut B_L(PA_8);
DigitalOut F_L(PB_4);

/*
* rotation infinitesimale vers la gauche, de puissance 0<P<1
*/
void rotationG(float P){
    PWM_R = P;
    B_R = 0;
    F_R = 1;
    PWM_L = P;
    B_L = 0;
    F_L = 0;
    wait(0.05);
}

/*
* rotation infinitesimale vers la droite, de puissance 0<P<1
*/
void rotationD(float P){
    PWM_R = P;
    B_R = 0;
    F_R = 0;
    PWM_L = P;
    B_L = 0;
    F_L = 1;
    wait(0.05);
}


void virageGauche(int angle){}

//analyse des valeurs enregistrées --> angle de mur --> procédure de virage d'un angle donnée+ avancer 2cm --> mesure de distance --> correction angle --> avance

//Avancer tout droit 
void avancer(float P){
    PWM_R = P;
    B_R = 0;
    F_R = 1;
    PWM_L = P;
    B_L = 0;
    F_L = 1;
    wait(0.05);
}


int main() {
    
    int distFront = 22;
    int distBack = 22;
    int difDist;
    
    B_R = 0;
    F_R = 0;
    B_L = 0;
    F_L = 0;
    
    PWM_R.period_ms(20);
    PWM_L.period_ms(20);

    while(1) {

        distFront = sensorFront.distance(1);
        distBack = sensorBack.distance(1);
        
        difDist = distBack - distFront;
        
        if(difDist<-10){
            rotationG(0.8); //rotation à gauche
        }else if(difDist<0){
            rotationG(0.8); //rotation à gauche
            avancer(0.7);
        }else if(difDist>0){
            rotationD(0.8);    //rotation à droite
            avancer(0.7);
        }else if(difDist>10){
            rotationG(0.8);
        }

    }
}
    
/* CODE POUR 1 CAPTEUR ------- 
int main() {

    int dist[3] = {0,0,0};
    
    int distance = 0;
    
    B_R = 0;
    F_R = 0;
    B_L = 0;
    F_L = 0;
    
    PWM_R.period_ms(20);
    PWM_L.period_ms(20);

    while(1) {

        dist[2] = dist[1];
        dist[1] = dist[0];
        dist[0] = sensor.distance(1);
        
        if(dist[0] > 40) dist[0] = 40;
        //distance = (dist[0] + dist[1] + dist[2])/3.0;
        //pc.printf("%d\n",dist);
        distance = dist[0];

        if( distance < 20 ){
            rotationD(0.5);    //rotation à droite
        }else if(distance > 30 ){
            rotationG(0.7); //rotation à gauche
            wait(0.05);
            avancer(0.6);
        }else if(distance > 25 ){
            rotationG(0.5); //rotation à gauche
        }else{
                avancer(0.7);    //rotation à gauche
        }
        wait(0.05); //latence
    }
}
*/