You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
68 lines
2.3 KiB
68 lines
2.3 KiB
/*
|
|
* robo_wrapper.c
|
|
*
|
|
* 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 "drive "
|
|
#define RS_CMD_TURN_PREFIX "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 */
|
|
unsigned char response[128];
|
|
SHELL_SendToRobotAndGetResponse((const unsigned char*)"drive speed 0 0", response, sizeof(response));
|
|
/* set mode to speed */
|
|
unsigned char cmd[BUF_SIZE] = RS_CMD_DRIVE_PREFIX;
|
|
McuUtility_strcat(cmd, sizeof(cmd), (unsigned char*)"mode speed");
|
|
SHELL_SendToRobotAndGetResponse(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_SendToRobotAndGetResponse(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_SendToRobotAndGetResponse(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_SendToRobotAndGetResponse(cmd, response, sizeof(response));
|
|
McuShell_SendStr(response, McuShell_GetStdio()->stdOut);
|
|
return true;
|
|
}
|
|
|