-------------------------------------------------------------------------------
-- Lego Mindstorms Demo 2 -- Touch Sensors with counter                       -
-- a.k.a. "Parked-In"                                                         -
-- David Johnson, Joe Bondi C. S. Draper Laboratory                           -
-- For MIT Unified                                                            -
--                                                                            -
-- October 6, 2005                                                            -
--                                                                            -
-- Goal:  The car drives forward until it hits something, and then goes in    -
--        reverse until it hits... Just like what happens when you get parked -
--        in on some side-street in downtown Boston.  The car will give up    -
--        after 10 bumps.                                                     -
-------------------------------------------------------------------------------

with Lego;
use Lego;

procedure BumperCar_Limited is
   
   -- Define how we will address each motor and sensor
   Drive_Motor : constant Output_Port := Output_A;
   Front_Touch : constant Sensor_Port := Sensor_1;
   Rear_Touch  : constant Sensor_Port := Sensor_3;
   
   -- Define a counter to keep track of the number of bumps so far.
   Bump_Counter : Integer := 0;
   
begin

   -- Initialize the Wheels
   Output_Power(
      Output => Drive_Motor,
      Power  => Power_High);

   -- Initialize the Touch Sensors
   Config_Sensor(
      Sensor => Front_Touch,
      Config => Config_Touch);

   Config_Sensor(
      Sensor => Rear_Touch,
      Config => Config_Touch);

   -- Start by Driving Forward
   Output_On_Forward(Output => Drive_Motor);

   loop

      -- Start backing up if the car hits something.
      if Get_Sensor_Value(Sensor => Front_Touch) = 1 then
      
         Output_Reverse(Output => Drive_Motor);
         
         -- Increment the bump counter.
         -- Also, wait a half second so that we don't increment more than once,
         -- This could happen if the car bumps into something soft and the touch
         -- sensor gets held down for more than one loop cycle.
         Bump_Counter := Bump_Counter + 1; Wait(50);
         
      end if;

      -- Make the escape if we hit something behind us.
      if Get_Sensor_Value(Sensor => Rear_Touch) = 1 then
         Output_Forward(Output => Drive_Motor);
         Bump_Counter := Bump_Counter + 1; Wait(50);
      end if;

      -- Check the total number of bumps so far to see if it is time to stop.
      exit when Bump_Counter >= 10;

   end loop;

   -- Now that the loop is done, we must command the drive motors to stop,
   -- or else the car will continue to drive in whatever direction it was
   -- last commanded.
   Output_Off(Output => Drive_Motor);

end BumperCar_Limited;