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;