File : motor_handler.adb


--                              -*- Mode: Ada -*-
-- Filename        : motor_handler.adb
-- Description     : synchronization for the motor
-- Author          : Christfried Webers
-- Created On      : Mon Nov  8 10:49:07 1999
-- Last Modified By: .
-- Last Modified On: .
-- Update Count    : 0
-- Status          : Experimental
-------------------------------------------------------------------
with Text_IO;           use Text_IO;
with Ada.Float_Text_IO; use Ada.Float_Text_IO;

with Ada.Real_Time; use Ada.Real_Time;

with Ada.Task_Identification; use Ada.Task_Identification;
with Ada.Exceptions;          use Ada.Exceptions;

with Flex_Common; use Flex_Common;
with Flex_Driver;
with Motor;

---------------------
package body Motor_Handler is

   ----------------------------------------------------------------------------
   ----
   --
   -- Global constants
   --
   ----------------------------------------------------------------------------
   ----

   LinearPositionScale  : constant Float := 87.462; -- result in mm
   AngularPositionScale : constant Float := 659.4; -- result in degrees

   LinearVelocityScale  : constant Float := 87_462.0; -- result in m/s
   AngularVelocityScale : constant Float := 659.4; -- result in degrees per
                                                   --second

   LinearAccelerationScale  : constant Float := 10_000.0; -- result in m/s^2
   AngularAccelerationScale : constant Float := 606.1;   -- result in
                                                         --degrees/s^2

   LinearTorqueScale  : constant Float := 30_000.0;   -- standard settings for
                                                      --1.0
   AngularTorqueScale : constant Float := 40_000.0;   -- standard settings for
                                                      --1.0

   LinearAxis  : constant Raw_8Bit := 0;
   AngularAxis : constant Raw_8Bit := 1;

   Forward  : constant Raw_8Bit := 1;
   Backward : constant Raw_8Bit := 0;

   Left  : constant Raw_8Bit := 1;
   Right : constant Raw_8Bit := 0;

   ----------------------------------------------------------------------------
   ----
   --
   -- Reader
   --
   ----------------------------------------------------------------------------
   ----

   protected body Reader is

      procedure RecordPacket (PacketNumber : Raw_8Bit; Data : ByteArray) is
         First    : Integer               := Data'First;
         Axis     : Raw_8Bit range 0 .. 1 := Data (First + 8);
         Position : Float                 :=
            Float (FourBytes_To_Signed_32Bit
                      (Data (First + 9 .. First + 12)));

         Velocity : Float :=
            Float (FourBytes_To_Signed_32Bit
                      (Data (First + 13 .. First + 16)));

         Acceleration : Float :=
            Float (FourBytes_To_Signed_32Bit
                      (Data (First + 17 .. First + 20)));

         Torque : Float :=
            Float (FourBytes_To_Signed_32Bit
                      (Data (First + 21 .. First + 24)));

      begin
         case Axis is
            when LinearAxis =>
               NewStatus.LinearPosition :=
                 mm (Integer (Position / LinearPositionScale));

               NewStatus.LinearVelocity :=
                 LinVel (Velocity / LinearVelocityScale);

               NewStatus.LinearAcceleration :=
                 LinAcc (Acceleration / LinearAccelerationScale);

               NewStatus.LinearTorque :=
                 LinTorque (Torque / LinearTorqueScale);

            when AngularAxis =>
               Position := Position / AngularPositionScale;

               -- map the angle on -180.0 .. +180.0
               NewStatus.AngularPosition :=
                 Degrees (Float'Remainder (Position, 360.0));

               NewStatus.AngularVelocity :=
                 AngVel (Velocity / AngularVelocityScale);

               NewStatus.AngularAcceleration :=
                 AngAcc (Acceleration / AngularAccelerationScale);

               NewStatus.AngularTorque :=
                 AngTorque (Torque / AngularTorqueScale);

               NewStatus.Timestamp := Clock;
               LastStatus          := NewStatus;
               NewDataArrived      := True;
               FirstDataArrived    := True;
         end case;
      end RecordPacket;

      entry GetCurrentStatus (MotorStatus : out Motor.Status) when
           FirstDataArrived is
      begin
         MotorStatus := LastStatus;
      end GetCurrentStatus;

      entry WaitForNewData when NewDataArrived or NonBlockingActive is
      begin
         if WaitForNewData'Count = 0 then
            NewDataArrived := False;
         end if;
      end WaitForNewData;

      procedure NonBlocking is
      begin
         NonBlockingActive := True;
      end NonBlocking;

   end Reader;

   ----------------------------------------------------------------------------
   ----
   --
   -- Commander
   --
   ----------------------------------------------------------------------------
   ----

   procedure SendLinearCommand
     (Velocity     : LinVel;
      Acceleration : LinAcc;
      Torque       : LinTorque)
   is
      Packet          : MotorCommandType := LinMotorCommandPacket;
      RawVelocity     : Raw_32Bit;
      RawAcceleration : Raw_32Bit;
      RawTorque       : Raw_32Bit;
      RawDirection    : Raw_8Bit;

   begin
      if Velocity >= 0.0 then
         RawDirection := Forward;
      else -- Velocity < 0.0
         RawDirection := Backward;
      end if;

      RawVelocity     :=
        Raw_32Bit (abs (Float (Velocity) * LinearVelocityScale));
      RawAcceleration :=
        Raw_32Bit (abs (Float (Acceleration) * LinearAccelerationScale));

      RawTorque := Raw_32Bit (abs (Float (Torque) * LinearTorqueScale));

      Packet (5)        := LinearAxis;
      Packet (6 .. 9)   := Raw_32Bit_To_FourBytes (RawVelocity);
      Packet (10 .. 13) := Raw_32Bit_To_FourBytes (RawAcceleration);
      Packet (14 .. 17) := Raw_32Bit_To_FourBytes (RawTorque);
      Packet (18)       := RawDirection;

      Flex_Driver.WriteTask.SendPacket (ByteArray (Packet), LinMotorCommand);
   end SendLinearCommand;

   procedure SendAngularCommand
     (Velocity     : AngVel;
      Acceleration : AngAcc;
      Torque       : AngTorque)
   is
      Packet          : MotorCommandType := AngMotorCommandPacket;
      RawVelocity     : Raw_32Bit;
      RawAcceleration : Raw_32Bit;
      RawTorque       : Raw_32Bit;
      RawDirection    : Raw_8Bit;
   begin
      if Velocity >= 0.0 then
         RawDirection := Left;
      else -- Velocity < 0.0
         RawDirection := Right;
      end if;

      RawVelocity     :=
        Raw_32Bit (abs (Float (Velocity) * AngularVelocityScale));
      RawAcceleration :=
        Raw_32Bit (abs (Float (Acceleration) * AngularAccelerationScale));

      RawTorque := Raw_32Bit (abs (Float (Torque) * AngularTorqueScale));

      Packet (5)        := AngularAxis;
      Packet (6 .. 9)   := Raw_32Bit_To_FourBytes (RawVelocity);
      Packet (10 .. 13) := Raw_32Bit_To_FourBytes (RawAcceleration);
      Packet (14 .. 17) := Raw_32Bit_To_FourBytes (RawTorque);
      Packet (18)       := RawDirection;

      Flex_Driver.WriteTask.SendPacket (ByteArray (Packet), AngMotorCommand);
   end SendAngularCommand;

   protected body Commander is

      procedure StartReading is
      begin
         Flex_Driver.WriteTask.SendPacket
           (ByteArray (MotorStartSendingPacket),
            MotorStartStopSending);

         -- Flex_Driver.WriteTask.SendPacket(ByteArray(DIOStartSendingPacket),
         --                                 DIOStartStopSending);
      end StartReading;

      procedure StopReading is
      begin
         Flex_Driver.WriteTask.SendPacket
           (ByteArray (MotorStopSendingPacket),
            MotorStartStopSending);

         --Flex_Driver.WriteTask.SendPacket(ByteArray(DIOStopSendingPacket),
         --                                 DIOStartStopSending);
      end StopReading;

      procedure SetLinearVelocity (Velocity : LinVel) is
      begin
         LinearVelocity := Velocity;
         SendLinearCommand (LinearVelocity, LinearAcceleration, LinearTorque);
      end SetLinearVelocity;

      procedure SetAngularVelocity (Velocity : AngVel) is
      begin
         AngularVelocity := Velocity;
         SendAngularCommand
           (AngularVelocity,
            AngularAcceleration,
            AngularTorque);
      end SetAngularVelocity;

      procedure SetLinearAcceleration (Acceleration : LinAcc) is
      begin
         LinearAcceleration := Acceleration;
         SendLinearCommand (LinearVelocity, LinearAcceleration, LinearTorque);
      end SetLinearAcceleration;

      procedure SetAngularAcceleration (Acceleration : AngAcc) is
      begin
         AngularAcceleration := Acceleration;
         SendAngularCommand
           (AngularVelocity,
            AngularAcceleration,
            AngularTorque);
      end SetAngularAcceleration;

      procedure SetLinearTorque (Torque : LinTorque) is
      begin
         LinearTorque := Torque;
         SendLinearCommand (LinearVelocity, LinearAcceleration, LinearTorque);
      end SetLinearTorque;

      procedure SetAngularTorque (Torque : AngTorque) is
      begin
         AngularTorque := Torque;
         SendAngularCommand
           (AngularVelocity,
            AngularAcceleration,
            AngularTorque);
      end SetAngularTorque;

   end Commander;

end Motor_Handler;