NXC : Cacahuete

cacahuete.c

#define gauche IN_4
#define devant IN_2
#define droite IN_1
#define paslibre_g (SensorUS(gauche) < 15)
#define paslibre_d (SensorUS(droite) < 15)
#define motors OUT_BC
#define VITESSE_ROT 75

void setup(){
     SetSensorTouch(devant);
     SetSensorLowspeed(droite);
     SetSensorLowspeed(gauche);
}

void demi_tour() { RotateMotorEx(motors, VITESSE_ROT, 1080, -100, true, false);}
void quart_tour_d() { RotateMotorEx(motors, VITESSE_ROT, 540, 100, true, false);}
void quart_tour_g() { RotateMotorEx(motors, VITESSE_ROT, 540, -100, true, false);}

task main() {
     setup();
     bool bol = 1;
     while(bol) {
         OnFwd(motors,75);
         until(Sensor(devant) == 1);
         Off(motors);
         RotateMotor(motors, 75, -360);
         if (paslibre_d && !(paslibre_g)){    // d
            quart_tour_g();
         } else if (paslibre_g && !(paslibre_d)) {
            quart_tour_d();
         } else if (paslibre_d  && paslibre_g) {
            demi_tour();
         } else  {
            TextOut(42, LCD_LINE3, "Wtf ?!");
            bol = 0;
         }
     }
}