diff --git a/ADIS_Csharp/RaspiControl/AppState.cs b/ADIS_Csharp/RaspiControl/AppState.cs new file mode 100644 index 0000000..81b45b2 --- /dev/null +++ b/ADIS_Csharp/RaspiControl/AppState.cs @@ -0,0 +1,17 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace RaspiControl { + public enum APP_STATE { + STARTUP = 0, + INIT = 1, + CALIBRATE = 2, + READY = 3, + FOLLOW_LINE = 4, + IDLE = 5, + FINAL = 6, + } +} diff --git a/ADIS_Csharp/RaspiControl/MqttConstants.cs b/ADIS_Csharp/RaspiControl/MqttConstants.cs new file mode 100644 index 0000000..a8cc79a --- /dev/null +++ b/ADIS_Csharp/RaspiControl/MqttConstants.cs @@ -0,0 +1,20 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace RaspiControl { + internal static class MqttConstants { + #region Publish + public static string MOBILE_NAV_TURN_TOPIC = "mobile/cmd/nav/turn"; + public static string MOBILE_NAV_MOVE_TOPIC = "mobile/cmd/nav/move"; + public static string MOBILE_NAV_STOP_TOPIC = "mobile/cmd/nav/stop"; + public static string SPLITFLAP_DISPLAY = "splitFlap/cmd/display"; + #endregion + + #region subscribe + public static string DEVICE_STATUS_APP_TOPIC = "device/status/+/app"; + #endregion + } +} diff --git a/ADIS_Csharp/RaspiControl/Navigation.cs b/ADIS_Csharp/RaspiControl/Navigation.cs deleted file mode 100644 index 1510584..0000000 --- a/ADIS_Csharp/RaspiControl/Navigation.cs +++ /dev/null @@ -1,53 +0,0 @@ -using System; -using System.Collections.Generic; -using System.Linq; -using System.Text; -using System.Threading.Tasks; - -namespace RaspiControl { - internal class Navigation { - private int angleRight; - private int angleLeft; - public Navigation() { - this.angleRight = 0; - this.angleLeft = 0; - } - - public void ResetAngle() { - this.angleRight = 0; - this.angleRight= 0; - } - - public int GetAngleLeftStep() { - this.angleLeft -= 10; - if (this.angleLeft < -180) { - this.angleLeft = -180; - } - return -10; - } - - public int GetAngleRightStep() { - this.angleRight += 10; - if (this.angleRight > 180) { - this.angleRight = 180; - } - return 10; - } - - public int IncreaseAngleLeft() { - this.angleLeft -= 10; - if(this.angleLeft < -180) { - this.angleLeft = -180; - } - return this.angleLeft; - } - - public int IncreaseAngleRight() { - this.angleRight += 10; - if (this.angleRight > 180) { - this.angleRight = 180; - } - return this.angleRight; - } - } -} diff --git a/ADIS_Csharp/RaspiControl/NavigationConstants.cs b/ADIS_Csharp/RaspiControl/NavigationConstants.cs new file mode 100644 index 0000000..90ad6d4 --- /dev/null +++ b/ADIS_Csharp/RaspiControl/NavigationConstants.cs @@ -0,0 +1,15 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace RaspiControl { + internal static class NavigationConstants { + public static int TURN_ANGLE_LEFT = -10; + public static int TURN_ANGLE_RIGHT = 10; + public static int SPEED_FORWARD = 100; + public static int SPEED_BACKWARD = -100; + public static bool STOP = true; + } +} diff --git a/ADIS_Csharp/RaspiControl/Program.cs b/ADIS_Csharp/RaspiControl/Program.cs index 262d227..d81f287 100644 --- a/ADIS_Csharp/RaspiControl/Program.cs +++ b/ADIS_Csharp/RaspiControl/Program.cs @@ -2,22 +2,22 @@ using System.Text; using M2Mqtt; using M2Mqtt.Messages; -using Swan.Formatters; +using System.Text.Json; +using System.Linq; +using System.Runtime.CompilerServices; namespace RaspiControl { class Programm { private static MqttClient client; - private static Navigation navigation; - private static Joystick joystick; + 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[] { "APROG/REQUEST" }, new byte[] { MqttMsgBase.QOS_LEVEL_AT_MOST_ONCE }); - navigation= new Navigation(); - joystick= new Joystick(); + 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; } catch (Exception ex) { @@ -30,25 +30,81 @@ namespace RaspiControl { case JoystickButton.None: break; case JoystickButton.Left: - client.Publish("mobile/cmd/nav/left", Encoding.UTF8.GetBytes($"{navigation.GetAngleLeftStep()}"), MqttMsgBase.QOS_LEVEL_AT_MOST_ONCE, false); + 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("mobile/cmd/nav/right", Encoding.UTF8.GetBytes($"{navigation.GetAngleRightStep()}"), MqttMsgBase.QOS_LEVEL_AT_MOST_ONCE, false); + 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: - navigation.ResetAngle(); - client.Publish("mobile/cmd/nav/drive", Encoding.UTF8.GetBytes("true"), MqttMsgBase.QOS_LEVEL_AT_MOST_ONCE, false); + 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, Encoding.UTF8.GetBytes(payload),MqttMsgBase.QOS_LEVEL_AT_MOST_ONCE,false); + } private static void Client_MqttMsgPublishReceived(object sender, MqttMsgPublishEventArgs e) { - Console.Write(e.Topic + "\n"); - Console.Write(Encoding.UTF8.GetString(e.Message) + "\n"); + 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: + break; + case APP_STATE.INIT: + PublishSplitFlapDisplay("INIT"); + break; + case APP_STATE.CALIBRATE: + break; + case APP_STATE.FOLLOW_LINE: + PublishSplitFlapDisplay("AUTO"); + break; + case APP_STATE.IDLE: + break; + case APP_STATE.FINAL: + break; + case APP_STATE.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 b90fa41..f1edd23 100644 --- a/ADIS_Csharp/RaspiControl/RaspiControl.csproj +++ b/ADIS_Csharp/RaspiControl/RaspiControl.csproj @@ -1,4 +1,4 @@ - + Exe @@ -8,7 +8,7 @@ - + diff --git a/ADIS_Sumo_Styger/Sumo/AdoptToHW.c b/ADIS_Sumo_Styger/Sumo/AdoptToHW.c index 1fa1f10..15dce8f 100644 --- a/ADIS_Sumo_Styger/Sumo/AdoptToHW.c +++ b/ADIS_Sumo_Styger/Sumo/AdoptToHW.c @@ -62,6 +62,7 @@ static const RobotHW_Config RobotHWConfigTable[] = {.id=ID_ROBOT_R36, .version=ROBOT_HW_V2, .invertDirLeft=TRUE, .invertDirRight=TRUE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE }, {.id=ID_ROBOT_R37, .version=ROBOT_HW_V2, .invertDirLeft=TRUE, .invertDirRight=FALSE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE }, /* verified */ {.id=ID_ROBOT_R44, .version=ROBOT_HW_V2, .invertDirLeft=TRUE, .invertDirRight=TRUE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE }, /* verified */ + {.id=ID_ROBOT_R45, .version=ROBOT_HW_V2, .invertDirLeft=TRUE, .invertDirRight=FALSE, .swapQuadLeft=FALSE,.swapQuadRight=FALSE }, }; static const RobotHW_Config *GetRobotHWDesc(ID_Robot_e id) { diff --git a/ADIS_Sumo_Styger/Sumo/Drive.c b/ADIS_Sumo_Styger/Sumo/Drive.c index 6eba40c..e6e48dc 100644 --- a/ADIS_Sumo_Styger/Sumo/Drive.c +++ b/ADIS_Sumo_Styger/Sumo/Drive.c @@ -258,6 +258,7 @@ static void DRV_PrintHelp(const McuShell_StdIOType *io) { McuShell_SendHelpStr((unsigned char*)" mode ", (unsigned char*)"Set driving mode (none|stop|speed)\r\n", io->stdOut); #endif McuShell_SendHelpStr((unsigned char*)" speed ", (unsigned char*)"Move left and right motors with given speed\r\n", io->stdOut); + McuShell_SendHelpStr((unsigned char*)" incSpeed ", (unsigned char*)"Increase left and right motors with given speed\r\n", io->stdOut); #if PL_CONFIG_USE_POS_PID McuShell_SendHelpStr((unsigned char*)" pos ", (unsigned char*)"Move left and right wheels to given position\r\n", io->stdOut); McuShell_SendHelpStr((unsigned char*)" pos reset", (unsigned char*)"Reset drive and wheel position\r\n", io->stdOut); @@ -329,7 +330,24 @@ uint8_t DRV_ParseCommand(const unsigned char *cmd, bool *handled, const McuShell McuShell_SendStr((unsigned char*)"Wrong argument(s)\r\n", io->stdErr); res = ERR_FAILED; } -#if PL_CONFIG_USE_POS_PID + }else if (McuUtility_strncmp((char*)cmd, (char*)"drive incSpeed ", sizeof("drive incSpeed ")-1)==0) { + p = cmd+sizeof("drive incSpeed"); + if (McuUtility_xatoi(&p, &val1)==ERR_OK) { + if (McuUtility_xatoi(&p, &val2)==ERR_OK) { + int32_t leftSpeed = DRV_Status.speed.left+val1; + int32_t rightSpeed = DRV_Status.speed.right+val2; + if (DRV_SetSpeed(leftSpeed, rightSpeed)!=ERR_OK) { + McuShell_SendStr((unsigned char*)"failed\r\n", io->stdErr); + } + *handled = TRUE; + } else { + McuShell_SendStr((unsigned char*)"failed\r\n", io->stdErr); + } + } else { + McuShell_SendStr((unsigned char*)"Wrong argument(s)\r\n", io->stdErr); + res = ERR_FAILED; + } + #if PL_CONFIG_USE_POS_PID } else if (McuUtility_strncmp((char*)cmd, (char*)"drive pos reset", sizeof("drive pos reset")-1)==0) { QuadCounter_SetPosLeft(0); QuadCounter_SetPosRight(0); diff --git a/ADIS_Sumo_Styger/Sumo/Identify.c b/ADIS_Sumo_Styger/Sumo/Identify.c index 7087e9e..ed86807 100644 --- a/ADIS_Sumo_Styger/Sumo/Identify.c +++ b/ADIS_Sumo_Styger/Sumo/Identify.c @@ -61,6 +61,7 @@ static const ID_RobotDevice idTable[] = {.id=ID_ROBOT_R36, .name=(const unsigned char*)"Robot R36", .uuid={ {0x00,0x00,0x3A,0x00,0x41,0xB5,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }}, {.id=ID_ROBOT_R37, .name=(const unsigned char*)"Robot R37", .uuid={ {0x00,0x00,0x1C,0x00,0xF1,0xBA,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }}, {.id=ID_ROBOT_R44, .name=(const unsigned char*)"Robot R44", .uuid={ {0x00,0x00,0x2F,0x00,0x41,0x6A,0xD2,0x67,0x15,0x32,0x45,0x4E,0x32,0x00,0x02,0x30} }}, + {.id=ID_ROBOT_R45, .name=(const unsigned char*)"Robot R45", .uuid={ {0x00,0x00,0x12,0x00,0x11,0xB9,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }}, }; static ID_Robot_e IdentifyDevice(void) { diff --git a/ADIS_Sumo_Styger/Sumo/Turn.c b/ADIS_Sumo_Styger/Sumo/Turn.c index f14200f..094f59e 100644 --- a/ADIS_Sumo_Styger/Sumo/Turn.c +++ b/ADIS_Sumo_Styger/Sumo/Turn.c @@ -575,6 +575,8 @@ uint8_t TURN_ParseCommand(const unsigned char *cmd, bool *handled, const McuShel if (McuUtility_xatoi(&p, &angle)==ERR_OK) { TURN_TurnAngle((int16_t)angle, NULL); TURN_Turn(TURN_STOP, NULL); + DRV_SetSpeed(100, 100); + DRV_SetMode(DRV_MODE_SPEED); *handled = TRUE; } else { McuShell_SendStr((unsigned char*)"Wrong argument\r\n", io->stdErr);