add Status Topic to RoboLib, implement timer in RaspiControl and states based on status messages

ADIS_Csharp/RaspiControl/Application.cs

@@ -1,25 +1,59 @@
 using NLog.LayoutRenderers;
+using RobotLib.Status;
 using System;
 using System.Collections.Generic;
+using System.Diagnostics;
 using System.Linq;
 using System.Text;
 using System.Text.Json;
 using System.Threading.Tasks;
 
 namespace RaspiControl {
-    
+
     internal class Application {
         private ChallengeFactory? challenge;
         private StateMachine sm;
         private readonly NLog.Logger log;
+        private Stopwatch stopwatch;
+        private uint PresentCounter;
         public Application() {
             this.log = NLog.LogManager.GetCurrentClassLogger();
             this.sm = new StateMachine();
             this.challenge = new ChallengeFactory();
             this.challenge.PublisherSubscriber.ConnectionStateChanged += this.PublisherSubscriber_ConnectionStateChanged;
             this.challenge.PublisherSubscriber.Connect("localhost", "ADIS", "TEST");
+            this.challenge.RobotMobile.Status.StatusChanged += Status_StatusChanged;
+            this.challenge.RobotMobile.Status.PresentChanged += Status_PresentChanged;
             Joystick joystick = new Joystick();
             joystick.JoystickChanged += Joystick_JoystickChanged;
+            this.stopwatch = new Stopwatch();
+            this.PresentCounter = 0;
+        }
+
+        public void Status_StatusChanged(object? sender, StatusEventArgs e) {
+            try {
+                if (e.Status == RoboStatus.Auto) {
+                    this.sm.MoveNext(Command.autoMode);
+                    this.smHandler();
+                }
+                if(e.Status == RoboStatus.Final) {
+                    this.sm.MoveNext(Command.finished);
+                    this.smHandler();
+                }
+                if(e.Status == RoboStatus.Failure) {
+                    this.sm.MoveNext(Command.failure);
+                    this.smHandler();
+                }
+            }catch(Exception ex) {
+                this.log.Error(ex.Message);
+            }
+        }
+
+        public void Status_PresentChanged(object? sender, PresentEventArgs e) { 
+            if(this.sm.CurrentState == ProcessState.Auto) {
+                this.PresentCounter++;
+                this.challenge.RobotStationary.SplitFlap.Display(this.PresentCounter.ToString().PadLeft(4,'0'));
+            }
         }
 
         public void Run() {
@@ -27,8 +61,12 @@ namespace RaspiControl {
             if (!success) {
                 throw new Exception("Could not connect to broker on localhost");
             } else {
-                this.sm.MoveNext(Command.Connected);
+                try {
-                this.log.Info($"Current State: {this.sm.CurrentState}");
+                    this.sm.MoveNext(Command.Connected);
+                    this.log.Info($"Current State: {this.sm.CurrentState}");
+                } catch (Exception ex) {
+                    this.log.Error(ex.Message);
+                }
             }
         }
 
@@ -53,6 +91,10 @@ namespace RaspiControl {
                     break;
                 case ProcessState.StartMoveManual:
                     // mode automatic:false
+                    if (this.stopwatch.IsRunning) {
+                        this.stopwatch.Stop();
+                    }
+                    this.stopwatch.Start();
                     this.challenge.RobotStationary.SplitFlap.Display("WALD");
                     this.challenge.RobotMobile.Movement.SetMobilityMode(false);
                     this.challenge.RobotMobile.Movement.AddSpeed(NavigationConstants.SPEED_FORWARD);
@@ -89,35 +131,47 @@ namespace RaspiControl {
                     this.challenge.RobotMobile.Movement.Stop();
                     break;
                 case ProcessState.Auto:
+                    this.challenge.RobotStationary.SplitFlap.Display("AUTO");
                     break;
                 case ProcessState.Final:
+                    this.PresentCounter = 0;
+                    this.stopwatch.Stop();
+                    long elapsedTime = stopwatch.ElapsedMilliseconds / 1000;
+                    this.challenge.RobotStationary.SplitFlap.Display(elapsedTime.ToString().PadLeft(4,'0'));
                     break;
                 case ProcessState.Error:
+                    //this.PresentCounter = 0;
                     break;
             }
         }
 
         private void Joystick_JoystickChanged(object sender, JoystickEventArgs e) {
-            switch (e.Button) {
+            try {
-                case JoystickButton.None:
+                switch (e.Button) {
-                    break;
+                    case JoystickButton.None:
-                case JoystickButton.Left:
+                        break;
-                    this.sm.MoveNext(Command.JoystickLeft);
+                    case JoystickButton.Left:
-                    break;
+                        this.sm.MoveNext(Command.JoystickLeft);
-                case JoystickButton.Right:
+                        break;
-                    this.sm.MoveNext(Command.JoystickRight);
+                    case JoystickButton.Right:
-                    break;
+                        this.sm.MoveNext(Command.JoystickRight);
-                case JoystickButton.Up:
+                        break;
-                    this.sm.MoveNext(Command.JoystickUp); 
+                    case JoystickButton.Up:
-                    break;
+                        this.sm.MoveNext(Command.JoystickUp);
-                case JoystickButton.Down:
+                        break;
-                    this.sm.MoveNext(Command.JoystickDown);
+                    case JoystickButton.Down:
-                    break;
+                        this.sm.MoveNext(Command.JoystickDown);
-                case JoystickButton.Center:
+                        break;
-                    this.sm.MoveNext(Command.JoystickCenter);
+                    case JoystickButton.Center:
-                    break;
+                        this.sm.MoveNext(Command.JoystickCenter);
+                        break;
+                }
+                if(e.Button != JoystickButton.None) {
+                    this.smHandler();
+                }
+            } catch (Exception ex) {
+                this.log.Error(ex.Message);
             }
-            this.smHandler();
         }
     }
 }

ADIS_Csharp/RaspiControl/RaspiControl.csproj

@@ -8,7 +8,7 @@
   </PropertyGroup>
 
   <Target Name="PostBuild" AfterTargets="PostBuildEvent">
-    <Exec Command="cd &quot;$(TargetDir)&quot;&#xD;&#xA;&quot;$(SolutionDir)\SecureUpload.exe&quot; . pi-hslu:ADIS/$(ProjectName)" />
+    <Exec Command="cd &quot;$(TargetDir)&quot;&#xD;&#xA;&quot;$(SolutionDir)\SecureUpload.exe&quot; . pi-home:ADIS/$(ProjectName)" />
   </Target>
 
   <ItemGroup>

ADIS_Csharp/RaspiControl/StateMachine.cs

@@ -31,6 +31,8 @@ namespace RaspiControl {
         JoystickRight,
         JoystickCenter,
         autoMode,
+        finished,
+        failure,
         none
     }
     internal class StateMachine {
@@ -69,28 +71,48 @@ namespace RaspiControl {
                     {new StateTransition(ProcessState.Ready,Command.JoystickLeft), ProcessState.StartMoveManual },
                     {new StateTransition(ProcessState.Ready,Command.JoystickRight), ProcessState.StartMoveManual },
 
+
                     {new StateTransition(ProcessState.StartMoveManual,Command.none), ProcessState.MoveManual },
+                    {new StateTransition(ProcessState.StartMoveManual, Command.autoMode), ProcessState.Auto },
 
                     {new StateTransition(ProcessState.MoveManualF, Command.none), ProcessState.MoveManual },
+                    {new StateTransition(ProcessState.MoveManualF, Command.autoMode), ProcessState.Auto },
                     {new StateTransition(ProcessState.MoveManualB, Command.none), ProcessState.MoveManual },
+                    {new StateTransition(ProcessState.MoveManualB, Command.autoMode), ProcessState.Auto },
                     {new StateTransition(ProcessState.MoveManualL, Command.none), ProcessState.MoveManual },
+                    {new StateTransition(ProcessState.MoveManualL, Command.autoMode), ProcessState.Auto },
                     {new StateTransition(ProcessState.MoveManualR, Command.none), ProcessState.MoveManual },
+                    {new StateTransition(ProcessState.MoveManualR, Command.autoMode), ProcessState.Auto },
 
                     {new StateTransition(ProcessState.MoveManual, Command.JoystickUp), ProcessState.MoveManualF },
                     {new StateTransition(ProcessState.MoveManual, Command.JoystickDown), ProcessState.MoveManualB },
                     {new StateTransition(ProcessState.MoveManual, Command.JoystickLeft), ProcessState.MoveManualL },
                     {new StateTransition(ProcessState.MoveManual, Command.JoystickRight), ProcessState.MoveManualR },
                     {new StateTransition(ProcessState.MoveManual, Command.JoystickCenter), ProcessState.StopManual },
+                    {new StateTransition(ProcessState.MoveManual, Command.autoMode), ProcessState.Auto },
 
                     {new StateTransition(ProcessState.StopManual,Command.JoystickUp), ProcessState.ResumeMoveManual },
                     {new StateTransition(ProcessState.StopManual,Command.JoystickDown), ProcessState.ResumeMoveManual },
                     {new StateTransition(ProcessState.StopManual,Command.JoystickLeft), ProcessState.ResumeMoveManual },
                     {new StateTransition(ProcessState.StopManual,Command.JoystickRight), ProcessState.ResumeMoveManual },
+                    {new StateTransition(ProcessState.StopManual, Command.autoMode), ProcessState.Auto },
 
                     {new StateTransition(ProcessState.ResumeMoveManual, Command.none),ProcessState.MoveManual },
-
+                    {new StateTransition(ProcessState.ResumeMoveManual, Command.autoMode), ProcessState.Auto },
-                    {new StateTransition(ProcessState.MoveManual, Command.autoMode), ProcessState.Auto },
+
-                    
+                    {new StateTransition(ProcessState.Auto, Command.finished), ProcessState.Final },
+                    {new StateTransition(ProcessState.Auto, Command.JoystickUp), ProcessState.StartMoveManual },
+                    {new StateTransition(ProcessState.Auto, Command.JoystickDown), ProcessState.StartMoveManual },
+                    {new StateTransition(ProcessState.Auto, Command.JoystickLeft), ProcessState.StartMoveManual },
+                    {new StateTransition(ProcessState.Auto, Command.JoystickRight), ProcessState.StartMoveManual },
+                    {new StateTransition(ProcessState.Auto, Command.JoystickCenter), ProcessState.StartMoveManual },
+                    {new StateTransition(ProcessState.Auto, Command.failure), ProcessState.Error },
+
+                    {new StateTransition(ProcessState.Error, Command.JoystickUp), ProcessState.StartMoveManual },
+                    {new StateTransition(ProcessState.Error, Command.JoystickDown), ProcessState.StartMoveManual },
+                    {new StateTransition(ProcessState.Error, Command.JoystickLeft), ProcessState.StartMoveManual },
+                    {new StateTransition(ProcessState.Error, Command.JoystickRight), ProcessState.StartMoveManual },
+                    {new StateTransition(ProcessState.Error, Command.JoystickCenter), ProcessState.StartMoveManual },
                 };
         }
 

ADIS_Csharp/RobotLib/Robot.cs

@@ -3,6 +3,7 @@ using RobotLib.SplitFlap;
 using RobotLib.Movement;
 using RobotLib.Battery;
 using System.Timers;
+using RobotLib.Status;
 
 namespace RobotLib
 {
@@ -23,6 +24,7 @@ namespace RobotLib
                 Battery = new DevBattery(Com);
                 LineSensor = new DevLineSensor(com);
                 Movement = new DevMovement(com);
+                Status = new DevStatus(com);
             }
             else if(type == RobotMode.Stationary)
             {
@@ -46,6 +48,8 @@ namespace RobotLib
         public DevMovement Movement { get; }
         public DevLineSensor LineSensor { get; }
 
+        public DevStatus Status { get; }
+
         public void Connect(string host, int port)
         {
             //Com.Connect(host, port);

ADIS_Csharp/RobotLib/Status/DevStatus.cs

@@ -0,0 +1,37 @@
+using RobotLib.Communication;
+using RobotLib.SplitFlap;
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Text.Json;
+using System.Threading.Tasks;
+
+namespace RobotLib.Status {
+    public class DevStatus: DevBase {
+        private const string TOPIC_STATUS_MODE = "/mobile/status/mode/";
+        private const string TOPIC_STATUS_PRESENT = "/mobile/status/present/";
+
+        public event EventHandler<StatusEventArgs> StatusChanged;
+        public event EventHandler<PresentEventArgs> PresentChanged;
+
+        public DevStatus(IPublisherSubscriber com) : base(com, new List<string>() { TOPIC_STATUS_MODE, TOPIC_STATUS_PRESENT }) { }
+
+
+        protected override void ParseMessage(string fromTopic, string message) {
+            if (fromTopic == TOPIC_STATUS_MODE) {
+                StatusEventArgs eventArgs = new(message);
+                StatusChanged?.Invoke(this, eventArgs);
+            }else if(fromTopic == TOPIC_STATUS_PRESENT) {
+                PresentEventArgs presentEventArgs = new(true);
+                PresentChanged?.Invoke(this, presentEventArgs);
+            }
+        }
+
+        private T GetValueFromMesage<T>(string parameter, string message) {
+            var data = JsonSerializer.Deserialize<Dictionary<string, T>>(message);
+            data.TryGetValue(parameter, out T value);
+            return value;
+        }
+    }
+}

ADIS_Csharp/RobotLib/Status/PresentEventArgs.cs

@@ -0,0 +1,15 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace RobotLib.Status {
+    public class PresentEventArgs {
+        public bool Present { get; }
+
+        public PresentEventArgs(bool present) {
+            Present = present;
+        }
+    }
+}

ADIS_Csharp/RobotLib/Status/RoboStatus.cs

@@ -0,0 +1,15 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace RobotLib.Status {
+    
+    public static class RoboStatus {
+        public static readonly string Auto = "AUTO";
+        public static readonly string Manual = "MANUAL";
+        public static readonly string Final = "FINAL";
+        public static readonly string Failure = "FAILURE";
+    }
+}

ADIS_Csharp/RobotLib/Status/StatusEventArgs.cs

@@ -0,0 +1,15 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace RobotLib.Status {
+    public class StatusEventArgs {
+        public string Status { get; }
+
+        public StatusEventArgs(string status) {
+            Status = status;
+        }
+    }
+}