File : testflex.adb
-- -*- Mode: Ada -*-
-- Filename : testflex.adb
-- Description : Test program for flex controller
-- Author : Christfried Webers
-- Created On : Mon Oct 25 17:14:13 1999
-- Last Modified By: .
-- Last Modified On: .
-- Update Count : 0
-- Status : Unknown, Use with caution!
with Text_IO; use Text_IO;
with Ada.Integer_Text_IO; use Ada.Integer_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 Metrics; use Metrics;
with Motor;
with Sonar;
with Laser;
procedure TestFlex is
Program_Active : Boolean := True;
ShowSonar : Boolean := True;
ShowMotor : Boolean := True;
ShowSonarFrequency : Boolean := True;
ShowMotorFrequency : Boolean := True;
--------------------------------------------------------
function CalcFrequency
(Start, Stop : Time;
Counter : Integer)
return Float
is
begin
return Float (Counter) /
Float (((Stop - Start) / Milliseconds (1))) *
1000.0;
end CalcFrequency;
--------------------------------------------------------
task Waiting_For_A_Key;
task body Waiting_For_A_Key is
ActChar : Character;
CharAvailable : Boolean;
begin
while Program_Active loop
Get_Immediate (ActChar, CharAvailable);
case ActChar is
when 'h' =>
Put_Line ("---- Stopped Motor! ---");
Program_Active := False;
exit;
when 'y' =>
Motor.SetLinearVelocity (1.0);
when 'b' =>
Motor.SetLinearVelocity (-1.0);
when 'g' =>
Motor.SetAngularVelocity (45.0);
null;
when 'j' =>
Motor.SetAngularVelocity (-45.0);
null;
when 'N' =>
Sonar.ResumeReading;
when 'n' =>
Sonar.SuspendReading;
when 'm' =>
ShowMotor := not ShowMotor;
when 's' =>
ShowSonar := not ShowSonar;
when 'p' =>
ShowMotorFrequency := not ShowMotorFrequency;
when 'P' =>
ShowSonarFrequency := not ShowSonarFrequency;
when others =>
null;
end case;
delay 0.1;
end loop;
Put_Line ("Terminated Waiting_For_A_Key");
exception
when E : others =>
Put_Line ("- " & Image (Current_Task) & " terminating because of");
Put_Line
("- exception " &
Exception_Name (E) &
" (" &
Exception_Message (E) &
")");
end Waiting_For_A_Key;
--------------------------------------------------------
procedure PutMotor (Reading : Motor.Status) is
begin
Put (" Pos:");
Put (Integer (Reading.LinearPosition), 8);
Put (Float (Reading.AngularPosition), Fore => 4, Aft => 2, Exp => 0);
Put (" Vel:");
Put (Float (Reading.LinearVelocity), Aft => 2, Exp => 0);
Put (Float (Reading.AngularVelocity), Aft => 2, Exp => 0);
Put (" Acc:");
Put (Float (Reading.LinearAcceleration), Aft => 2, Exp => 0);
Put (Float (Reading.AngularAcceleration), Aft => 2, Exp => 0);
Put (" Tor:");
Put (Float (Reading.LinearTorque), Aft => 2, Exp => 0);
Put (Float (Reading.AngularTorque), Aft => 2, Exp => 0);
New_Line;
end PutMotor;
task ReadMotor;
task body ReadMotor is
MotorData : Motor.Status;
StartTime : Time := Clock;
Counter : Integer := 0;
Interval : Time_Span := Milliseconds (1000);
NextTime : Time := Clock + Interval;
Frequency : Float;
begin
while Program_Active loop
Motor.WaitForNewData;
Counter := Counter + 1;
Motor.GetCurrentStatus (MotorData);
if ShowMotor then
PutMotor (MotorData);
end if;
if NextTime < Clock then
if ShowMotorFrequency then
Put ("Motor reading frequency is ");
Frequency := CalcFrequency (StartTime, Clock, Counter);
Put (Frequency, Aft => 2);
Put (" Herz");
New_Line;
end if;
NextTime := Clock + Interval;
end if;
end loop;
Put_Line ("- flex: Terminated ReadMotor");
exception
when E : others =>
Put_Line ("- " & Image (Current_Task) & " terminating because of");
Put_Line
("- exception " &
Exception_Name (E) &
" (" &
Exception_Message (E) &
")");
end ReadMotor;
--------------------------------------------------------
procedure PutSonar (SonarData : Sonar.Readings) is
begin
for I in Sonar.Index loop
if not SonarData (I).Valid then
Put (" -------- ");
else
Put ("(");
Put (Integer (SonarData (I).Distance), 4);
Put (",");
Put (Integer (SonarData (I).Angle), 3);
Put (")");
end if;
end loop;
New_Line;
end PutSonar;
task ReadSonar;
task body ReadSonar is
SonarData : Sonar.Status;
StartTime : Time := Clock;
Counter : Integer := 0;
Frequency : Float;
Interval : Time_Span := Milliseconds (1000);
NextTime : Time := Clock + Interval;
begin
Sonar.Init;
while Program_Active loop
Sonar.WaitForNewData;
Counter := Counter + 1;
Sonar.GetCurrentStatus (SonarData);
if ShowSonar then
PutSonar (SonarData.Sonar);
end if;
if NextTime < Clock then
if ShowSonarFrequency then
Put ("Sonar reading frequency is ");
Frequency := CalcFrequency (StartTime, Clock, Counter);
Put (Frequency, Aft => 2);
Put (" Herz");
New_Line;
end if;
NextTime := Clock + Interval;
end if;
end loop;
Sonar.Shutdown;
Put_Line ("- flex: Terminated ReadSonar");
exception
when E : others =>
Put_Line ("- " & Image (Current_Task) & " terminating because of");
Put_Line
("- exception " &
Exception_Name (E) &
" (" &
Exception_Message (E) &
")");
end ReadSonar;
--------------------------------------------------------
begin
Motor.Init;
Sonar.Init;
while Program_Active loop
delay 0.1;
end loop;
Sonar.Shutdown;
Motor.Shutdown;
Put_Line ("- flex: Terminated main program body");
exception
when E : others =>
Put_Line ("- " & Image (Current_Task) & " terminating because of");
Put_Line
("- exception " &
Exception_Name (E) &
" (" &
Exception_Message (E) &
")");
end TestFlex;