add wrapper for mqtt commands to rs485, there is an error in the rs485

ADIS_ESP32_Eclipse/main/robo_wrapper.c

@@ -4,25 +4,65 @@
  *  Created on: 09.12.2022
  *      Author: jonas
  */
-
 #include "robo_wrapper.h"
+#include "McuUtility.h"
+#include "McuShell.h"
+#include "Shell.h"
+
+#define RS_CMD_DRIVE_PREFIX "rs sendcmd 0x01 drive "
+#define RS_CMD_TURN_PREFIX "rs sendcmd 0x01 turn "
+#define BUF_SIZE 50
 
 /* Sets the robot to automatic / manual mode */
 bool Robo_Wrapper_SetMode(bool automatic){
+	if(automatic){
+		/* tbd */
+	}else{
+		/* set speed to zero */
+		Robo_Wrapper_Nav_Move(0);
+		/* set mode to speed */
+		unsigned char cmd[BUF_SIZE] = RS_CMD_DRIVE_PREFIX;
+		unsigned char response[128];
+		McuUtility_strcat(cmd, sizeof(cmd), (unsigned char*)"mode speed");
+		SHELL_SendToESPAndGetResponse(cmd, response, sizeof(response));
+		McuShell_SendStr(response, McuShell_GetStdio()->stdOut);
+	}
 	return true;
 }
 
 /* Lets the robot turn to a specific angle and then proceed straight with its previous movement */
 bool Robo_Wrapper_Nav_Turn(int16_t angle){
+	unsigned char cmd[BUF_SIZE] = RS_CMD_TURN_PREFIX;
+	unsigned char response[128];
+	unsigned char angleA[5] = "";
+	McuUtility_Num16sToStr(angleA, sizeof(angleA), angle);
+	McuUtility_strcat(cmd, sizeof(cmd), (unsigned char*)angleA);
+	SHELL_SendToESPAndGetResponse(cmd, response, sizeof(response));
+	McuShell_SendStr(response, McuShell_GetStdio()->stdOut);
 	return true;
 }
 
 /* Sets the movementspeed of the robot */
 bool Robo_Wrapper_Nav_Move(int16_t speed){
+	unsigned char cmd[BUF_SIZE] = RS_CMD_DRIVE_PREFIX;
+	unsigned char speedA[5] = "";
+	unsigned char response[128];
+	McuUtility_strcat(cmd, sizeof(cmd), (unsigned char*)"incSpeed ");
+	McuUtility_Num16sToStr(speedA, sizeof(speedA), speed);
+	McuUtility_strcat(cmd, sizeof(cmd), (unsigned char*)speedA);
+	McuUtility_strcat(cmd, sizeof(cmd), (unsigned char*)" ");
+	McuUtility_strcat(cmd, sizeof(cmd), (unsigned char*)speedA);
+	SHELL_SendToESPAndGetResponse(cmd, response, sizeof(response));
+	McuShell_SendStr(response, McuShell_GetStdio()->stdOut);
 	return true;
 }
 
 /* Stops the movement of the robot */
 bool Robo_Wrapper_Nav_Stop(void){
+	unsigned char cmd[BUF_SIZE] = RS_CMD_DRIVE_PREFIX;
+	unsigned char response[128];
+	McuUtility_strcat(cmd, sizeof(cmd), (unsigned char*)"mode stop");
+	SHELL_SendToESPAndGetResponse(cmd, response, sizeof(response));
+	McuShell_SendStr(response, McuShell_GetStdio()->stdOut);
 	return true;
 }