--                              -*- Mode: Ada -*-
-- Filename        : flex_common.ads
-- Description     : common data definitions and routines
-- Author          : Christfried Webers
-- Created On      : Wed Nov 10 15:02:14 1999
-- Last Modified By: .
-- Last Modified On: .
-- Update Count    : 0
-- Status          : Experimental
------------------------------------------------------------------------------

package Flex_Common is

   type Raw_8Bit     is mod 2**8;
   type Raw_16Bit    is mod 2**16;
   type Raw_32Bit    is mod 2**32;

   type Signed_32Bit is range -2**31..2**31-1;

   type ByteArray    is array (Integer range <>) of Raw_8Bit;
   type ByteArrayPointer is access ByteArray;

   -----------------------------------------------------------------------------
   --
   -- Utility routines
   --
   -----------------------------------------------------------------------------

   function ByteArrayToString (Data: in ByteArray) return String;
   function StringToByteArray (Data: in String) return ByteArray;

   function FourBytes_To_Raw_32Bit(Bytes: in ByteArray) return Raw_32Bit;
   function FourBytes_To_Signed_32Bit(Bytes: in ByteArray) return Signed_32Bit;
   function TwoBytes_To_Raw_16Bit(Bytes: in ByteArray) return Raw_16Bit;
   function Raw_32Bit_To_FourBytes(Number : in Raw_32Bit) return ByteArray;
   -- function Raw_32Bit_To_Signed_32Bit (Number : in Raw_32Bit) return Signed_32Bit;

   procedure PutHex(Number: in Raw_8Bit);
   procedure PutHex(Number: in Raw_16Bit);
   procedure PutHex(Number: in Raw_32Bit);

   procedure ShowContents(Contents: in ByteArray);

   -----------------------------------------------------------------------------
   --
   -- Packet types and definitions
   --
   -----------------------------------------------------------------------------
   type SendPacketType  is (LinMotorCommand, AngMotorCommand,
                            MotorStartStopSending,
                            SonarStartStopSending,
                            DIOStartStopSending);

   MotorDataID      : constant Raw_16Bit := 16#0221#; -- motor data from flex
   SonarDataID      : constant Raw_16Bit := 16#0402#; -- sonar data from flex
   DIODataID        : constant Raw_16Bit := 16#0501#; -- digital IO data from flex

   MotorCommandID   : constant Raw_16Bit := 16#0207#;
   MotorStartStopID : constant Raw_16Bit := 16#0222#;
   SonarStartStopID : constant Raw_16Bit := 16#0400#;
   DIOStartStopID   : constant Raw_16Bit := 16#0500#;


   type MotorCommandType          is new ByteArray(1..18);
   type MotorStartStopSendingType is new ByteArray(1..12);
   type SonarStartStopSendingType is new ByteArray(1..17);
   type DIOStartStopSendingType   is new ByteArray(1.. 8);

   -- Linux: 02 FF 22 08 00 01 86 A0 00 00 00 03
   MotorStartSendingPacket : MotorStartStopSendingType :=
     ( 16#02#, 16#FF#, 16#22#, 16#08#,  --
       16#00#, 16#00#, 16#00#, 16#00#,  -- timestamp
       16#00#, 16#00#, 16#00#, 16#03#   -- 03 = start
       );

   -- Linux: 02 FF 22 08 00 01 86 A0 00 00 00 00
   MotorStopSendingPacket : MotorStartStopSendingType :=
     ( 16#02#, 16#FF#, 16#22#, 16#08#,  --
       16#00#, 16#00#, 16#00#, 16#00#,  -- timestamp
       16#00#, 16#00#, 16#00#, 16#00#   -- 00 = stop
       );

   LinMotorCommandPacket : MotorCommandType :=
     ( 16#02#, 16#01#, 16#07#, 16#0E#,   --
       16#00#,                           -- axis
       16#00#, 16#00#, 16#00#, 16#00#,   -- velocity
       16#00#, 16#00#, 16#00#, 16#00#,   -- acceleration
       16#00#, 16#00#, 16#00#, 16#00#,   -- torque
       16#00#                            -- sign
       );

   AngMotorCommandPacket : MotorCommandType :=
     ( 16#02#, 16#01#, 16#07#, 16#0E#,   --
       16#01#,                           -- axis
       16#00#, 16#00#, 16#00#, 16#00#,   -- velocity
       16#00#, 16#00#, 16#00#, 16#00#,   -- acceleration
       16#00#, 16#00#, 16#00#, 16#00#,   -- torque
       16#00#                            -- sign
       );

   LinMotorEmergencyStopPacket : MotorCommandType :=
     ( 16#02#, 16#01#, 16#07#, 16#0E#,   --
       16#00#,                           -- axis
       16#00#, 16#00#, 16#00#, 16#00#,   -- velocity
       16#00#, 16#00#, 16#FF#, 16#FF#,   -- acceleration
       16#00#, 16#00#, 16#FF#, 16#FF#,   -- torque
       16#00#                            -- sign
       );

   AngMotorEmergencyStopPacket : MotorCommandType :=
     ( 16#02#, 16#01#, 16#07#, 16#0E#,   --
       16#01#,                           -- axis
       16#00#, 16#00#, 16#00#, 16#00#,   -- velocity
       16#00#, 16#00#, 16#FF#, 16#FF#,   -- acceleration
       16#00#, 16#00#, 16#FF#, 16#FF#,   -- torque
       16#00#                            -- sign
       );

   --    MotorDataPacket : ByteArray :=
   --      ( 16#02#, 16#01#, 16#21#, 16#19#,   -- 02 PacketNumber 21 Length
   --        16#00#, 16#00#, 16#00#, 16#00#,   -- 00 00 00 00
   --        16#00#, 16#00#, 16#00#, 16#00#,   -- time stamp
   --        16#00#,                           -- axis
   --        16#00#, 16#00#, 16#00#, 16#00#,   -- position
   --        16#00#, 16#00#, 16#00#, 16#00#,   -- velocity
   --        16#00#, 16#00#, 16#00#, 16#00#,   -- acceleration
   --        16#00#, 16#00#, 16#00#, 16#00#    -- torque
   --        );

   -- Linux: 04 FF 00 0D 00 00 75 30 00 00 00 00 00 00 00 00 02
   SonarStartSendingPacket : SonarStartStopSendingType :=
     (16#04#, 16#FF#, 16#00#, 16#0D#,  -- 04 FF 00 Length
      -- 16#00#, 16#00#, 16#75#, 16#30#,  -- Sending-Speed  5 Hz
      16#00#, 16#00#, 16#3A#, 16#98#,  -- Sending-Speed 10 Hz
      16#00#, 16#00#, 16#00#, 16#00#,  --
      16#00#, 16#00#, 16#00#, 16#00#,  --
      16#02#                           --
      );

   -- Linux: 04 FF 00 0D 00 00 00 00 00 00 00 00 00 00 00 00 00
   SonarStopSendingPacket : SonarStartStopSendingType :=
     (16#04#, 16#FF#, 16#00#, 16#0D#,  -- 04 FF 00 Length
      16#00#, 16#00#, 16#00#, 16#00#,  --
      16#00#, 16#00#, 16#00#, 16#00#,  --
      16#00#, 16#00#, 16#00#, 16#00#,  --
      16#00#                           --
      );

   DIOStartSendingPacket : DIOStartStopSendingType :=
     ( 16#05#, 16#05#, 16#00#, 16#04#,   -- 05 PacketNumber 00 Length
       16#00#, 16#00#, 16#27#, 16#10#    -- 2710 = start
       );

   DIOStopSendingPacket : DIOStartStopSendingType :=
     ( 16#05#, 16#05#, 16#00#, 16#04#,   -- 05 PacketNumber 00 Length
       16#00#, 16#00#, 16#00#, 16#00#    -- 0000 = stop
       );

end Flex_Common;