diff --git a/ADIS_Csharp/RaspiControl/Application.cs b/ADIS_Csharp/RaspiControl/Application.cs new file mode 100644 index 0000000..560bbd3 --- /dev/null +++ b/ADIS_Csharp/RaspiControl/Application.cs @@ -0,0 +1,123 @@ +using NLog.LayoutRenderers; +using System; +using System.Collections.Generic; +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; + 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"); + Joystick joystick = new Joystick(); + joystick.JoystickChanged += Joystick_JoystickChanged; + } + + public void Run() { + bool success = this.challenge.PublisherSubscriber.Connect("localhost", "ADIS", "TEST"); + if (!success) { + throw new Exception("Could not connect to broker on localhost"); + } else { + this.sm.MoveNext(Command.Connected); + this.log.Info($"Current State: {this.sm.CurrentState}"); + } + } + + private void PublisherSubscriber_ConnectionStateChanged(object? sender, bool e) { + if (e == false) { + this.log.Error("Connection to MQTT broker lost"); + } else { + this.log.Info("Connected to MQTT broker"); + } + } + + private void smHandler() { + this.log.Info($"Current State: {this.sm.CurrentState}"); + switch (this.sm.CurrentState) { + case ProcessState.Startup: + break; + case ProcessState.Init: + break; + case ProcessState.Ready: + // show sf redy + this.challenge.RobotStationary.SplitFlap.Display("REDY"); + break; + case ProcessState.StartMoveManual: + // mode automatic:false + this.challenge.RobotStationary.SplitFlap.Display("WALD"); + this.challenge.RobotMobile.Movement.SetMobilityMode(false); + this.challenge.RobotMobile.Movement.AddSpeed(NavigationConstants.SPEED_FORWARD); + this.sm.MoveNext(Command.none); + break; + case ProcessState.ResumeMoveManual: + this.challenge.RobotMobile.Movement.SetMobilityMode(false); + this.challenge.RobotMobile.Movement.AddSpeed(NavigationConstants.SPEED_FORWARD); + this.sm.MoveNext(Command.none); + break; + case ProcessState.MoveManual: + break; + case ProcessState.MoveManualF: + // move + 100 + this.challenge.RobotMobile.Movement.AddSpeed(NavigationConstants.SPEED_FORWARD); + this.sm.MoveNext(Command.none); + break; + case ProcessState.MoveManualB: + // move -100 + this.challenge.RobotMobile.Movement.AddSpeed(NavigationConstants.SPEED_BACKWARD); + this.sm.MoveNext(Command.none); + break; + case ProcessState.MoveManualL: + this.challenge.RobotMobile.Movement.Turn(NavigationConstants.TURN_ANGLE_LEFT); + this.sm.MoveNext(Command.none); + break; + case ProcessState.MoveManualR: + // turn right 30 + this.challenge.RobotMobile.Movement.Turn(NavigationConstants.TURN_ANGLE_RIGHT); + this.sm.MoveNext(Command.none); + break; + case ProcessState.StopManual: + // stop + this.challenge.RobotMobile.Movement.Stop(); + break; + case ProcessState.Auto: + break; + case ProcessState.Final: + break; + case ProcessState.Error: + break; + } + } + + private void Joystick_JoystickChanged(object sender, JoystickEventArgs e) { + switch (e.Button) { + case JoystickButton.None: + break; + case JoystickButton.Left: + this.sm.MoveNext(Command.JoystickLeft); + break; + case JoystickButton.Right: + this.sm.MoveNext(Command.JoystickRight); + break; + case JoystickButton.Up: + this.sm.MoveNext(Command.JoystickUp); + break; + case JoystickButton.Down: + this.sm.MoveNext(Command.JoystickDown); + break; + case JoystickButton.Center: + this.sm.MoveNext(Command.JoystickCenter); + break; + } + this.smHandler(); + } + } +} diff --git a/ADIS_Csharp/RaspiControl/ChallengeFactory.cs b/ADIS_Csharp/RaspiControl/ChallengeFactory.cs new file mode 100644 index 0000000..941a357 --- /dev/null +++ b/ADIS_Csharp/RaspiControl/ChallengeFactory.cs @@ -0,0 +1,21 @@ +using RobotLib.Communication; +using RobotLib; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace RaspiControl { + internal class ChallengeFactory { + public MqttPublisherSubscriber PublisherSubscriber { get; } + public Robot RobotStationary { get; } + public Robot RobotMobile { get; } + + public ChallengeFactory() { + this.PublisherSubscriber = MqttPublisherSubscriber.Instance; + this.RobotStationary = new Robot(this.PublisherSubscriber); + this.RobotMobile = new Robot(this.PublisherSubscriber); + } + } +} diff --git a/ADIS_Csharp/RaspiControl/Joystick.cs b/ADIS_Csharp/RaspiControl/Joystick.cs index 8f84b83..7a7bca8 100644 --- a/ADIS_Csharp/RaspiControl/Joystick.cs +++ b/ADIS_Csharp/RaspiControl/Joystick.cs @@ -32,13 +32,14 @@ namespace RaspiControl { public event EventHandler? JoystickChanged; private void OnJoystickChanged(JoystickButton state) { - Console.WriteLine($"Joystick was pushed: {state}"); + this.log.Trace($"Joystick was pushed: {state}"); this.JoystickChanged?.Invoke(this, new JoystickEventArgs(state)); } #endregion - + private readonly NLog.Logger log; public Joystick() { Pi.Init(); + this.log = NLog.LogManager.GetCurrentClassLogger(); foreach (var pin in inputPins) { pin.PinMode = GpioPinDriveMode.Input; } diff --git a/ADIS_Csharp/RaspiControl/NLog.config b/ADIS_Csharp/RaspiControl/NLog.config new file mode 100644 index 0000000..6c004ed --- /dev/null +++ b/ADIS_Csharp/RaspiControl/NLog.config @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/ADIS_Csharp/RaspiControl/Program.cs b/ADIS_Csharp/RaspiControl/Program.cs index 5c36bfe..5df5de7 100644 --- a/ADIS_Csharp/RaspiControl/Program.cs +++ b/ADIS_Csharp/RaspiControl/Program.cs @@ -5,108 +5,16 @@ using System.Text.Json; namespace RaspiControl { class Programm { - private static MqttClient client; + private static readonly NLog.Logger log = NLog.LogManager.GetCurrentClassLogger(); static void Main(string[] args) { try { - client = new MqttClient("localhost"); - client.MqttMsgPublishReceived += Client_MqttMsgPublishReceived; - string clientId = Guid.NewGuid().ToString(); - client.Connect(clientId); - client.Subscribe(new string[] { MqttConstants.DEVICE_STATUS_APP_TOPIC }, new byte[] { MqttMsgBase.QOS_LEVEL_AT_MOST_ONCE }); - Joystick joystick = new Joystick(); - joystick.JoystickChanged += Joystick_JoystickChanged; + Application app = new Application(); + app.Run(); } catch (Exception ex) { - Console.WriteLine(ex.Message); + log.Error(ex); } } - - private static void Joystick_JoystickChanged(object sender, JoystickEventArgs e) { - switch (e.Button) { - case JoystickButton.None: - break; - case JoystickButton.Left: - client.Publish(MqttConstants.MOBILE_NAV_TURN_TOPIC, Encoding.UTF8.GetBytes($"{NavigationConstants.TURN_ANGLE_LEFT}"), MqttMsgBase.QOS_LEVEL_AT_MOST_ONCE, false); - break; - case JoystickButton.Right: - client.Publish(MqttConstants.MOBILE_NAV_TURN_TOPIC, Encoding.UTF8.GetBytes($"{NavigationConstants.TURN_ANGLE_RIGHT}"), MqttMsgBase.QOS_LEVEL_AT_MOST_ONCE, false); - break; - case JoystickButton.Up: - client.Publish(MqttConstants.MOBILE_NAV_MOVE_TOPIC, Encoding.UTF8.GetBytes($"{NavigationConstants.SPEED_FORWARD}"), MqttMsgBase.QOS_LEVEL_AT_MOST_ONCE, false); - break; - case JoystickButton.Down: - client.Publish(MqttConstants.MOBILE_NAV_MOVE_TOPIC, Encoding.UTF8.GetBytes($"{NavigationConstants.SPEED_BACKWARD}"), MqttMsgBase.QOS_LEVEL_AT_MOST_ONCE, false); - break; - case JoystickButton.Center: - client.Publish(MqttConstants.MOBILE_NAV_STOP_TOPIC, Encoding.UTF8.GetBytes($"{NavigationConstants.STOP}"), MqttMsgBase.QOS_LEVEL_EXACTLY_ONCE, false); - break; - } - } - - private static void PublishSplitFlapDisplay(string message) { - string payload = JsonSerializer.Serialize>(new Dictionary() { { "message", message } }); - client.Publish(MqttConstants.SPLITFLAP_DISPLAY_TOPIC, Encoding.UTF8.GetBytes(payload),MqttMsgBase.QOS_LEVEL_AT_MOST_ONCE,false); - } - private static void Client_MqttMsgPublishReceived(object sender, MqttMsgPublishEventArgs e) { - string deviceId = ""; - Dictionary data; - if (e.Topic.StartsWith(string.Join('/',MqttConstants.DEVICE_STATUS_APP_TOPIC.Split('/').Take(2)))) { - if(e.Topic.EndsWith(MqttConstants.DEVICE_STATUS_APP_TOPIC.Split('/').Last())) { - deviceId = deviceIdFromStatusTopic(e.Topic); - if(deviceId == "mobile") { - try { - data = getValueFromTopic(Encoding.UTF8.GetString(e.Message)); - object value; - data.TryGetValue("state", out value); - if (value != null) { - APP_STATE appState = (APP_STATE)Convert.ToUInt16(value.ToString()); - switch (appState) { - case APP_STATE.STARTUP: - Console.WriteLine("Startup"); - client.Publish(MqttConstants.SPLITFLAP_INIT_TOPIC, new byte[] { }, MqttMsgBase.QOS_LEVEL_AT_MOST_ONCE, false); - break; - case APP_STATE.INIT: - Console.WriteLine("Init"); - PublishSplitFlapDisplay("INIT"); - break; - case APP_STATE.CALIBRATE: - break; - case APP_STATE.FOLLOW_LINE: - Console.WriteLine("Follow line"); - PublishSplitFlapDisplay("AUTO"); - break; - case APP_STATE.IDLE: - break; - case APP_STATE.FINAL: - break; - case APP_STATE.READY: - Console.WriteLine("ready"); - PublishSplitFlapDisplay("REDY"); - break; - default: - throw new ArgumentException(); - } - } - } catch(Exception) { - Console.WriteLine($"Invalid payload received: {Encoding.UTF8.GetString(e.Message)}"); - } - - - } - else { - Console.WriteLine($"on topic: {e.Topic} no key with state found! | {Encoding.UTF8.GetString(e.Message)}"); - } - } - } - } - - private static string deviceIdFromStatusTopic(string statusTopic) { - string[] topic = statusTopic.Split('/'); - return topic[topic.Length - 2]; - } - private static Dictionary getValueFromTopic(string topic) { - return JsonSerializer.Deserialize>(topic); - } } } \ No newline at end of file diff --git a/ADIS_Csharp/RaspiControl/RaspiControl.csproj b/ADIS_Csharp/RaspiControl/RaspiControl.csproj index f1edd23..d41197f 100644 --- a/ADIS_Csharp/RaspiControl/RaspiControl.csproj +++ b/ADIS_Csharp/RaspiControl/RaspiControl.csproj @@ -17,4 +17,14 @@ + + + + + + + PreserveNewest + + + diff --git a/ADIS_Csharp/RaspiControl/StateMachine.cs b/ADIS_Csharp/RaspiControl/StateMachine.cs new file mode 100644 index 0000000..396cf85 --- /dev/null +++ b/ADIS_Csharp/RaspiControl/StateMachine.cs @@ -0,0 +1,111 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace RaspiControl { + public enum ProcessState { + Startup, + Init, + Ready, + StartMoveManual, + ResumeMoveManual, + MoveManual, + MoveManualF, + MoveManualB, + MoveManualL, + MoveManualR, + StopManual, + Auto, + Final, + Error + } + + public enum Command { + Connected, + InitDone, + JoystickUp, + JoystickDown, + JoystickLeft, + JoystickRight, + JoystickCenter, + autoMode, + none + } + internal class StateMachine { + + class StateTransition { + readonly ProcessState CurrentState; + readonly Command Command; + + public StateTransition(ProcessState currentState, Command command) { + CurrentState = currentState; + Command = command; + } + + public override int GetHashCode() { + return 17 + 31 * CurrentState.GetHashCode() + 31 * Command.GetHashCode(); + } + + public override bool Equals(object obj) { + StateTransition other = obj as StateTransition; + return other != null && this.CurrentState == other.CurrentState && this.Command == other.Command; + } + } + + Dictionary transitions; + public ProcessState CurrentState { get; private set; } + + public StateMachine() { + CurrentState = ProcessState.Startup; + transitions = new Dictionary { + {new StateTransition(ProcessState.Startup,Command.Connected), ProcessState.Init }, + + {new StateTransition(ProcessState.Init,Command.JoystickCenter), ProcessState.Ready }, + + {new StateTransition(ProcessState.Ready,Command.JoystickUp), ProcessState.StartMoveManual }, + {new StateTransition(ProcessState.Ready,Command.JoystickDown), ProcessState.StartMoveManual }, + {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.MoveManualF, Command.none), ProcessState.MoveManual }, + {new StateTransition(ProcessState.MoveManualB, Command.none), ProcessState.MoveManual }, + {new StateTransition(ProcessState.MoveManualL, Command.none), ProcessState.MoveManual }, + {new StateTransition(ProcessState.MoveManualR, Command.none), ProcessState.MoveManual }, + + {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.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.ResumeMoveManual, Command.none),ProcessState.MoveManual }, + + {new StateTransition(ProcessState.MoveManual, Command.autoMode), ProcessState.Auto }, + + }; + } + + public ProcessState GetNext(Command command) { + StateTransition transition = new StateTransition(CurrentState, command); + ProcessState nextState; + if (!transitions.TryGetValue(transition, out nextState)) + throw new Exception("Invalid transition: " + CurrentState + " -> " + command); + return nextState; + } + + public ProcessState MoveNext(Command command) { + CurrentState = GetNext(command); + return CurrentState; + } + + } +}