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;


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;
      Frequency : Float;
      Interval  : Time_Span := Milliseconds(1000);
      NextTime  : Time := Clock + Interval;
   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;