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;