From ce8c08d652b9db4144e821909830f54476b6d2f5 Mon Sep 17 00:00:00 2001 From: greyes Date: Mon, 19 Aug 2024 09:21:11 +0200 Subject: [PATCH 1/4] Changed steps/s into mm/s --- lib/APPTurtle/src/App.cpp | 2 +- lib/APPTurtle/src/SerialMuxChannels.h | 14 +++++++------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/lib/APPTurtle/src/App.cpp b/lib/APPTurtle/src/App.cpp index f33e3c0d..4e9a8cc2 100644 --- a/lib/APPTurtle/src/App.cpp +++ b/lib/APPTurtle/src/App.cpp @@ -299,7 +299,7 @@ void App::handleTurtle() Max speed is currently unknown to DCS, but using a big value will ensure that RU drives as fast as possible as it will cap the speed to its own maximum. */ - const int16_t MOTOR_MAX_SPEED = 5000; + const int32_t MOTOR_MAX_SPEED = 373; SpeedData payload; /* diff --git a/lib/APPTurtle/src/SerialMuxChannels.h b/lib/APPTurtle/src/SerialMuxChannels.h index 55b9e7be..a0dcceec 100644 --- a/lib/APPTurtle/src/SerialMuxChannels.h +++ b/lib/APPTurtle/src/SerialMuxChannels.h @@ -173,16 +173,16 @@ typedef struct _CommandResponse /** Response Payload. */ union { - int16_t maxMotorSpeed; /**< Max speed [steps/s]. */ + int32_t maxMotorSpeed; /**< Max speed [mm/s]. */ }; } __attribute__((packed)) CommandResponse; /** Struct of the "Speed" channel payload. */ typedef struct _SpeedData { - int16_t left; /**< Left motor speed [steps/s] */ - int16_t right; /**< Right motor speed [steps/s] */ - int16_t center; /**< Center motor speed [steps/s] */ + int32_t left; /**< Left motor speed [mm/s] */ + int32_t right; /**< Right motor speed [mm/s] */ + int32_t center; /**< Center motor speed [mm/s] */ } __attribute__((packed)) SpeedData; /** Struct of the "Current Vehicle Data" channel payload. */ @@ -191,9 +191,9 @@ typedef struct _VehicleData int32_t xPos; /**< X position [mm]. */ int32_t yPos; /**< Y position [mm]. */ int32_t orientation; /**< Orientation [mrad]. */ - int16_t left; /**< Left motor speed [steps/s]. */ - int16_t right; /**< Right motor speed [steps/s]. */ - int16_t center; /**< Center speed [steps/s]. */ + int32_t left; /**< Left motor speed [mm/s]. */ + int32_t right; /**< Right motor speed [mm/s]. */ + int32_t center; /**< Center speed [mm/s]. */ SMPChannelPayload::Range proximity; /**< Range at which object is found [range]. */ } __attribute__((packed)) VehicleData; From 9d8ca77572334db9bd311651efa5f3e7538388d7 Mon Sep 17 00:00:00 2001 From: greyes Date: Mon, 19 Aug 2024 09:26:20 +0200 Subject: [PATCH 2/4] Use incoming speed --- lib/APPTurtle/src/App.cpp | 44 +++++++++++++++++++-------------------- lib/APPTurtle/src/App.h | 6 +++--- 2 files changed, 24 insertions(+), 26 deletions(-) diff --git a/lib/APPTurtle/src/App.cpp b/lib/APPTurtle/src/App.cpp index 4e9a8cc2..af3e41fa 100644 --- a/lib/APPTurtle/src/App.cpp +++ b/lib/APPTurtle/src/App.cpp @@ -140,7 +140,7 @@ void App::setup() else { /* Trigger immediately. */ - m_turtleStepTimer.start(0U); + m_turtleMovementTimer.start(0U); isSuccessful = true; } } @@ -295,32 +295,30 @@ void App::handleTurtle() /* Check for new data. */ if (true == m_isNewTurtleSpeedSetpoint) { - /* - Max speed is currently unknown to DCS, but using a big value will ensure that - RU drives as fast as possible as it will cap the speed to its own maximum. - */ - const int32_t MOTOR_MAX_SPEED = 373; - SpeedData payload; - - /* - Move turtle regardless of the exact speed setpoint received. - */ - if (m_turtleSpeedSetpoint.linear.x > 0.0F) + SpeedData payload; + payload.left = 0; + payload.right = 0; + + int32_t linearSpeed = m_turtleSpeedSetpoint.linear.x * 1000; /* Linear speed in mm/s */ + int32_t angularSpeed = m_turtleSpeedSetpoint.angular.z * 1000; /* Angular speed in mm/s */ + + /* Linear speed has preference over angular speed. */ + if (linearSpeed > 0.0F) { - payload.left = MOTOR_MAX_SPEED; - payload.right = MOTOR_MAX_SPEED; + payload.left = linearSpeed; + payload.right = linearSpeed; } /* Turn left. */ - else if (m_turtleSpeedSetpoint.angular.z > 0.0F) + else if (angularSpeed > 0.0F) { - payload.left = -MOTOR_MAX_SPEED; - payload.right = MOTOR_MAX_SPEED; + payload.left = -angularSpeed; + payload.right = angularSpeed; } /* Turn right. */ - else if (m_turtleSpeedSetpoint.angular.z < 0.0F) + else if (angularSpeed < 0.0F) { - payload.left = MOTOR_MAX_SPEED; - payload.right = -MOTOR_MAX_SPEED; + payload.left = angularSpeed; + payload.right = -angularSpeed; } if (false == m_smpServer.sendData(m_serialMuxProtChannelIdMotorSpeeds, &payload, sizeof(payload))) @@ -330,11 +328,11 @@ void App::handleTurtle() else { m_isNewTurtleSpeedSetpoint = false; - m_turtleStepTimer.start(TURTLE_STEP_TIMER_INTERVAL); + m_turtleMovementTimer.start(TURTLE_STEP_TIMER_INTERVAL); } } - if (true == m_turtleStepTimer.isTimeout()) + if (true == m_turtleMovementTimer.isTimeout()) { SpeedData payload; payload.left = 0; @@ -345,7 +343,7 @@ void App::handleTurtle() LOG_WARNING("Failed to send motor speeds to RU."); } - m_turtleStepTimer.stop(); + m_turtleMovementTimer.stop(); } } diff --git a/lib/APPTurtle/src/App.h b/lib/APPTurtle/src/App.h index 9a0fb11a..7c75e2de 100644 --- a/lib/APPTurtle/src/App.h +++ b/lib/APPTurtle/src/App.h @@ -71,7 +71,7 @@ class App m_serialMuxProtChannelIdMotorSpeeds(0U), m_smpServer(Board::getInstance().getRobot().getStream(), this), m_statusTimer(), - m_turtleStepTimer(), + m_turtleMovementTimer(), m_turtleSpeedSetpoint(), m_isNewTurtleSpeedSetpoint(true) { @@ -136,9 +136,9 @@ class App SimpleTimer m_statusTimer; /** - * Timer for the steps of the turtle. + * Timer for the movement of the turtle. */ - SimpleTimer m_turtleStepTimer; + SimpleTimer m_turtleMovementTimer; /** * Turtle speed setpoint. From 32cc8900331f18b61648488dfaa1cba343e7ac71 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Mon, 19 Aug 2024 16:04:00 +0200 Subject: [PATCH 3/4] Send linear and angular speeds to RU --- lib/APPTurtle/src/App.cpp | 44 ++++++++------------------- lib/APPTurtle/src/App.h | 6 ++-- lib/APPTurtle/src/SerialMuxChannels.h | 13 ++++++++ 3 files changed, 29 insertions(+), 34 deletions(-) diff --git a/lib/APPTurtle/src/App.cpp b/lib/APPTurtle/src/App.cpp index af3e41fa..bd130386 100644 --- a/lib/APPTurtle/src/App.cpp +++ b/lib/APPTurtle/src/App.cpp @@ -274,10 +274,9 @@ bool App::setupSerialMuxProtServer() bool isSuccessful = false; m_serialMuxProtChannelIdStatus = m_smpServer.createChannel(STATUS_CHANNEL_NAME, STATUS_CHANNEL_DLC); - m_serialMuxProtChannelIdMotorSpeeds = - m_smpServer.createChannel(SPEED_SETPOINT_CHANNEL_NAME, SPEED_SETPOINT_CHANNEL_DLC); + m_serialMuxProtChannelIdTurtle = m_smpServer.createChannel(TURTLE_CHANNEL_NAME, TURTLE_CHANNEL_DLC); - if ((0U == m_serialMuxProtChannelIdStatus) || (0U == m_serialMuxProtChannelIdMotorSpeeds)) + if ((0U == m_serialMuxProtChannelIdStatus) || (0U == m_serialMuxProtChannelIdTurtle)) { LOG_ERROR("Failed to create SerialMuxProt channels."); } @@ -295,33 +294,16 @@ void App::handleTurtle() /* Check for new data. */ if (true == m_isNewTurtleSpeedSetpoint) { - SpeedData payload; - payload.left = 0; - payload.right = 0; + TurtleSpeed payload; + int32_t linearSpeed = m_turtleSpeedSetpoint.linear.x * 1000; /* Linear speed in mm/s */ + int32_t angularSpeed = m_turtleSpeedSetpoint.angular.z * 1000; /* Angular speed in mrad/s */ - int32_t linearSpeed = m_turtleSpeedSetpoint.linear.x * 1000; /* Linear speed in mm/s */ - int32_t angularSpeed = m_turtleSpeedSetpoint.angular.z * 1000; /* Angular speed in mm/s */ + payload.linearCenter = linearSpeed; + payload.angular = angularSpeed; - /* Linear speed has preference over angular speed. */ - if (linearSpeed > 0.0F) - { - payload.left = linearSpeed; - payload.right = linearSpeed; - } - /* Turn left. */ - else if (angularSpeed > 0.0F) - { - payload.left = -angularSpeed; - payload.right = angularSpeed; - } - /* Turn right. */ - else if (angularSpeed < 0.0F) - { - payload.left = angularSpeed; - payload.right = -angularSpeed; - } + LOG_DEBUG("Linear speed: %d mm/s, Angular speed: %d mrad/s", payload.linearCenter, payload.angular); - if (false == m_smpServer.sendData(m_serialMuxProtChannelIdMotorSpeeds, &payload, sizeof(payload))) + if (false == m_smpServer.sendData(m_serialMuxProtChannelIdTurtle, &payload, sizeof(payload))) { LOG_WARNING("Failed to send motor speeds to RU."); } @@ -334,11 +316,11 @@ void App::handleTurtle() if (true == m_turtleMovementTimer.isTimeout()) { - SpeedData payload; - payload.left = 0; - payload.right = 0; + TurtleSpeed payload; + payload.linearCenter = 0; + payload.angular = 0; - if (false == m_smpServer.sendData(m_serialMuxProtChannelIdMotorSpeeds, &payload, sizeof(payload))) + if (false == m_smpServer.sendData(m_serialMuxProtChannelIdTurtle, &payload, sizeof(payload))) { LOG_WARNING("Failed to send motor speeds to RU."); } diff --git a/lib/APPTurtle/src/App.h b/lib/APPTurtle/src/App.h index 7c75e2de..a8ec184e 100644 --- a/lib/APPTurtle/src/App.h +++ b/lib/APPTurtle/src/App.h @@ -68,7 +68,7 @@ class App App() : m_ros(), m_serialMuxProtChannelIdStatus(0U), - m_serialMuxProtChannelIdMotorSpeeds(0U), + m_serialMuxProtChannelIdTurtle(0U), m_smpServer(Board::getInstance().getRobot().getStream(), this), m_statusTimer(), m_turtleMovementTimer(), @@ -121,9 +121,9 @@ class App uint8_t m_serialMuxProtChannelIdStatus; /** - * SerialMuxProt Channel id for sending motor speeds. + * SerialMuxProt Channel id for sending Turtle speeds. */ - uint8_t m_serialMuxProtChannelIdMotorSpeeds; + uint8_t m_serialMuxProtChannelIdTurtle; /** * SerialMuxProt Server Instance diff --git a/lib/APPTurtle/src/SerialMuxChannels.h b/lib/APPTurtle/src/SerialMuxChannels.h index a0dcceec..55cff258 100644 --- a/lib/APPTurtle/src/SerialMuxChannels.h +++ b/lib/APPTurtle/src/SerialMuxChannels.h @@ -86,6 +86,12 @@ /** DLC of Line Sensor Channel */ #define LINE_SENSOR_CHANNEL_DLC (sizeof(LineSensorData)) +/** Name of the Channel to send Turtle Speeds. */ +#define TURTLE_CHANNEL_NAME "TURTLE" + +/** DLC of Turtle Channel */ +#define TURTLE_CHANNEL_DLC (sizeof(TurtleSpeed)) + /****************************************************************************** * Types and Classes *****************************************************************************/ @@ -209,6 +215,13 @@ typedef struct _LineSensorData uint16_t lineSensorData[5U]; /**< Line sensor data [digits] normalized to max 1000 digits. */ } __attribute__((packed)) LineSensorData; +/** Struct of the "Turtle" channel payload. */ +typedef struct _TurtleSpeed +{ + int32_t linearCenter; /**< Linear speed of the vehicle center. [mm/s] */ + int32_t angular; /**< Angular speed. [mrad/s] */ +} __attribute__((packed)) TurtleSpeed; + /****************************************************************************** * Functions *****************************************************************************/ From ce2c6bdb500a5fd5b4c712a5dfb0f83c4837fe35 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Tue, 20 Aug 2024 11:11:21 +0200 Subject: [PATCH 4/4] Added milli conversion factor --- lib/APPTurtle/src/App.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/lib/APPTurtle/src/App.cpp b/lib/APPTurtle/src/App.cpp index bd130386..0c1c9ab4 100644 --- a/lib/APPTurtle/src/App.cpp +++ b/lib/APPTurtle/src/App.cpp @@ -294,9 +294,10 @@ void App::handleTurtle() /* Check for new data. */ if (true == m_isNewTurtleSpeedSetpoint) { - TurtleSpeed payload; - int32_t linearSpeed = m_turtleSpeedSetpoint.linear.x * 1000; /* Linear speed in mm/s */ - int32_t angularSpeed = m_turtleSpeedSetpoint.angular.z * 1000; /* Angular speed in mrad/s */ + TurtleSpeed payload; + const int32_t MILLI_CONVERSION_FACTOR = 1000; + int32_t linearSpeed = m_turtleSpeedSetpoint.linear.x * MILLI_CONVERSION_FACTOR; /* Linear speed in mm/s */ + int32_t angularSpeed = m_turtleSpeedSetpoint.angular.z * MILLI_CONVERSION_FACTOR; /* Angular speed in mrad/s */ payload.linearCenter = linearSpeed; payload.angular = angularSpeed;