-------------------------------------------------------------------------------
-- Lego Mindstorms Demo 1 -- Touch Sensors                                    -
-- a.k.a. "Parked-In"                                                         -
-- David Johnson, 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.                          -
-------------------------------------------------------------------------------

with Lego;
use Lego;

procedure BumperCar 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;

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);

   -- It never ends ...
   loop

      -- Start backing up if the car hits something
      if Get_Sensor_Value(Sensor => Front_Touch) = 1 then
         Output_Reverse(Output => Drive_Motor);
      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);
      end if;

   end loop;

end BumperCar;