NXC : Leopard Adulte

Leopard_Adulte.c

// On reprend le prgm de Léopard. Objectif : on reste à dix centimètres environ 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);     // Le capteur tactile est un arrêt d'urgence...
     SetSensorLowspeed(Devant);
}

void recolle() {
     // Cherche à s'approcher de la cible à +- 10cm
     while(SensorUS(Devant) > 10 && !Sensor(Touch))
     {
        OnFwd(Motors, 100);
     }
     OnFwd(Motors, vitesse); // cette action sera stoppée plus tard dans main
}


void follow() {
     // Conserve la distance
     
     if(Sensor(Touch))
     {
         // On recule immédiatement si on a touché la cible
         Off(Motors);
         OnRev(Motors, vitesse);
     }
     else
     {
          // Ajustement de la vitesse en fonction de la distance
          if(SensorUS(Devant) <= 20 && SensorUS(Devant) > 13) {
             vitesse = vitesse -PAS;
          } else if(SensorUS(Devant) > 20)
          {
             vitesse = vitesse +PAS;
          }
          // Vérifications de sécurité...
          if(vitesse < 0) vitesse = 0;
          if(vitesse > 100) vitesse = 100;

          // Ajustement quand on est proche de la cible (<=13cm)
          if(SensorUS(Devant) < 5)
          {
              vitesse = vitesse -PAS_RECUL;
          } else if(SensorUS(Devant) <= 13 && SensorUS(Devant) > 5) {
              vitesse = vitesse +PAS_RECUL;
          }
     
          // Suivi et régulation de la distance
          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();}
}