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;