// Variables pour capteurs
byte dir, s1,s2,s3,s4,s5;
byte dist;
bool result;
unsigned int rawData[], normData[];
  int scaledData[];
  int cval;
  int SV;
//Variables d'état du programme et du jeu
bool balle = 0, capture = 0, arret = 0, demitour = 0, couleur, retour = 0;

task affichage()
{
  while(true)
  {
    SV = ReadSensorColorEx(S1, cval, rawData, normData, scaledData); 
    TextOut(1, LCD_LINE1, "cval =");
    TextOut(1, LCD_LINE2, "      ");
    NumOut(1, LCD_LINE2, cval);
    
    TextOut(1, LCD_LINE3, "Couleur =");
    TextOut(1, LCD_LINE4, "      ");
    NumOut(1, LCD_LINE4, couleur);
  }
}



// Cette fonction permet au robot de se garer
sub Garage()
{  
  int i;
    
  // Le robot est positioné face au but adverse. Son garage est donc derrière lui

  //quart de tour droite : Attention ceci n'est valable que si le camp du robot est celui qui a le garage à droite du but.
  OnFwd(OUT_A, 50);
  OnFwd(OUT_B, -50);
  demitour = 1;
  Wait(1450);
  demitour = 0;   
  Off(OUT_AB);


  // Avancer jusqu'à la ligne rouge
  do
  {
    SV = ReadSensorColorEx(S1, cval, rawData, normData, scaledData);
    OnFwd(OUT_AB, 50);
  } while (cval != INPUT_REDCOLOR)
   
   do // Dépasser la ligne rouge
   {
    SV = ReadSensorColorEx(S1, cval, rawData, normData, scaledData);
    OnFwd(OUT_AB, 50);
   } while (cval == INPUT_REDCOLOR)
   
   do // Se tourner vers la droite
   {
    SV = ReadSensorColorEx(S1, cval, rawData, normData, scaledData);
    OnFwd(OUT_A, 70); 
    OnFwd(OUT_B, -70);
   } while (cval != INPUT_REDCOLOR)
      
        
    while((cval != INPUT_BLACKCOLOR)) // Manoeuvre pour suivre la ligne rouge jusqu'à la ligne noire
    {
      SV = ReadSensorColorEx(IN_1, cval, rawData, normData, scaledData);
      
      /* if ( (cval == INPUT_BLACKCOLOR) || (cval == INPUT_BLACKCOLOR) ) // Condition de sortie
        break;*/
      
      if (cval == INPUT_REDCOLOR) // Initialisation de la valeur de couleur à chaque itération, déterminant que faire ensuite
        couleur = 1;
      else
        couleur = 0;
    
      if (couleur == 1)  // Avancer tant qu'on est sur la ligne
      OnFwd(OUT_AB,60); 
 
      if (couleur == 0) // Une fois sorti de la ligne, algorithme de repositionnement
      {
        while ((couleur == 0) && (cval != INPUT_BLACKCOLOR))               
        {      
         
          for(i = 1; i <= 20; i++) // Balayage à droite
          {
            OnFwd(OUT_A, 70); 
            OnFwd(OUT_B, -70);
            TextOut(1, LCD_LINE8, "      ");
            TextOut(1, LCD_LINE8, "Droite");
            Wait(10);
            Off(OUT_AB);
       
            SV = ReadSensorColorEx(IN_1, cval, rawData, normData, scaledData);
            if (cval == INPUT_REDCOLOR) // Une fois retourné sur la ligne, on recommence la boucle
            {
              couleur = 1;                  
              break;
            }
          }
          
          for(i = 1; i <= 20; i++) // Balayage à gauche
          {
            OnFwd(OUT_A, -70);
            OnFwd(OUT_B, 70);
            TextOut(1, LCD_LINE8, "      ");
            TextOut(1, LCD_LINE8, "Gauche");
            Wait(10);
            Off(OUT_AB);
            
            SV = ReadSensorColorEx(IN_1, cval, rawData, normData, scaledData);
            if (cval == INPUT_REDCOLOR) // Une fois retourné sur la ligne, on recommence la boucle
            {                   
              couleur = 1;                 
              break;
            }
          }
      }
    }
     SV = ReadSensorColorEx(IN_1, cval, rawData, normData, scaledData);
  }     
  
  
  // Nous sommes en face de la ligne noire. La suivre jusqu'au bout
  do
  {
    OnFwd(OUT_AB, 50);
    SV = ReadSensorColorEx(S1, cval, rawData, normData, scaledData);
  } while (cval == INPUT_BLACKCOLOR)

  // Faire ensuite un demi-tour
  OnFwd(OUT_A, 50); 
  OnFwd(OUT_B, -50);
  demitour = 1;
  Wait(2450);
  demitour = 0;   
  Off(OUT_AB);
  }
 
 
task main()
{
SetSensorColorFull(IN_1);

StartTask(affichage);
Wait(1000);
Garage();



}