Code Fangio CR

fangio.c

/*
          !========================================================================!
          !             Programme de Mathieu GABORIT et Lucas BOURNEUF             !
          !                   pour le projet FANGIO (séquence 3)                   !
          !                        Licence : WTFPL                                 !
          !========================================================================!
//*/



//======================
//       DEFINES
//======================
#define Couleur IN_2
#define Touch IN_4
#define trop_a_droite (couleur_vue == INPUT_REDCOLOR)
#define trop_a_gauche (couleur_vue == INPUT_GREENCOLOR)
#define marque (couleur_vue == INPUT_BLUECOLOR)
#define motors OUT_BC
#define NB_MARQUE_MAX 18
#define TEMPS_EMERGENCY_JAUNE 50
#define VITESSE_DE_CROISIERE 65
#define YA_UNE_LIGNE (couleur_vue == INPUT_BLACKCOLOR || couleur_vue == INPUT_REDCOLOR || couleur_vue == INPUT_GREENCOLOR)

// contantes utilisées pour éclaircir le code
#define SUIVRE_LIGNE 0
#define CHERCHE_LIGNE 1
#define ESQUIVE_OBSTACLE 2


//======================
//  VARIABLES GLOBALES
//======================
int couleur_vue;
int derniere_couleur_vue = INPUT_BLACKCOLOR;
int compteur = 0; // compteur des marques
int Etat_Esprit = CHERCHE_LIGNE; // définit ce que fait le robot (utilisation des defines)



//======================
//      PROTOTYPES
//======================
void setup();           // Initialisation et lancement du programme
task Emergency_stop();  // Tache gérant la fin de programme selon trois conditions non exclusives : bord de circuit, 18 marques bleues ou bouton d'arrêt d'urgence




//======================
//   SHORT FONCTIONS
//======================
void quart_tour_d() { RotateMotorEx(motors, 100, 630, 100, true, true);}    // faire un quart de tour par la droite
void quart_tour_g() { RotateMotorEx(motors, 100, 630, -100, true, true);}  // faire un quart de tour par la gauche
void demi_tour() {                                                          // fait un demi tour par la droite
    quart_tour_g();
    quart_tour_g();
}




// ===========================
// Définitions des fonctions
// ===========================

void setup()  // Initialisation et lancement du programme
{
     SetSensorColorFull(Couleur);  // port couleur
     SetSensorTouch(Touch);        // port tactile
     StartTask(Emergency_stop);    // tache de gestion de fin de prgm  (A désactiver une fois qu'on a vérifié que ça marchait bien)
} // end setup()




task Emergency_stop() // gère les arrêts d'urgence en vérifiant en continu le bouton Touch, le capteur color et le compteur.
{
     bool nuitDesTemps = 0;
     while(!nuitDesTemps)    // jusqu'à la nuit la nuit des temps
     {
          // si le capteur couleur voit du jaune, s'il à compté 18 marques ou plus, ou si le bouton est pressé
          if((couleur_vue == INPUT_YELLOWCOLOR))
          {
               Wait(TEMPS_EMERGENCY_JAUNE);  // protection pour éviter de s'arrêter si le capteur détecte du jaune par accident
               if(Sensor(Couleur) == INPUT_YELLOWCOLOR)
               {
                      Off(motors);    // arrêt des moteurs
                      StopAllTasks(); // arrêt du programme
               }
          }
          else if(compteur >= NB_MARQUE_MAX || Sensor(Touch))
          {
                Off(motors);    // arrêt des moteurs
                StopAllTasks(); // arrêt du programme
          }
     }
}// end Emergency_stop()


task main() // main(), l'épine dorsale du prgm
{
     setup();   // intialisation
     int left_rotate;  // vars contenant l'angle de rotation
     int right_rotate; // ..
     bool boucle = true; // dans le cas où on fait des boucles, on a un boléen tout prêt.
     
     // boucle principale de programme. Etudie les différentes couleurs
     while(true)
     {
          couleur_vue = Sensor(Couleur);   // Pour éviter de tester en permanence le capteur couleur, on utilisera une variable actualisée à chaque tour de boucle.
          // switch maître sur la variable couleur_vue
          switch (couleur_vue)
          {
          case INPUT_REDCOLOR:        // ROUGE
                if(derniere_couleur_vue == INPUT_REDCOLOR) left_rotate =40;
                else if(derniere_couleur_vue == INPUT_WHITECOLOR)
                     left_rotate = 180;
                else left_rotate = 80;
                RotateMotorEx(motors, 80, left_rotate, -50, true, false);
                break;
                
          case INPUT_GREENCOLOR:      // VERT
               if(derniere_couleur_vue == INPUT_GREENCOLOR) right_rotate =40;
               else if(derniere_couleur_vue == INPUT_WHITECOLOR)
                    right_rotate = 180;
               else right_rotate = 80;
               RotateMotorEx(motors, 80, right_rotate, 50, true, false);
               break;
               
          case INPUT_BLUECOLOR:      // BLEU
                if (couleur_vue != derniere_couleur_vue && Etat_Esprit == SUIVRE_LIGNE)
                {
                   Wait(5);
                   if(Sensor(Couleur) == couleur_vue)
                   {
                          compteur++;
                          TextOut(25,LCD_LINE2, NumToStr(compteur));
                   }
                }
                OnFwd(motors,70);
                break;

          case INPUT_YELLOWCOLOR:    // JAUNE
               Wait(TEMPS_EMERGENCY_JAUNE);  // protection pour éviter de s'arrêter si le capteur détecte du jaune par accident
               if(Sensor(Couleur) == INPUT_YELLOWCOLOR)
                       Off(motors);
               break;

          case INPUT_WHITECOLOR:     // BLANC
               Etat_Esprit = CHERCHE_LIGNE;
               Wait(10);
               if(Sensor(Couleur) == INPUT_WHITECOLOR)
               {
                     // on avance jusqu'à trouver une ligne
                     OnFwd(motors, 100);
                     while(!YA_UNE_LIGNE)
                     {couleur_vue = Sensor(Couleur);}
                     RotateMotorEx(motors, 100, 140, 0, false, true); // parcourir quelques centimètres
                     while(couleur_vue != INPUT_BLACKCOLOR && couleur_vue != INPUT_BLUECOLOR) // si ya pas de ligne noire (ou bleue)
                     {
                         couleur_vue = Sensor(Couleur);
                         /*
                         if(couleur_vue == INPUT_GREENCOLOR)
                                demi_tour(); // on fait un demi tour pour dépasser la ligne sans passer par la ligne jaune
                         //*/
                         if(couleur_vue == INPUT_WHITECOLOR || couleur_vue == INPUT_REDCOLOR)
                                RotateMotorEx(motors, 75, 10, -100, true, false);
                     }
               }
               Etat_Esprit = SUIVRE_LIGNE; // on a trouvé une ligne !
               OnFwd(motors,VITESSE_DE_CROISIERE);
               break;


          case INPUT_BLACKCOLOR:    // NOIR
               OnFwd(motors,VITESSE_DE_CROISIERE);
               
               
          default:  OnFwd(motors,VITESSE_DE_CROISIERE);
          }
          
          
          derniere_couleur_vue = couleur_vue;
     }
} // end main()



// fin du prgm