From 3b900f1d8a7d9810cae08d9d66c7d7d03b7d7e25 Mon Sep 17 00:00:00 2001 From: Simon Frei Date: Sun, 18 Dec 2022 10:32:06 +0100 Subject: [PATCH 1/4] use tuned params (pid and turn) --- ADIS_Sumo/Sumo/Pid.c | 10 +++++----- ADIS_Sumo/Sumo/Turn.c | 6 +++--- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/ADIS_Sumo/Sumo/Pid.c b/ADIS_Sumo/Sumo/Pid.c index aa8b125..062c482 100644 --- a/ADIS_Sumo/Sumo/Pid.c +++ b/ADIS_Sumo/Sumo/Pid.c @@ -630,11 +630,11 @@ void PID_Init(void) { lineFwConfig.lastError = 0; lineFwConfig.integral = 0; #elif PL_IS_INTRO_ZUMO_ROBOT2 || PL_IS_INTRO_ZUMO_K22 - lineFwConfig.pFactor100 = 5500; + lineFwConfig.pFactor100 = 4000;//5500; lineFwConfig.iFactor100 = 15; - lineFwConfig.dFactor100 = 100; + lineFwConfig.dFactor100 = 500;//100; lineFwConfig.iAntiWindup = 100000; - lineFwConfig.maxSpeedPercent = 40; + lineFwConfig.maxSpeedPercent = 25;//40; lineFwConfig.lastError = 0; lineFwConfig.integral = 0; #else @@ -672,11 +672,11 @@ void PID_Init(void) { posLeftConfig.iAntiWindup = 200; posLeftConfig.maxSpeedPercent = 40; #elif (PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE) /* line/maze, high speed */ - posLeftConfig.pFactor100 = 1000; + posLeftConfig.pFactor100 = 2000;//1000; posLeftConfig.iFactor100 = 2; posLeftConfig.dFactor100 = 50; posLeftConfig.iAntiWindup = 200; - posLeftConfig.maxSpeedPercent = 40; + posLeftConfig.maxSpeedPercent = 35;//40; #else /* defaults */ posLeftConfig.pFactor100 = 1000; posLeftConfig.iFactor100 = 1; diff --git a/ADIS_Sumo/Sumo/Turn.c b/ADIS_Sumo/Sumo/Turn.c index 094f59e..966ce2b 100644 --- a/ADIS_Sumo/Sumo/Turn.c +++ b/ADIS_Sumo/Sumo/Turn.c @@ -63,11 +63,11 @@ #define TURN_STEPS_POST_LINE_TIMEOUT_MS 500 #define TURN_STEPS_STOP_TIMEOUT_MS 150 #elif PL_IS_INTRO_ZUMO_K22 /* no LiPo */ - #define TURN_STEPS_90 720 + #define TURN_STEPS_90 650//720 /*!< number of steps for a 90 degree turn */ - #define TURN_STEPS_LINE 100 /*230*/ + #define TURN_STEPS_LINE 170//100 /*230*/ /*!< number of steps stepping over the line */ - #define TURN_STEPS_POST_LINE 150 + #define TURN_STEPS_POST_LINE 240//150 /*!< number of steps after the line, before making a turn */ #define TURN_STEPS_90_TIMEOUT_MS 1000 #define TURN_STEPS_LINE_TIMEOUT_MS 200 From cc9bb2860f687a8860740de9b308fc778a58d30a Mon Sep 17 00:00:00 2001 From: Simon Frei Date: Sun, 18 Dec 2022 14:20:46 +0100 Subject: [PATCH 2/4] Set App state to manual drive on set mobility mode automatic=false --- ADIS_ESP32_Eclipse/main/robo_wrapper.c | 1 + 1 file changed, 1 insertion(+) diff --git a/ADIS_ESP32_Eclipse/main/robo_wrapper.c b/ADIS_ESP32_Eclipse/main/robo_wrapper.c index 2a78d8e..db818ac 100644 --- a/ADIS_ESP32_Eclipse/main/robo_wrapper.c +++ b/ADIS_ESP32_Eclipse/main/robo_wrapper.c @@ -31,6 +31,7 @@ bool Robo_Wrapper_SetMode(bool automatic){ unsigned char cmd[BUF_SIZE] = RS_CMD_DRIVE_PREFIX; McuUtility_strcat(cmd, sizeof(cmd), (unsigned char*)"mode speed"); SHELL_SendToRobotAndGetResponse(cmd, response, sizeof(response)); + SHELL_SendToRobotAndGetResponse((const unsigned char*)"app manualdrive", response, sizeof(response)); McuShell_SendStr(response, McuShell_GetStdio()->stdOut); } return true; From bff452c03b2711bc25f2c32d0de98d359e3013e5 Mon Sep 17 00:00:00 2001 From: Simon Frei Date: Sun, 18 Dec 2022 14:22:07 +0100 Subject: [PATCH 3/4] add Status Topic to RoboLib, implement timer in RaspiControl and states based on status messages --- ADIS_Csharp/RaspiControl/Application.cs | 98 ++++++++++++++----- ADIS_Csharp/RaspiControl/RaspiControl.csproj | 2 +- ADIS_Csharp/RaspiControl/StateMachine.cs | 28 +++++- ADIS_Csharp/RobotLib/Robot.cs | 4 + ADIS_Csharp/RobotLib/Status/DevStatus.cs | 37 +++++++ .../RobotLib/Status/PresentEventArgs.cs | 15 +++ ADIS_Csharp/RobotLib/Status/RoboStatus.cs | 15 +++ .../RobotLib/Status/StatusEventArgs.cs | 15 +++ 8 files changed, 188 insertions(+), 26 deletions(-) create mode 100644 ADIS_Csharp/RobotLib/Status/DevStatus.cs create mode 100644 ADIS_Csharp/RobotLib/Status/PresentEventArgs.cs create mode 100644 ADIS_Csharp/RobotLib/Status/RoboStatus.cs create mode 100644 ADIS_Csharp/RobotLib/Status/StatusEventArgs.cs diff --git a/ADIS_Csharp/RaspiControl/Application.cs b/ADIS_Csharp/RaspiControl/Application.cs index 560bbd3..38d395a 100644 --- a/ADIS_Csharp/RaspiControl/Application.cs +++ b/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); - this.log.Info($"Current State: {this.sm.CurrentState}"); + try { + 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) { - 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; + try { + 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; + } + if(e.Button != JoystickButton.None) { + this.smHandler(); + } + } catch (Exception ex) { + this.log.Error(ex.Message); } - this.smHandler(); } } } diff --git a/ADIS_Csharp/RaspiControl/RaspiControl.csproj b/ADIS_Csharp/RaspiControl/RaspiControl.csproj index d41197f..4ebbd9f 100644 --- a/ADIS_Csharp/RaspiControl/RaspiControl.csproj +++ b/ADIS_Csharp/RaspiControl/RaspiControl.csproj @@ -8,7 +8,7 @@ - + diff --git a/ADIS_Csharp/RaspiControl/StateMachine.cs b/ADIS_Csharp/RaspiControl/StateMachine.cs index 396cf85..e2f3b29 100644 --- a/ADIS_Csharp/RaspiControl/StateMachine.cs +++ b/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.MoveManual, Command.autoMode), ProcessState.Auto }, - + {new StateTransition(ProcessState.ResumeMoveManual, 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 }, }; } diff --git a/ADIS_Csharp/RobotLib/Robot.cs b/ADIS_Csharp/RobotLib/Robot.cs index edb1f5f..1f66061 100644 --- a/ADIS_Csharp/RobotLib/Robot.cs +++ b/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); diff --git a/ADIS_Csharp/RobotLib/Status/DevStatus.cs b/ADIS_Csharp/RobotLib/Status/DevStatus.cs new file mode 100644 index 0000000..44ba3a2 --- /dev/null +++ b/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 StatusChanged; + public event EventHandler PresentChanged; + + public DevStatus(IPublisherSubscriber com) : base(com, new List() { 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(string parameter, string message) { + var data = JsonSerializer.Deserialize>(message); + data.TryGetValue(parameter, out T value); + return value; + } + } +} diff --git a/ADIS_Csharp/RobotLib/Status/PresentEventArgs.cs b/ADIS_Csharp/RobotLib/Status/PresentEventArgs.cs new file mode 100644 index 0000000..c10116f --- /dev/null +++ b/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; + } + } +} diff --git a/ADIS_Csharp/RobotLib/Status/RoboStatus.cs b/ADIS_Csharp/RobotLib/Status/RoboStatus.cs new file mode 100644 index 0000000..fc6f115 --- /dev/null +++ b/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"; + } +} diff --git a/ADIS_Csharp/RobotLib/Status/StatusEventArgs.cs b/ADIS_Csharp/RobotLib/Status/StatusEventArgs.cs new file mode 100644 index 0000000..cc191b6 --- /dev/null +++ b/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; + } + } +} From 734b8859b92fd06e8fcfab3887e3d5550f04f3d9 Mon Sep 17 00:00:00 2001 From: Simon Frei Date: Sun, 18 Dec 2022 14:22:45 +0100 Subject: [PATCH 4/4] publish status and presentcount --- ADIS_Sumo/Sumo/Application.c | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/ADIS_Sumo/Sumo/Application.c b/ADIS_Sumo/Sumo/Application.c index 6d1d187..201a71c 100644 --- a/ADIS_Sumo/Sumo/Application.c +++ b/ADIS_Sumo/Sumo/Application.c @@ -144,7 +144,9 @@ #if PL_HAS_MIDI #include "MidiMusic.h" #endif - +#if PL_CONFIG_USE_ESP32 +#include "McuESP32.h" +#endif typedef enum { APP_STATE_STARTUP, APP_STATE_INIT, @@ -371,23 +373,28 @@ static void StateMachine(bool buttonPress) { presentCnt = MAZE_GetPresentCount(); MAZE_ClearSolution(); LF_StartFollowing(); + McuShell_SendStr((unsigned char*)"mqtt publish \"/mobile/status/mode/\" \"AUTO\"", McuESP32_GetTxToESPStdio()->stdOut); + McuShell_SendStr((unsigned char*)"\r\n", McuESP32_GetTxToESPStdio()->stdOut); } break; case APP_STATE_AUTO: if(!LF_IsFollowing() && MAZE_IsSolved()){ //done SHELL_SendString((unsigned char*)"MAZE: done, stopped!!!\r\n"); + McuShell_SendStr((unsigned char*)"mqtt publish \"/mobile/status/mode/\" \"FINAL\"", McuESP32_GetTxToESPStdio()->stdOut); + McuShell_SendStr((unsigned char*)"\r\n", McuESP32_GetTxToESPStdio()->stdOut); appState = APP_STATE_IDLE; }else if(!LF_IsFollowing() && !MAZE_IsSolved() && mazeFailure < 5){ // failed mazeFailure++; - SHELL_SendString((unsigned char*)"MAZE: Failure, stopped OMG!!!\r\n"); MAZE_ClearSolution(); DRV_SetSpeed(0, 0); DRV_SetMode(DRV_MODE_SPEED); LF_StartFollowing(); }else if(!LF_IsFollowing() && !MAZE_IsSolved() && mazeFailure >= 5){ appState = APP_STATE_IDLE; + McuShell_SendStr((unsigned char*)"mqtt publish \"/mobile/status/mode/\" \"FAILURE\"", McuESP32_GetTxToESPStdio()->stdOut); + McuShell_SendStr((unsigned char*)"\r\n", McuESP32_GetTxToESPStdio()->stdOut); } if(MAZE_GetPresentCount() != presentCnt){ presentCnt = MAZE_GetPresentCount(); @@ -396,6 +403,8 @@ static void StateMachine(bool buttonPress) { McuUtility_Num8uToStr(counter, sizeof(counter), presentCnt); SHELL_SendString((unsigned char*)counter); SHELL_SendString((unsigned char*)"\r\n"); + McuShell_SendStr((unsigned char*)"mqtt publish \"/mobile/status/present/\" \"true\"", McuESP32_GetTxToESPStdio()->stdOut); + McuShell_SendStr((unsigned char*)"\r\n", McuESP32_GetTxToESPStdio()->stdOut); } break; default: @@ -499,6 +508,8 @@ static uint8_t AutoCalibrateReflectance(void) { static uint8_t SetManualMode(){ appState = APP_STATE_MANUAL_MOVE; + McuShell_SendStr((unsigned char*)"mqtt publish \"/mobile/status/mode/\" \"MANUAL\"", McuESP32_GetTxToESPStdio()->stdOut); + McuShell_SendStr((unsigned char*)"\r\n", McuESP32_GetTxToESPStdio()->stdOut); return ERR_OK; }