package Digital_Output is

   -- Identifier of a digital output line
   type Channel_Id is range 1..2;

   -- Digital output value
   type Digital_State is (Low, High);

   --------------------------------------------------------------------
   -- Close
   --------------------------------------------------------------------
   --
   -- This procedure must be called once the package is not longer to
   -- be used, to close the output device

   procedure Close;


   --------------------------------------------------------------------
   -- Set
   --------------------------------------------------------------------
   --
   -- This procedure sets a particular digital channel to the given
   -- digital value


   procedure Set (Channel : Channel_Id; To : Digital_State);

   --------------------------------------------------------------------
   -- Get
   --------------------------------------------------------------------
   --
   -- This function returns the digital value of a particular channel

   function Get (Channel : Channel_Id) return Digital_State;

end Digital_Output;
------------------------------------------------------------------
-- Pulse Width Modulation example
--
-- This package contains the protected object that is used to store
-- the desired positions for two servo mechanisms, and the
-- implementation that takes care of generating the timed signals
-- needed to communicate these desired positions to the hardware
-- devices
-------------------------------------------------------------------

with System;
with Ada.Real_Time;
with Ada.Real_Time.Timing_Events;

package PWM is

   package Timing_Events renames Ada.Real_Time.Timing_Events;

   ----------------------------------------------------------------
   -- Configuration constants
   ----------------------------------------------------------------
   Angular_Range       : constant:= 240.0;         --degrees
   Max_Pulse_Width     : constant Duration:=0.100; --seconds
   Min_Pulse_Width     : constant Duration:=0.200; --seconds
   Central_Pulse_Width : constant Duration:=
     (Max_Pulse_Width+Min_Pulse_Width)/2.0;
   Pulse_Period        : constant Duration:=2.000; --seconds

   ----------------------------------------------------------------
   -- Identifier of each of the two servo mechanims
   ----------------------------------------------------------------
   type Servo_Id is (Steering,Throtle);

   ----------------------------------------------------------------
   -- Angular position of a servo mechanism (in degrees)
   ----------------------------------------------------------------
   type Position is digits 5
     range -Angular_Range/2.0 .. Angular_Range/2.0;

   ----------------------------------------------------------------
   -- Extension of the Timing_Event tagged type to store an
   -- additional attribute with the servo Id
   ----------------------------------------------------------------
   type Servo_Timing_Event is new Timing_Events.Timing_Event with record
      Id : Servo_Id;
   end record;

   ----------------------------------------------------------------
   -- Values of this type represent the positions of several servos
   ----------------------------------------------------------------
   type Servo_Position is array (Servo_Id) of Position;

   ----------------------------------------------------------------
   -- Values of this type represent time values associated with
   -- the servos
   ----------------------------------------------------------------
   type Servo_Time is array (Servo_Id) of Ada.Real_Time.Time;

   ----------------------------------------------------------------
   -- Protected object used to store the desired poritions for the
   -- servos
   ----------------------------------------------------------------
   protected Servo_Control is

      -------------------------------------------------------------
      -- Set the position of a given servo to the given value
      -------------------------------------------------------------
      procedure Set_Position(Id : Servo_Id; Pos : Position);

      -------------------------------------------------------------
      -- Initiate the control actions on a servo
      -------------------------------------------------------------
      procedure Init (Id : Servo_Id);

      pragma Priority(System.Interrupt_Priority'Last);

   private

      Commanded_Position : Servo_Position:=(0.0,0.0);
      Pulse_Start_Time : Servo_Time;

      -------------------------------------------------------------
      -- Start the pulse on one of the servo outputs
      -------------------------------------------------------------
      procedure Start_Pulse (Event : in out Timing_Events.Timing_Event);

      -------------------------------------------------------------
      -- Finish the pulse on one of the servo outputs
      -------------------------------------------------------------
      procedure End_Pulse (Event : in out Timing_Events.Timing_Event);

   end Servo_Control;

end PWM;
with Ada.Text_IO;
with Ada.Real_Time;

package body Digital_Output is


   -- This is a simulated body for the digital output package, which
   -- writes changes made to the digital lines in a text file

   use type Ada.Real_Time.Time;

   Max_Data : constant:= 120;

   type Dig_State is array(Channel_Id) of Digital_State;
   State : Dig_State:=(Low,Low);

   Initial_Time : Ada.Real_Time.Time:=Ada.Real_Time.Clock;

   Interval_Log : array(1..Max_Data) of Duration;
   State_Log : array(1..Max_Data) of Dig_State;
   Log_Num : Integer range 0..Max_Data:=0;

   -----------
   -- Close --
   -----------

   procedure Close is

      Image : array(Digital_State) of String(1..4):=("   0","   1");

      Data_File : Ada.Text_IO.File_Type;

   begin
      Ada.Text_IO.Create(Data_File,Ada.Text_IO.Out_File,"pwm_data.txt");
      Ada.Text_IO.Put_Line(Data_File, "# PWM example");
      Ada.Text_IO.Put_Line(Data_File, "# time channel_1 channel_2");
      Ada.Text_IO.Put_Line(Data_File, "#");
      for I in 1..Log_Num loop
         Ada.Text_IO.Put_Line
           (Data_File,Duration'Image(Interval_Log(I))&
            Image(State_Log(I)(1))&Image(State_Log(I)(2)));
      end loop;
      Ada.Text_IO.Close(Data_File);
   end Close;

   ---------
   -- Set --
   ---------

   procedure Set (Channel : Channel_Id; To : Digital_State) is
   begin
      -- Change state
      State(Channel):=To;
      -- print new state
      if Log_Num<Max_Data then
         Log_Num:=Log_Num+1;
         Interval_Log(Log_Num):=
           Ada.Real_Time.To_Duration(Ada.Real_Time.Clock-Initial_Time);
         State_Log(Log_Num):=State;
      end if;
   end Set;

   ---------
   -- Get --
   ---------

   function Get (Channel : Channel_Id) return Digital_State is
   begin
      return State(Channel);
   end Get;

end Digital_Output;
with Digital_Output;

package body PWM is

   use type Ada.Real_Time.Time;

   -- Mapping between servo Id and digital channel
   Digital_Channel : constant array(Servo_Id) of
     Digital_Output.Channel_Id := (Steering => 1, Throtle => 2);

   -- Timed events, one per servo
   Timer : array(Servo_Id) of Servo_Timing_Event;

   -------------------
   -- Servo_Control --
   -------------------

   protected body Servo_Control is

      ------------------
      -- Set_Position --
      ------------------

      procedure Set_Position (Id : Servo_Id; Pos : Position) is
      begin
         Commanded_Position(Id):=Pos;
      end Set_Position;

      ----------
      -- Init --
      ----------

      procedure Init (Id : Servo_Id) is
      begin
         Pulse_Start_Time(Id):=Ada.Real_Time.Clock;
         Timer(Id).Id:=Id;
         Start_Pulse (Timing_Events.Timing_Event(Timer(Id)));
      end Init;

      -----------------
      -- Start_Pulse --
      -----------------

      procedure Start_Pulse (Event : in out Timing_Events.Timing_Event) is
         Pulse_Stop_Time : Ada.Real_Time.Time;
         Id : Servo_Id:=
           Servo_Timing_Event(Timing_Events.Timing_Event'Class(Event)).Id;
      begin
         -- Start the pulse on the corresponding channel
         Digital_Output.Set(Digital_Channel(Id),Digital_Output.High);
         -- Calculate the stop time
         Pulse_Stop_Time:=Pulse_Start_Time(Id)+
           Ada.Real_Time.To_Time_Span
           (Duration(Float(Commanded_Position(Id)/Angular_Range)*
                     Float(Max_Pulse_Width-Min_Pulse_Width))+
            Central_Pulse_Width);
         -- Program the timed event for the nearest stop time
         Timing_Events.Set_Handler
           (Event,Pulse_Stop_Time,End_Pulse'Access);
      end Start_Pulse;

      ---------------
      -- End_Pulse --
      ---------------

      procedure End_Pulse (Event : in out Timing_Events.Timing_Event) is
         Id : Servo_Id:=
           Servo_Timing_Event(Timing_Events.Timing_Event'Class(Event)).Id;
      begin
         -- Stop the pulse on the corresponding channel
         Digital_Output.Set(Digital_Channel(Id),Digital_Output.Low);
         -- Program the timed event for the next stop time
         Pulse_Start_Time(Id):=Pulse_Start_Time(Id)+
           Ada.Real_Time.To_Time_Span(Pulse_Period);
         Timing_Events.Set_Handler
           (Event,Pulse_Start_Time(Id),Start_Pulse'Access);
      end End_Pulse;

   end Servo_Control;

end PWM;
with Pwm;
with Digital_Output;

procedure Test_Pwm is
   use type Pwm.Position;
begin
   -- Set initial position and start the PWM control
   Pwm.Servo_Control.Set_Position(Pwm.Steering,100.0);
   Pwm.Servo_Control.Set_Position(Pwm.Throtle,-100.0);
   Pwm.Servo_Control.Init(Pwm.Steering);
   Pwm.Servo_Control.Init(Pwm.Throtle);
   -- Change position after some time
   delay 3.0;
   Pwm.Servo_Control.Set_Position(Pwm.Steering,0.0);
   Pwm.Servo_Control.Set_Position(Pwm.Throtle,100.0);
   -- Let the experiment run for some time and then close it
   delay 20.0;
   Digital_Output.Close;
end Test_Pwm;