jonas
/
ADIS_Projects
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add statemachine, use RobotLib and logger

Browse Source
main
Simon Frei 4 years ago
parent a3a2d14328
commit 900ff1ae96
7 changed files with 309 additions and 98 deletions
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Show Stats Download Patch File Download Diff File
  1. 123
      ADIS_Csharp/RaspiControl/Application.cs
  2. 21
      ADIS_Csharp/RaspiControl/ChallengeFactory.cs
  3. 5
      ADIS_Csharp/RaspiControl/Joystick.cs
  4. 37
      ADIS_Csharp/RaspiControl/NLog.config
  5. 100
      ADIS_Csharp/RaspiControl/Program.cs
  6. 10
      ADIS_Csharp/RaspiControl/RaspiControl.csproj
  7. 111
      ADIS_Csharp/RaspiControl/StateMachine.cs

123
ADIS_Csharp/RaspiControl/Application.cs
Unescape View File

@ -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();
}
}
}

21
ADIS_Csharp/RaspiControl/ChallengeFactory.cs
Unescape View File

@ -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);
}
}
}

5
ADIS_Csharp/RaspiControl/Joystick.cs
Unescape View File

@ -32,13 +32,14 @@ namespace RaspiControl {
public event EventHandler<JoystickEventArgs>? 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<BootstrapWiringPi>();
this.log = NLog.LogManager.GetCurrentClassLogger();
foreach (var pin in inputPins) {
pin.PinMode = GpioPinDriveMode.Input;
}

37
ADIS_Csharp/RaspiControl/NLog.config
Unescape View File

@ -0,0 +1,37 @@
<?xml version="1.0" encoding="utf-8" ?>
<nlog xmlns="http://www.nlog-project.org/schemas/NLog.xsd"
xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
autoReload="true">
<targets>
<target xsi:type="File"
name="logfile"
fileName="${processname}_${date:format=yy-MM-dd}.log"
archiveEvery="Day"
archiveNumbering="Rolling"
maxArchiveFiles="14"
concurrentWrites="true"
keepFileOpen="true"
layout="${date:format=dd.MM.yy HH\:mm\:ss.fff} [${threadid}] ${level:uppercase=true:padding=-5} ${logger:shortName=false} - ${message}"
header="${processname} v.${assembly-version} started on ${date:format=dd.MM.yy HH\:mm\:ss}. Machine: ${machinename}. User: ${identity}"
footer="${processname} v.${assembly-version} stopped on ${date:format=dd.MM.yy HH\:mm\:ss}. Machine: ${machinename}. User: ${identity}"/>
<target xsi:type="NLogViewer"
name="viewer"
includeSourceInfo="true"
newLine="true"
address="tcp4://127.0.0.1:9999"/>
<target xsi:type="ColoredConsole"
name="console"
layout="${date:format=dd.MM.yy HH\:mm\:ss.fff} [${threadid}] ${level:uppercase=true:padding=-5} ${logger:shortName=false} - ${message}"
header="${processname} v.${assembly-version} started on ${date:format=dd.MM.yy HH\:mm\:ss}. Machine: ${machinename}. User: ${identity}"
footer="${processname} v.${assembly-version} stopped on ${date:format=dd.MM.yy HH\:mm\:ss}. Machine: ${machinename}. User: ${identity}"
/>
</targets>
<rules>
<logger name="*" minlevel="Trace" writeTo="console" />
<logger name="*" minlevel="Trace" writeTo="logfile" />
<logger name="*" minlevel="Trace" writeTo="viewer" />
</rules>
</nlog>

100
ADIS_Csharp/RaspiControl/Program.cs
Unescape View File

@ -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<Dictionary<string, string>>(new Dictionary<string, string>() { { "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<string, object> 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<string, object> getValueFromTopic(string topic) {
return JsonSerializer.Deserialize<Dictionary<string, object>>(topic);
}
}
}

10
ADIS_Csharp/RaspiControl/RaspiControl.csproj
Unescape View File

@ -17,4 +17,14 @@
<PackageReference Include="Unosquare.WiringPi" Version="0.5.1" />
</ItemGroup>
<ItemGroup>
<ProjectReference Include="..\RobotLib\RobotLib.csproj" />
</ItemGroup>
<ItemGroup>
<None Update="NLog.config">
<CopyToOutputDirectory>PreserveNewest</CopyToOutputDirectory>
</None>
</ItemGroup>
</Project>

111
ADIS_Csharp/RaspiControl/StateMachine.cs
Unescape View File

@ -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<StateTransition, ProcessState> transitions;
public ProcessState CurrentState { get; private set; }
public StateMachine() {
CurrentState = ProcessState.Startup;
transitions = new Dictionary<StateTransition, ProcessState> {
{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;
}
}
}
Write Preview
Loading…
Cancel
Save