labyrinthe

Bonsoir a toute et a tous:) 
je travail actuellement sur un projet avec un robot lego RCX 2.0
le robot doit sortir tout seul d'une labyrinthe tracer sur le sol avec tout simplement du scotche noir et des point en scotche blanc comme ceci


voila ce que jai fait jusqua present mais rien qui marche

JAI VREMENT BESOIN DE VOTRE AIDE SVP le plus vite possible
merci davance

#define THRESHOLD 48
#define speed 40
#define TEMPS  400
#define TURN (140*(40/speed))                                                 // Changement pas encore testé
int P=0;
int t = 0;

task main()
{
    SetSensor(SENSOR_1,SENSOR_LIGHT);


    OnFwd(OUT_AC);
    while(true){
   
   
   

          while (SENSOR_1 > THRESHOLD)                                     // QUESTIONS  Pourquoi il ne regarder mais ne déclanche pas quand il passe au dessus de la ligne noir
          {
          t=0;                                                                   // QUESTION  Détection du point blanc pas toujours facile
          OnRev(OUT_C); OnFwd(OUT_A);
          Wait(50);
          t =t+1;
          until(SENSOR_1 <= THRESHOLD && t<2  );                                     // Tester avec la fonction TIMES_UP
          t=0;
          OnFwd(OUT_C); OnRev(OUT_A);
          Wait(70);
          t =t+1;
          until(SENSOR_1 <= THRESHOLD && t<2  );

          }
          OnFwd(OUT_AC);
       






          {

                                   // c'est ici qu'il faut commencer l'algo de PLEDGE
          OnFwd(OUT_AC);
          Wait(1000*(40/speed)); // ATTENTION CHANGEMENT PAS ENCORE TESTE   // Attendre le temps qu'il faut pour que les roues arrivent sur la ligne perpendiculaire pour pouvoir bien pivoter, multiplier par (40/VITESSE) Permet de changer la VITESSE tout en respectant le temps de pose necessaire

                    if ( P >= 0 )
                    {
                    OnFwd(OUT_C); OnRev(OUT_A);          // Droite
                    Wait (TURN);
                    PlayTone(2500, 50);
                    Wait(1000);
                              if (SENSOR_1 <= THRESHOLD)
                              {
                              P += -1;
                              OnFwd(OUT_AC);
                              Wait(100);
                              PlayTone(2700, 50);
                              }
                              else
                              {
                              OnRev(OUT_C ); OnFwd(OUT_A);
                              Wait (TURN);                                              // Trouver le temps pour 90°
                                        if (SENSOR_1 <= THRESHOLD)                                 // On va tout droit
                                        {
                                        OnFwd(OUT_AC);
                                        Wait(100);
                                        }
                                        else
                                        {
                                        OnRev(OUT_C); OnFwd(OUT_A);
                                        Wait (TURN);                              // On va à gauche
                                                  if (SENSOR_1 <= THRESHOLD)
                                                  {
                                                  P += 1;
                                                  OnFwd(OUT_AC);
                                                  Wait(100);
                                                  }
                                                  else                                            // On fait demi tour
                                                  {
                                                  OnRev(OUT_C); OnFwd(OUT_A);
                                                  Wait (TURN);
                                                  until (SENSOR_1 <= THRESHOLD);
                                                  OnFwd(OUT_AC);
                                                  Wait(100);
                                                  }
                                        }
                              }
                    }
                    if ( P < 0)
                    {
                    OnRev(OUT_C); OnFwd(OUT_A);
                    Wait (TURN);
                                                                                  // Ici on tourne à gauche de 90°
                              if (SENSOR_1 <= THRESHOLD)
                              {
                              P += 1;
                              OnFwd(OUT_AC);
                              Wait(100);
                              }
                              else
                              {
                              OnFwd(OUT_C); OnRev(OUT_A);
                              Wait (TURN);                                                // On va tout droit
                                       if (SENSOR_1 <= THRESHOLD)
                                       {
                                       OnFwd(OUT_AC);
                                       Wait(100);
                                       }
                                       else
                                       {
                                       OnFwd(OUT_C); OnRev(OUT_A);
                                       Wait (TURN);                                               // On tourna à droite
                                                 if (SENSOR_1 <= THRESHOLD)
                                                 {
                                                 P += -1;
                                                 OnFwd(OUT_AC);
                                                 Wait(100);
                                                 }
                                                 else                                                 // Demi-tour
                                                 {
                                                 OnRev(OUT_A);
                                                 OnFwd(OUT_C);
                                                 Wait(TURN);
                                                 until (SENSOR_1 <= THRESHOLD);
                                                 OnFwd(OUT_AC);
                                                 Wait(100);
                                                 }
                                       }
                              }
                    }
          }
       }


}