// *************************************************************
// * LEGO MINDSTORMS: RCX                                      *
// * --------------------------------------------------------- *
// * BOT     : Roverbot (Patrol)                               *
// * SERIAL  : 2006.001.rcx.c1                                 *
// *                                                           *
// * PROGRAM : roverbotPatrol.nqc                              *
// * AUTHOR  : Shane L. Larson, gravitino@earthlink.net        *
// * DATE    : 13 May 2006                                     *
// * --------------------------------------------------------- *
// * NOTES:                                                    *
// * This is my first program in NQC.  Designed for my first   *
// * bot -- the Roverbot from the RIS 2.0 Constructopedia.  I  *
// * had to decide on the tasks, which I roughly chose from    *
// * challenges I could extract from the RIS Windoze software. *
// *                                                           *
// * For this design of the bot, it is supposed to patrol back *
// * and forth between two black lines.  When it reaches a     *
// * it turns around and seeks the other line.  If someone     *
// * hits a bumper, it stops and emits a klaxxon alarm.        *
// *                                                           *
// *************************************************************


// ------- CONSTANTS -------------------------------------------

#define LeftDrive OUT_A
#define RightDrive OUT_C

#define LeftBump SENSOR_1
#define RightBump SENSOR_3
#define LineSeeker SENSOR_2

#define TURN_TIME 19  
#define KLAXXON SOUND_FAST_UP


// ------- TASKS -----------------------------------------------

task main()
{
   // --- local vars ----------------------------
   
   int kFLAG;
   int rFLAG;

   // --- start main() here ---------------------
   // -------------------------------------------
   
   // --- Initializations ----- 
   
   kFLAG = 0;
   rFLAG = 0;

   SetSensor(LeftBump, SENSOR_TOUCH);
   SetSensor(RightBump, SENSOR_TOUCH);
   SetSensor(LineSeeker, SENSOR_LIGHT);

   do
   {
      // On patrol, both bot drives go forward
      OnFwd(LeftDrive + RightDrive);
      
      // Drive state changes if bumped, or reaches the borders
      until( (LeftBump == 1) || (RightBump == 1) || (LineSeeker <= 48) );

      if ( (LeftBump == 1) || (RightBump == 1) )
      {
         kFLAG = 1;  // if bump, then kick out of loop
      }
      else // If reached border, make the turn
      {
         ClearTimer(0);  // making the turn around based on timing
         
         // rFLAG simply makes the bot turn around to the right at
         // one border and around to the left at the other border.

         // Initiate turn around by toggling one of the drive trains
         if (rFLAG == 0) 
         {
            Toggle(RightDrive);
         }
         else
         {
            Toggle(LeftDrive);
         }

         // This until looks for bumps during the turn around
         until( (Timer(0) > TURN_TIME) || (LeftBump == 1) || (RightBump == 1) );

         // If we are at this point, either someone bumped us, or we
         // have made the complete turn around.  Do the right thing.

         if ( (LeftBump == 1) || (RightBump == 1) )
         {
            kFLAG = 1;  // if bump, then kick out of loop
         }
         else  // if not bump, then toggle drive to drive straight

         {
            if (rFLAG == 0)
            {
               Toggle(RightDrive);
               rFLAG = 1;
            }
            else
            {
               Toggle(LeftDrive);
               rFLAG = 0;
            }
         }
      }

   } while( kFLAG == 0 );
   

   // If we reached this point, then someone bumped the bot.  In this
   // case stop all motion, and sound the klaxxon alarm.

   Off(LeftDrive + RightDrive);

   PlaySound(KLAXXON);
   Wait(50);
   PlaySound(KLAXXON);

}