NXC : Leopard Adulte

Leopard_Adulte.c

// On reprend le prgm de Léopard. Objectif : on reste à dix centimètres environs de la cible, qui avance et recule.

#define Touch IN_1
#define Devant IN_4
#define Motors OUT_BC
#define PAS 5
#define PAS_RECUL 5

int vitesse = 50;

void setup() {
     SetSensorTouch(Touch);
     SetSensorLowspeed(Devant);
}

void recolle() {
     // Cherche à s'approcher de la cible à +- 10cm
     while(SensorUS(Devant) > 10 && !Sensor(Touch))
     {
        OnFwd(Motors, 100);
     }
     OnFwd(Motors, vitesse);
}


void follow() {
     // Conserve la distance
     
     if(Sensor(Touch))
     {
         Off(Motors);
         OnRev(Motors, vitesse);
     }
     else
     {

     if(SensorUS(Devant) <= 20 && SensorUS(Devant) > 13) {
        vitesse = vitesse -PAS;
     } else if(SensorUS(Devant) > 20)
     {
        vitesse = vitesse +PAS;
     }
     if(vitesse < 0) vitesse = 0;
     if(vitesse > 100) vitesse = 100;
     if(SensorUS(Devant) < 5)
     {
         vitesse = vitesse -PAS_RECUL;
     } else if(SensorUS(Devant) <= 13 && SensorUS(Devant) > 5) {
         vitesse = vitesse +PAS_RECUL;
     }
     
     if (SensorUS(Devant) > 15) {
         OnFwd(Motors, vitesse);
     } else if(SensorUS(Devant) < 11) {
         OnRev(Motors, vitesse);

     }
     else
     {
         Off(Motors);
     }
     
     }
}


task main (){
     setup();
     recolle();
     while(1){follow();}
}