-- A double hyphen indicates the start of a comment. the comments 
-- ends at the end of the line. Comments are ignored by the Ada
-- compiler.

with Lego;  -- Informs compiler that this program will be making use of 
-- a package called Lego. This package contains Lego related subprograms
-- and types that we need to use

use Lego;

-------------------------------------------------------------------
--| Name of file: demo_basic_rover.adb
--| Unified.C&P Lecture #2
--|
--| Program to demo the basic functionality of the Lego Mindstorms rover
--| Programmer: Jane B
--| Date Created: September 01, 2004
--| Date Last Modified: September 14, 2004
--|
--| Comments: 
--| 1) It is important to note that HOW the rover is wired makes
--| a big difference. I.e., even if "Motor A" is connected to RCX output
--| "A," the orientation of how the terminals of the wire is connected
--| (if it is rotated by 90 degrees) will change the motors rotation 
--| direction.
--| 2) Make sure the robot is calibrated: make sure the switch that 
--| represents "centered" is triggered and that the steering column 
--| is actually centered. If, for some reason the steering column is 
--| centered and the touch sensor isn't triggered, the program will 
--| attempt to center the robot by operating the turning motor until 
--| the touch sensor is triggered.
-------------------------------------------------------------------     

procedure Demo_Basic_Rover is -- program heading (start of the program)

   -- Defines Left as a constant object with value 0, and Right to be 1
   -- The value of Left/Right cannot be changed by any subsequent 
   -- program statements
   Left  : constant Integer := 0;  
   Right : constant Integer := 1;  

   -- Defines the Drive motor power source to be Output_A
   Drive : constant Output_Port := Output_A;  
   -- Defines the Steering motor power source to be Output_B
   Steer : constant Output_Port := Output_B;  

   -- Defines straight to be a sensor port, corretaled to Sensor_1
   Straight : constant Sensor_Port := Sensor_1;  
   -- Defines Bumper to be a sensor port, corretaled to Sensor_2
   Bumper : constant Sensor_Port := Sensor_2;  


   -------------------------------------------------------------------
   -- Steer_Left: procedure to steer the front wheels of the rover 
   -- to the left
   -- inputs:  none
   -- outputs: none
   -- postconditions: rover's front wheels are turned left
   procedure Steer_Left is 
   begin
      --power the steering motor forward
      Output_On_Forward(Output => Steer);
      Wait(Hundredths_Of_A_Second => 25); -- wait for 0,25 seconds
      Output_Off(Output => Steer); -- turn off power to steering motor
   end Steer_Left;


   -------------------------------------------------------------------
   -- Steer_Right: procedure to steer the front wheels of the rover 
   -- to the right
   -- inputs:  none
   -- outputs: none
   -- postconditions: rover's front wheels are turned right
   procedure Steer_Right is 
   begin
      --power the steering motor backwards
      Output_On_Reverse(Output => Steer);
      Wait(Hundredths_Of_A_Second => 25); -- wait for 0,25 seconds
      Output_Off(Output => Steer); --turn off power to steering motor
   end Steer_Right;


   -------------------------------------------------------------------
   -- Steer_Center: procedure to straighten the rover front wheels
   -- inputs:  none
   -- outputs: none
   -- postconditions: rover front wheels are aligned straight
   procedure Steer_Center is 
   begin
      --change the direction in which steering motor rotates
      Set_Output_Direction(
         Output    => Steer,                  
         Direction => Output_Direction_Toggle);
      --turn the steering power on
      Output_On(Output => Steer);
      --use the straight sensor input to center the drive
      while (Get_Sensor_Value(Sensor=>Straight)=0) loop
         Wait(Hundredths_Of_A_Second => 1);
      end loop;

      Output_Off(Output => Steer); --turn off power to steering motor
   end Steer_Center;


   -------------------------------------------------------------------
   -- Go_Forward: procedure to move the rover forward
   -- inputs:  none
   -- outputs: none
   -- postconditions: rover moves forward
   procedure Go_Forward is 
   begin
      --turn on drive motor power
      Output_On(Output => Drive);
   end Go_Forward;


   -------------------------------------------------------------------
   -- Forward: procedure to move the rover forward for a given duration
   --          of time
   -- inputs:  amount of time (in 10th of a second) the rover 
   --          moves forward
   -- outputs: none
   -- postconditions: rover moves forward for the given duration of time
   procedure Forward (
         Tenths_Of_A_Second : in     Integer ) is 
   begin
      -- provide power to the drive motor
      Output_On_Forward(Output => Drive);
      -- wait for required duration of time
      Wait(Hundredths_Of_A_Second => Tenths_Of_A_Second * 10);
      --turn off the power to the drive motor
      Output_Off(Output => Drive);
   end Forward;


   -------------------------------------------------------------------
   -- Back: procedure to move the rover backward for a given duration 
   --       of time
   -- inputs:  amount of time (in 10th of a second) the rover moves back
   -- outputs: none
   -- postconditions: rover moves back for the given duration of time
   procedure Back (
         Tenths_Of_A_Second : in     Integer ) is 
   begin
      --provide power to the drive motor in reverse
      Output_On_Reverse(Output => Drive);
      -- wait for the required time
      Wait(Hundredths_Of_A_Second => Tenths_Of_A_Second * 10);
      -- turn off power to the drive motor
      Output_Off(Output => Drive);
   end Back;


   -------------------------------------------------------------------
   -- Turn: procedure to turn the rover front wheels in a specified 
   --       direction
   -- inputs:  direction in which the rover has to turn
   -- outputs: none
   -- postconditions: rover front wheels in the required direction
   procedure Turn (
         Direction : in     Integer ) is 
   begin

      if Direction = Left then  -- if the direction is to the left
         Steer_Center;          -- center the front wheels
         Steer_Left;            -- turn the wheels left
      else -- turn right
         Steer_Center;
         Steer_Right;
      end if;
   end Turn;


   -------------------------------------------------------------------
   -- Initialize_Rover: procedure to configure the rover
   -- inputs:     none
   -- outputs:    none
   -- postconditions: the sensors on the rover correlated to input and
   --                 output ports
   procedure Initialize_Rover is 
   begin
      --defines the Straight sensor port to accept Touch Sensor input
      Config_Sensor(
         Sensor => Straight,    
         Config => Config_Touch);

      --defines the Bumper sensor port to accept Touch Sensor input
      Config_Sensor(
         Sensor => Bumper,      
         Config => Config_Touch);

      --set drive motor power to High
      Output_Power(
         Output => Drive,     
         Power  => Power_High);

      --set steering motor power to Low
      Output_Power(
         Output => Steer,    
         Power  => Power_Low);

      --align the rover to the center
      Steer_Center;
   end Initialize_Rover;




   -------------------------------------------------------------------
   -- This is the part of the code that has to be changed in 
   -- C&P pset #2, #3
   -------------------------------------------------------------------
begin

   Initialize_Rover;    -- initialize the rover

   Forward(20);         -- move rover forward for 20*0,10 seconds
   Turn(Left);          -- turn rover left
   Forward(5);
   Steer_Center;        -- straightens the rover

   Back(20);            -- move rover backwards for 2*0,10 seconds
   Turn(Right);         -- turn rover right
   Back(5);
   Steer_Center;

   Forward(20);

end Demo_Basic_Rover;