Merge branch 'main' of gitlab.enterpriselab.ch:adis_team_gueti_roaster/adis_hs2022_team_4 into main

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

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

ADIS_Csharp/RaspiControl/Navigation.cs

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

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

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 });
+                client.Subscribe(new string[] { MqttConstants.DEVICE_STATUS_APP_TOPIC }, new byte[] { MqttMsgBase.QOS_LEVEL_AT_MOST_ONCE });
-                navigation= new Navigation();
+                Joystick joystick = new 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(MqttConstants.MOBILE_NAV_STOP_TOPIC, Encoding.UTF8.GetBytes($"{NavigationConstants.STOP}"), MqttMsgBase.QOS_LEVEL_EXACTLY_ONCE, false);
-                    client.Publish("mobile/cmd/nav/drive", Encoding.UTF8.GetBytes("true"), MqttMsgBase.QOS_LEVEL_AT_MOST_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, 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");
+            string deviceId = "";
-            Console.Write(Encoding.UTF8.GetString(e.Message) + "\n");
+            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:
+                                            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<string, object> getValueFromTopic(string topic) {
+            return JsonSerializer.Deserialize<Dictionary<string, object>>(topic);
         }
     }
 }

ADIS_Csharp/RaspiControl/RaspiControl.csproj

@@ -1,4 +1,4 @@
-<Project Sdk="Microsoft.NET.Sdk">
+<Project Sdk="Microsoft.NET.Sdk">
 
   <PropertyGroup>
     <OutputType>Exe</OutputType>
@@ -8,7 +8,7 @@
   </PropertyGroup>
 
   <Target Name="PostBuild" AfterTargets="PostBuildEvent">
-    <Exec Command="cd &quot;$(TargetDir)&quot;&#xD;&#xA;&quot;$(SolutionDir)\SecureUpload.exe&quot; . pi-home:ADIS/$(ProjectName)" />
+    <Exec Command="cd &quot;$(TargetDir)&quot;&#xD;&#xA;&quot;$(SolutionDir)\SecureUpload.exe&quot; . pi-hslu:ADIS/$(ProjectName)" />
   </Target>
 
   <ItemGroup>

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) {

ADIS_Sumo_Styger/Sumo/Drive.c

@@ -258,6 +258,7 @@ static void DRV_PrintHelp(const McuShell_StdIOType *io) {
   McuShell_SendHelpStr((unsigned char*)"  mode <mode>", (unsigned char*)"Set driving mode (none|stop|speed)\r\n", io->stdOut);
 #endif
   McuShell_SendHelpStr((unsigned char*)"  speed <left> <right>", (unsigned char*)"Move left and right motors with given speed\r\n", io->stdOut);
+  McuShell_SendHelpStr((unsigned char*)"  incSpeed <left> <right>", (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 <left> <right>", (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);

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) {

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