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;
+ }
+ }
+}
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;
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;
}
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