//******************************************************************
// Programme : Dandineur
// Date projet : 11/11/2000
// Date modification : 12/11/2000
// Auteur : Quesson
// V 0.1.0:12/11/2000 Le robot avance. En cas de detection d'un obstacle
// il recul puis fait demi-tour.
// V 0.1.1:12/11/2000 Idem avec une trajectoire aleatoire.
//******************************************************************

#define RECUL 300
#define DEMI_TOUR 200


//La tache suivante detecte un choc et invite le robot a eviter
//l'obstacle
task Si_choc() 
{
  
  while(true) 
  {
   if (SENSOR_1 == 1)                    //attente declanchement
   { 
     
     stop Deplacement;
     OnRev(OUT_A+OUT_C);                 //inverse la marche
     Wait(RECUL);                        //pendant la duree recul    
     Off(OUT_C);                         //stop un moteur pour tourner
     Wait(DEMI_TOUR);
     OnFwd(OUT_A+OUT_C);                 //on repart en avant...
     start Deplacement;
   }
  }
}

//La tache suivante permet le deplacement aleatoire du dandineur
task Deplacement()
{
  //Definition des variables qui serviront a definir une trajectoire
  int Duree_Dplct;
  int Direction_Dplct;
  int Direction;

  while(true)
  {
    //initialisation des variables :
    Duree_Dplct=Random(1000);
    Direction_Dplct=Random(500);
    Direction=Random(1)+1;
    
    //Le cycle de deplacement est le suivant : tout droit puis on tourne...
    OnFwd(OUT_A+OUT_C);                 //Tout droit pendant
    Wait(Duree_Dplct);                  // la duree de deplacement
    
    if(Direction == 1) 
    {  
      Off(OUT_C);                       //arret du moteur C
      Wait(Direction_Dplct);            //on tourne
      OnFwd(OUT_C);                     //et on repart...
      
    }
    else 
    {
      Off(OUT_A);                       //arret du moteur A
      Wait(Direction_Dplct);            //on tourne
      OnFwd(OUT_A);                     //et on repart...
    }
  }
}


//Tache principale : initialisation du robot
task main()
{
  SetSensor(SENSOR_1,SENSOR_TOUCH);  //active detecteur de contact
  OnFwd(OUT_A+OUT_C);                //marche des moteurs
  
  start Deplacement;                 //active deplacement 
  start Si_choc;                     //active tache Si_choc(); 
}