File content as of revision 85:38cbf5fd22e1:

#include "entete.h"
#include "AX12/AX12.h"

DigitalIn CAMP(PA_15);
DigitalIn START(PB_7);
DigitalOut LEDR(PC_2);
DigitalOut LEDV(PC_3);
DigitalIn button(USER_BUTTON);
int SCouleur = VERT;

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;
DigitalIn start(PB_7);
AX12 umbrella(PA_9, PA_10, 1, 250000);

/* Debut du programme */
int main(void)
{
    Ticker ticker;
    ticker.attach(&Sharps, 0.01);
    umbrella.setMode(0);
    umbrella.setMaxTorque(200);
    umbrella.setGoal(0);
    depart();
    if (SCouleur == VERT) {
        GotoDist(5.5);
    }
    else if (SCouleur == VIOLET) {
        ;
    }
    else {
        TestDist3(2,2);
        TestThet3(1,1);
    }

    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;
}

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

void GotoThet (float timer, int signe) {
    Timer t;
    
    if (signe < 0) {
        roboclaw.SpeedAccelM1(accel_dista, vitesse_dista);
        roboclaw.SpeedAccelM2(-accel_dista, -vitesse_dista);
    }
    else {
        roboclaw.SpeedAccelM1(-accel_dista, -vitesse_dista);
        roboclaw.SpeedAccelM2(accel_dista, vitesse_dista);
    }
    
    t.reset();
    t.start();
    
    while (t.read() < timer) {
        if (Ravance != 1) {
            roboclaw.ForwardM1(0);
            roboclaw.ForwardM2(0);
            t.stop();
            while (Ravance != 1);
            t.start();
        }
        if (signe < 0) {
            roboclaw.SpeedAccelM1(accel_dista, vitesse_dista);
            roboclaw.SpeedAccelM2(-accel_dista, -vitesse_dista);
        }
        else {
            roboclaw.SpeedAccelM1(-accel_dista, -vitesse_dista);
            roboclaw.SpeedAccelM2(accel_dista, vitesse_dista);
        }
    }
    
    roboclaw.ForwardM1(0);
    roboclaw.ForwardM2(0);
    
    t.stop();
    t.reset();
    
    wait(waiting_time);
}