File content as of revision 80:cd4960dfa47e:

#include "RoboClaw/RoboClaw.h"
#include "mbed.h"

#define ADR 0x80
#define accel_angle 12000
#define vitesse_angle 10000
#define deccel_angle 12000

#define accel_dista 12000
#define vitesse_dista 12000
#define deccel_dista 12000

void Sharps();
BusOut drapeau (PC_8, PC_6, PC_5);

RoboClaw roboclaw(ADR, 460800, PA_11, PA_12);
AnalogIn Rcapt1(PA_4);
int RvalRcapt1 = 0;
AnalogIn Rcapt2(PB_0);
int RvalRcapt2 = 0;
AnalogIn Rcapt3(PC_1);
int RvalRcapt3 = 0;

int Ravance = 1;
#define R_SEUIL_SHARP 0.3

void GotoDist(float timer) {
    Timer t;
    t.reset();
    roboclaw.SpeedAccelM1M2(accel_dista, vitesse_dista, vitesse_dista);
    t.start();
    
    while (t.read() < timer) {
        if (Ravance != 1) {
            roboclaw.ForwardM1(0);
            roboclaw.ForwardM2(0);
            t.stop();
            while (Ravance != 1);
            t.start();
        }
    }
    
    roboclaw.ForwardM1(0);
    roboclaw.ForwardM2(0);
    t.stop();
    t.reset();
}

void GotoThet (float timer) {
    Timer t;
    t.reset();
    t.start();
    roboclaw.SpeedAccelM1M2(accel_dista, vitesse_dista, -vitesse_dista);
    
    while (t.read() < timer) {
        if (Ravance != 1) {
            roboclaw.ForwardM1(0);
            roboclaw.ForwardM2(0);
            t.stop();
            while (Ravance != 1);
            t.start();
        }
    }
    
    roboclaw.ForwardM1(0);
    roboclaw.ForwardM2(0);
    
    t.stop();
    t.reset();
}
/* Debut du programme */
int main(void)
{
    Ticker ticker;
    ticker.attach(&Sharps, 0.01);
    
    GotoDist(7);
    GotoThet(7);
    
    while(1);
}

void Sharps() {
    if (Rcapt1.read() > R_SEUIL_SHARP) RvalRcapt1++;
    else RvalRcapt1--;
    RvalRcapt1 = RvalRcapt1 > 10 ? 10 : RvalRcapt1;
    RvalRcapt1 = RvalRcapt1 < 0 ? 0 : RvalRcapt1;
    
    if (Rcapt2.read() > R_SEUIL_SHARP) RvalRcapt2++;
    else RvalRcapt2--;
    RvalRcapt2 = RvalRcapt2 > 10 ? 10 : RvalRcapt2;
    RvalRcapt2 = RvalRcapt2 < 0 ? 0 : RvalRcapt2;
    
    if (Rcapt3.read() > R_SEUIL_SHARP) RvalRcapt3++;
    else RvalRcapt3--;
    RvalRcapt3 = RvalRcapt3 > 10 ? 10 : RvalRcapt3;
    RvalRcapt3 = RvalRcapt3 < 0 ? 0 : RvalRcapt3;
    
    if (RvalRcapt1 >= 5 || RvalRcapt2 >= 5 || RvalRcapt3 >= 5)
        Ravance = 0;
    else 
        Ravance = 1;
        
    if (Ravance == 0)
        drapeau = 1;
    else 
        drapeau = 2;
}