From 4a76282d17832fafc219d59f62f838583a90e085 Mon Sep 17 00:00:00 2001 From: greyes Date: Fri, 16 Aug 2024 11:50:49 +0200 Subject: [PATCH 1/5] Use mm/s in PlatoonServices --- lib/PlatoonService/src/CollisionAvoidance.h | 12 +++++------ lib/PlatoonService/src/HeadingFinder.cpp | 6 +++--- lib/PlatoonService/src/HeadingFinder.h | 22 ++++++++++----------- lib/PlatoonService/src/Telemetry.h | 14 ++++++------- lib/PlatoonService/src/Waypoint.cpp | 18 ++++++++--------- lib/PlatoonService/src/Waypoint.h | 14 ++++++------- 6 files changed, 43 insertions(+), 43 deletions(-) diff --git a/lib/PlatoonService/src/CollisionAvoidance.h b/lib/PlatoonService/src/CollisionAvoidance.h index 2ed8acbd..223ab46b 100644 --- a/lib/PlatoonService/src/CollisionAvoidance.h +++ b/lib/PlatoonService/src/CollisionAvoidance.h @@ -68,7 +68,7 @@ class CollisionAvoidance */ CollisionAvoidance(Telemetry::Range closestProximityRangeValue, Telemetry::Range rangeThreshold) : m_closestProximityRangeValue(closestProximityRangeValue), - m_rangeThreshold(rangeThreshold){}; + m_rangeThreshold(rangeThreshold) {}; /** * Default Destructor. @@ -80,10 +80,10 @@ class CollisionAvoidance /** * Limit the speed of the vehicle to avoid collision. * - * @param[out] speedSetpoint The speed setpoint to be limited in steps/s. + * @param[out] speedSetpoint The speed setpoint to be limited in mm/s. * @param[in] vehicleData The vehicle data structure. */ - void limitSpeedToAvoidCollision(int16_t& speedSetpoint, const Telemetry& vehicleData) const + void limitSpeedToAvoidCollision(int32_t& speedSetpoint, const Telemetry& vehicleData) const { /* Is vehicle is closer than the threshold? */ if (vehicleData.proximity >= m_rangeThreshold) @@ -96,11 +96,11 @@ class CollisionAvoidance /** * Limit the speed of the vehicle to avoid collision. * - * @param[out] leftSpeedSetpoint The left motor speed setpoint to be limited in steps/s. - * @param[out] rightSpeedSetpoint The right motor speed setpoint to be limited in steps/s. + * @param[out] leftSpeedSetpoint The left motor speed setpoint to be limited in mm/s. + * @param[out] rightSpeedSetpoint The right motor speed setpoint to be limited in mm/s. * @param[in] vehicleData The vehicle data structure. */ - void limitSpeedToAvoidCollision(int16_t& leftSpeedSetpoint, int16_t& rightSpeedSetpoint, + void limitSpeedToAvoidCollision(int32_t& leftSpeedSetpoint, int32_t& rightSpeedSetpoint, const Telemetry& vehicleData) const { /* Is vehicle is closer than the threshold? */ diff --git a/lib/PlatoonService/src/HeadingFinder.cpp b/lib/PlatoonService/src/HeadingFinder.cpp index 0d3a8eb8..815c4d08 100644 --- a/lib/PlatoonService/src/HeadingFinder.cpp +++ b/lib/PlatoonService/src/HeadingFinder.cpp @@ -93,9 +93,9 @@ void HeadingFinder::setPIDFactors(int32_t pNumerator, int32_t pDenominator, int3 m_pidCtrl.setDFactor(dNumerator, dDenominator); } -int16_t HeadingFinder::process(int16_t& targetSpeedLeft, int16_t& targetSpeedRight) +int32_t HeadingFinder::process(int32_t& targetSpeedLeft, int32_t& targetSpeedRight) { - int16_t pidDelta = 0; + int32_t pidDelta = 0; /* Process PID controller when timer is done, new data is found, and the motors have a setpoint greater than 0. */ if ((true == m_pidProcessTime.isTimeout()) && (true == m_newOdometryData) && @@ -152,7 +152,7 @@ void HeadingFinder::setOdometryData(int32_t xPos, int32_t yPos, int32_t heading) m_newOdometryData = true; } -void HeadingFinder::setMotorSpeedData(int16_t speedLeft, int16_t speedRight) +void HeadingFinder::setMotorSpeedData(int32_t speedLeft, int32_t speedRight) { /* Set motor speeds. */ m_data.currentSpeedLeft = speedLeft; diff --git a/lib/PlatoonService/src/HeadingFinder.h b/lib/PlatoonService/src/HeadingFinder.h index 5159504a..7040e971 100644 --- a/lib/PlatoonService/src/HeadingFinder.h +++ b/lib/PlatoonService/src/HeadingFinder.h @@ -70,8 +70,8 @@ class HeadingFinder int32_t currentXPos; /**< Current X position [mm]. */ int32_t currentYPos; /**< Current Y position[mm]. */ int32_t currentHeading; /**< Current heading [mrad]. */ - int16_t currentSpeedLeft; /**< Current speed of the left motor [steps/s]. */ - int16_t currentSpeedRight; /**< Current speed of the right motor [steps/s]. */ + int32_t currentSpeedLeft; /**< Current speed of the left motor [mm/s]. */ + int32_t currentSpeedRight; /**< Current speed of the right motor [mm/s]. */ int32_t targetXPos; /**< Target X position[mm]. */ int32_t targetYPos; /**< Target Y position[mm]. */ int32_t targetHeading; /**< Target heading [mrad]. */ @@ -98,13 +98,13 @@ class HeadingFinder /** * Process the Heading finder and calculate the target speed for the motors. - * @param[out] targetSpeedLeft Target speed of the left motor [steps/s]. - * @param[out] targetSpeedRight Target speed of the right motor [steps/s]. + * @param[out] targetSpeedLeft Target speed of the left motor [mm/s]. + * @param[out] targetSpeedRight Target speed of the right motor [mm/s]. * - * @return the delta speed [steps/s] calculated by the PID controller, if available. + * @return the delta speed [mm/s] calculated by the PID controller, if available. * Otherwise 0. */ - int16_t process(int16_t& targetSpeedLeft, int16_t& targetSpeedRight); + int32_t process(int32_t& targetSpeedLeft, int32_t& targetSpeedRight); /** * Set lastest odometry data. @@ -116,10 +116,10 @@ class HeadingFinder /** * Set latest motor speed data. - * @param[in] speedLeft Speed of the left motor [steps/s]. - * @param[in] speedRight Speed of the right motor [steps/s]. + * @param[in] speedLeft Speed of the left motor [mm/s]. + * @param[in] speedRight Speed of the right motor [mm/s]. */ - void setMotorSpeedData(int16_t speedLeft, int16_t speedRight); + void setMotorSpeedData(int32_t speedLeft, int32_t speedRight); /** * Set the target heading the robot shall face to using its x and y coordinates. @@ -139,8 +139,8 @@ class HeadingFinder /** Period in ms for PID processing. */ static const uint32_t PID_PROCESS_PERIOD = 40U; - /** Maximum motor speed in encoder steps/s */ - static const int16_t MAX_MOTOR_SPEED = 3000; + /** Maximum motor speed in mm/s */ + static const int32_t MAX_MOTOR_SPEED = 373; /** The PID proportional factor numerator for the heading controller. */ static const int32_t PID_P_NUMERATOR = 4; diff --git a/lib/PlatoonService/src/Telemetry.h b/lib/PlatoonService/src/Telemetry.h index fcd9331f..023d9ac6 100644 --- a/lib/PlatoonService/src/Telemetry.h +++ b/lib/PlatoonService/src/Telemetry.h @@ -79,9 +79,9 @@ struct Telemetry 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]. */ Telemetry::Range proximity; /**< Range at which object is found [range]. */ /** @@ -97,12 +97,12 @@ struct Telemetry * @param[in] xPos X position [mm]. * @param[in] yPos Y position [mm]. * @param[in] orientation Orientation [mrad]. - * @param[in] left Left motor speed [steps/s]. - * @param[in] right Right motor speed [steps/s]. - * @param[in] center Center speed [steps/s]. + * @param[in] left Left motor speed [mm/s]. + * @param[in] right Right motor speed [mm/s]. + * @param[in] center Center speed [mm/s]. * @param[in] proximity Range at which object is found [range]. */ - Telemetry(int32_t xPos, int32_t yPos, int32_t orientation, int16_t left, int16_t right, int16_t center, + Telemetry(int32_t xPos, int32_t yPos, int32_t orientation, int32_t left, int32_t right, int32_t center, Telemetry::Range proximity) : xPos(xPos), yPos(yPos), diff --git a/lib/PlatoonService/src/Waypoint.cpp b/lib/PlatoonService/src/Waypoint.cpp index 4666f42a..ea85b4d5 100644 --- a/lib/PlatoonService/src/Waypoint.cpp +++ b/lib/PlatoonService/src/Waypoint.cpp @@ -92,9 +92,9 @@ Waypoint* Waypoint::fromJsonObject(const JsonObject& jsonWaypoint) JsonVariant jsonXPos = jsonWaypoint["X"]; /* X position [mm]. */ JsonVariant jsonYPos = jsonWaypoint["Y"]; /* Y position [mm]. */ JsonVariant jsonOrientation = jsonWaypoint["Orientation"]; /* Orientation [mrad]. */ - JsonVariant jsonLeft = jsonWaypoint["Left"]; /* Left motor speed [steps/s]. */ - JsonVariant jsonRight = jsonWaypoint["Right"]; /* Right motor speed [steps/s]. */ - JsonVariant jsonCenter = jsonWaypoint["Center"]; /* Center speed [steps/s]. */ + JsonVariant jsonLeft = jsonWaypoint["Left"]; /* Left motor speed [mm/s]. */ + JsonVariant jsonRight = jsonWaypoint["Right"]; /* Right motor speed [mm/s]. */ + JsonVariant jsonCenter = jsonWaypoint["Center"]; /* Center speed [mm/s]. */ if ((false == jsonXPos.isNull()) && (false == jsonYPos.isNull()) && (false == jsonOrientation.isNull()) && (false == jsonLeft.isNull()) && (false == jsonRight.isNull()) && (false == jsonCenter.isNull())) @@ -103,9 +103,9 @@ Waypoint* Waypoint::fromJsonObject(const JsonObject& jsonWaypoint) int32_t xPos = jsonXPos.as(); int32_t yPos = jsonYPos.as(); int32_t orientation = jsonOrientation.as(); - int16_t left = jsonLeft.as(); - int16_t right = jsonRight.as(); - int16_t center = jsonCenter.as(); + int32_t left = jsonLeft.as(); + int32_t right = jsonRight.as(); + int32_t center = jsonCenter.as(); waypoint = new (std::nothrow) Waypoint(xPos, yPos, orientation, left, right, center); } @@ -121,9 +121,9 @@ void Waypoint::serialize(String& serializedWaypoint) const jsonPayload["X"] = xPos; /* X position [mm]. */ jsonPayload["Y"] = yPos; /* Y position [mm]. */ jsonPayload["Orientation"] = orientation; /* Orientation [mrad]. */ - jsonPayload["Left"] = left; /* Left motor speed [steps/s]. */ - jsonPayload["Right"] = right; /* Right motor speed [steps/s]. */ - jsonPayload["Center"] = center; /* Center speed [steps/s]. */ + jsonPayload["Left"] = left; /* Left motor speed [mm/s]. */ + jsonPayload["Right"] = right; /* Right motor speed [mm/s]. */ + jsonPayload["Center"] = center; /* Center speed [mm/s]. */ size_t jsonBufferSize = measureJson(jsonPayload) + 1U; char jsonBuffer[jsonBufferSize]; diff --git a/lib/PlatoonService/src/Waypoint.h b/lib/PlatoonService/src/Waypoint.h index 3e03a755..06ee6623 100644 --- a/lib/PlatoonService/src/Waypoint.h +++ b/lib/PlatoonService/src/Waypoint.h @@ -65,9 +65,9 @@ struct Waypoint 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]. */ /** * Default constructor. @@ -82,11 +82,11 @@ struct Waypoint * @param[in] xPos X position [mm]. * @param[in] yPos Y position [mm]. * @param[in] orientation Orientation [mrad]. - * @param[in] left Left motor speed [steps/s]. - * @param[in] right Right motor speed [steps/s]. - * @param[in] center Center speed [steps/s]. + * @param[in] left Left motor speed [mm/s]. + * @param[in] right Right motor speed [mm/s]. + * @param[in] center Center speed [mm/s]. */ - Waypoint(int32_t xPos, int32_t yPos, int32_t orientation, int16_t left, int16_t right, int16_t center) : + Waypoint(int32_t xPos, int32_t yPos, int32_t orientation, int32_t left, int32_t right, int32_t center) : xPos(xPos), yPos(yPos), orientation(orientation), From 441dcc595ffdd3b0e0a06fcaba4519094e6fed7c Mon Sep 17 00:00:00 2001 From: greyes Date: Fri, 16 Aug 2024 11:51:00 +0200 Subject: [PATCH 2/5] Use mm/s in ConvoyLeader --- lib/ConvoyLeader/src/App.cpp | 2 +- lib/ConvoyLeader/src/DrivingState.cpp | 12 ++++++------ lib/ConvoyLeader/src/DrivingState.h | 18 +++++++++--------- lib/ConvoyLeader/src/SerialMuxChannels.h | 14 +++++++------- 4 files changed, 23 insertions(+), 23 deletions(-) diff --git a/lib/ConvoyLeader/src/App.cpp b/lib/ConvoyLeader/src/App.cpp index 3af3b7b1..7d7c44a8 100644 --- a/lib/ConvoyLeader/src/App.cpp +++ b/lib/ConvoyLeader/src/App.cpp @@ -382,7 +382,7 @@ void App::processPeriodicTasks() if ((true == m_motorSpeedTimer.isTimeout()) && (true == m_smpServer.isSynced())) { - int16_t centerSpeed = 0; + int32_t centerSpeed = 0; SpeedData payload; if (false == DrivingState::getInstance().getTopMotorSpeed(centerSpeed)) diff --git a/lib/ConvoyLeader/src/DrivingState.cpp b/lib/ConvoyLeader/src/DrivingState.cpp index d6681b3b..a82a419d 100644 --- a/lib/ConvoyLeader/src/DrivingState.cpp +++ b/lib/ConvoyLeader/src/DrivingState.cpp @@ -79,8 +79,8 @@ void DrivingState::process(StateMachine& sm) /* Compact implementation of the "Platoon Application Controller". */ /* Drive as fast as possible */ - int16_t linearSpeedSetpoint = m_maxMotorSpeed; - int16_t controlSpeed = m_maxMotorSpeed; + int32_t linearSpeedSetpoint = m_maxMotorSpeed; + int32_t controlSpeed = m_maxMotorSpeed; /* Constrain the setpoint based on platoon length. */ platoonLengthController(linearSpeedSetpoint); @@ -115,13 +115,13 @@ void DrivingState::exit() m_isActive = false; } -void DrivingState::setMaxMotorSpeed(int16_t maxSpeed) +void DrivingState::setMaxMotorSpeed(int32_t maxSpeed) { m_maxMotorSpeed = maxSpeed; LOG_DEBUG("DrivingState: Max motor speed: %d", m_maxMotorSpeed); } -bool DrivingState::getTopMotorSpeed(int16_t& topMotorSpeed) const +bool DrivingState::getTopMotorSpeed(int32_t& topMotorSpeed) const { topMotorSpeed = m_currentSpeedSetpoint; @@ -151,12 +151,12 @@ void DrivingState::setPlatoonLength(const int32_t platoonLength) * Private Methods *****************************************************************************/ -void DrivingState::platoonLengthController(int16_t& linearCenterSpeedSetpoint) +void DrivingState::platoonLengthController(int32_t& linearCenterSpeedSetpoint) { /* Limit the speed setpoint if platoon length greater than maximum allowed. */ if (m_platoonLength > MAX_PLATOON_LENGTH) { - int16_t maxPossibleSpeed = linearCenterSpeedSetpoint; + int32_t maxPossibleSpeed = linearCenterSpeedSetpoint; maxPossibleSpeed = maxPossibleSpeed - (m_platoonLength * maxPossibleSpeed / (MAX_PLATOON_LENGTH * 10)); diff --git a/lib/ConvoyLeader/src/DrivingState.h b/lib/ConvoyLeader/src/DrivingState.h index 131fa029..2e890fc2 100644 --- a/lib/ConvoyLeader/src/DrivingState.h +++ b/lib/ConvoyLeader/src/DrivingState.h @@ -96,18 +96,18 @@ class DrivingState : public IState * Set maximum motor speed. * This also sets the range factor as is dependent on the maximum motor speed. * - * @param[in] maxSpeed Maximum motor speed. + * @param[in] maxSpeed Maximum motor speed in mm/s. */ - void setMaxMotorSpeed(int16_t maxSpeed); + void setMaxMotorSpeed(int32_t maxSpeed); /** * Get the calculated top motor speed. * - * @param[out] topMotorSpeed The calculated top motor speed. + * @param[out] topMotorSpeed The calculated top motor speed in mm/s. * * @returns true if the top motor speed is valid. Otherwise, false. */ - bool getTopMotorSpeed(int16_t& topMotorSpeed) const; + bool getTopMotorSpeed(int32_t& topMotorSpeed) const; /** * Set latest vehicle data. @@ -147,11 +147,11 @@ class DrivingState : public IState /** Flag: State is active. */ bool m_isActive; - /** Maximum motor speed in steps/s. */ - int16_t m_maxMotorSpeed; + /** Maximum motor speed in mm/s. */ + int32_t m_maxMotorSpeed; - /** Current linear speed setpoint to apply to the vehicle. */ - int16_t m_currentSpeedSetpoint; + /** Current linear speed setpoint to apply to the vehicle in mm/s. */ + int32_t m_currentSpeedSetpoint; /** Latest vehicle data. */ Telemetry m_vehicleData; @@ -167,7 +167,7 @@ class DrivingState : public IState * * @param[out] speedSetpoint The speed setpoint. */ - void platoonLengthController(int16_t& speedSetpoint); + void platoonLengthController(int32_t& speedSetpoint); /** * Default constructor. diff --git a/lib/ConvoyLeader/src/SerialMuxChannels.h b/lib/ConvoyLeader/src/SerialMuxChannels.h index 6fd1d5f5..551be935 100644 --- a/lib/ConvoyLeader/src/SerialMuxChannels.h +++ b/lib/ConvoyLeader/src/SerialMuxChannels.h @@ -167,16 +167,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. */ @@ -185,9 +185,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 314abd7cfa86d0e6d462e77b707934c8125f6b0a Mon Sep 17 00:00:00 2001 From: greyes Date: Fri, 16 Aug 2024 11:51:13 +0200 Subject: [PATCH 3/5] Use mm/s in RemoteControl --- lib/RemoteControl/src/App.cpp | 4 ++-- lib/RemoteControl/src/SerialMuxChannels.h | 14 +++++++------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/lib/RemoteControl/src/App.cpp b/lib/RemoteControl/src/App.cpp index a13ee703..e15e3cec 100644 --- a/lib/RemoteControl/src/App.cpp +++ b/lib/RemoteControl/src/App.cpp @@ -370,8 +370,8 @@ void App::motorSpeedsTopicCallback(const String& payload) if ((false == leftSpeed.isNull()) && (false == rightSpeed.isNull())) { SpeedData motorSetpoints; - motorSetpoints.left = leftSpeed.as(); - motorSetpoints.right = rightSpeed.as(); + motorSetpoints.left = leftSpeed.as(); + motorSetpoints.right = rightSpeed.as(); if (true == m_smpServer.sendData(m_serialMuxProtChannelIdMotorSpeeds, reinterpret_cast(&motorSetpoints), sizeof(motorSetpoints))) diff --git a/lib/RemoteControl/src/SerialMuxChannels.h b/lib/RemoteControl/src/SerialMuxChannels.h index 55b9e7be..a0dcceec 100644 --- a/lib/RemoteControl/src/SerialMuxChannels.h +++ b/lib/RemoteControl/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 5149f8e59088fe374be654c99a8bf41728a97595 Mon Sep 17 00:00:00 2001 From: greyes Date: Fri, 16 Aug 2024 12:02:24 +0200 Subject: [PATCH 4/5] Use mm/s in ConvoyFollower --- lib/ConvoyFollower/src/App.cpp | 4 ++-- lib/ConvoyFollower/src/DrivingState.cpp | 6 +++--- lib/ConvoyFollower/src/DrivingState.h | 22 +++++++++++----------- lib/ConvoyFollower/src/SerialMuxChannels.h | 14 +++++++------- 4 files changed, 23 insertions(+), 23 deletions(-) diff --git a/lib/ConvoyFollower/src/App.cpp b/lib/ConvoyFollower/src/App.cpp index cc369fd6..de7cb2fc 100644 --- a/lib/ConvoyFollower/src/App.cpp +++ b/lib/ConvoyFollower/src/App.cpp @@ -388,8 +388,8 @@ void App::processPeriodicTasks() if ((true == m_motorSpeedTimer.isTimeout()) && (true == m_smpServer.isSynced())) { - int16_t leftSpeed = 0; - int16_t rightSpeed = 0; + int32_t leftSpeed = 0; + int32_t rightSpeed = 0; SpeedData payload; if (false == DrivingState::getInstance().getMotorSpeedSetpoints(leftSpeed, rightSpeed)) diff --git a/lib/ConvoyFollower/src/DrivingState.cpp b/lib/ConvoyFollower/src/DrivingState.cpp index f1deac22..f3bea3bf 100644 --- a/lib/ConvoyFollower/src/DrivingState.cpp +++ b/lib/ConvoyFollower/src/DrivingState.cpp @@ -127,7 +127,7 @@ void DrivingState::process(StateMachine& sm) else { int32_t pidDelta = 0; - int16_t centerSpeedSetpoint = 0; + int32_t centerSpeedSetpoint = 0; /* Longitudinal controller. */ if (true == m_ivsPidProcessTimer.isTimeout()) @@ -176,12 +176,12 @@ void DrivingState::exit() m_isActive = false; } -void DrivingState::setMaxMotorSpeed(int16_t maxSpeed) +void DrivingState::setMaxMotorSpeed(int32_t maxSpeed) { m_maxMotorSpeed = maxSpeed; } -bool DrivingState::getMotorSpeedSetpoints(int16_t& leftMotorSpeed, int16_t& rightMotorSpeed) const +bool DrivingState::getMotorSpeedSetpoints(int32_t& leftMotorSpeed, int32_t& rightMotorSpeed) const { leftMotorSpeed = m_leftMotorSpeed; rightMotorSpeed = m_rightMotorSpeed; diff --git a/lib/ConvoyFollower/src/DrivingState.h b/lib/ConvoyFollower/src/DrivingState.h index 00d57c8b..2cec1e61 100644 --- a/lib/ConvoyFollower/src/DrivingState.h +++ b/lib/ConvoyFollower/src/DrivingState.h @@ -102,19 +102,19 @@ class DrivingState : public IState /** * Set maximum motor speed. * - * @param[in] maxSpeed Maximum motor speed. + * @param[in] maxSpeed Maximum motor speed in mm/s. */ - void setMaxMotorSpeed(int16_t maxSpeed); + void setMaxMotorSpeed(int32_t maxSpeed); /** * Get the calculated motor speed setpoints. * - * @param[out] leftMotorSpeed The calculated left motor speed. - * @param[out] rightMotorSpeed The calculated right motor speed. + * @param[out] leftMotorSpeed The calculated left motor speed in mm/s. + * @param[out] rightMotorSpeed The calculated right motor speed in mm/s. * * @returns true if the speed setpoints are valid. Otherwise, false. */ - bool getMotorSpeedSetpoints(int16_t& leftMotorSpeed, int16_t& rightMotorSpeed) const; + bool getMotorSpeedSetpoints(int32_t& leftMotorSpeed, int32_t& rightMotorSpeed) const; /** * Set latest vehicle data. @@ -235,14 +235,14 @@ class DrivingState : public IState /** Flag: State is active. */ bool m_isActive; - /** Maximum motor speed. */ - int16_t m_maxMotorSpeed; + /** Maximum motor speed in mm/s. */ + int32_t m_maxMotorSpeed; - /** Calculated left motor speed. */ - int16_t m_leftMotorSpeed; + /** Calculated left motor speed in mm/s. */ + int32_t m_leftMotorSpeed; - /** Calculated left motor speed. */ - int16_t m_rightMotorSpeed; + /** Calculated left motor speed in mm/s. */ + int32_t m_rightMotorSpeed; /** Latest vehicle data. */ Telemetry m_vehicleData; diff --git a/lib/ConvoyFollower/src/SerialMuxChannels.h b/lib/ConvoyFollower/src/SerialMuxChannels.h index 6fd1d5f5..551be935 100644 --- a/lib/ConvoyFollower/src/SerialMuxChannels.h +++ b/lib/ConvoyFollower/src/SerialMuxChannels.h @@ -167,16 +167,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. */ @@ -185,9 +185,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 e1a9f4c5592f836dd09d96f2782df771053a094c Mon Sep 17 00:00:00 2001 From: greyes Date: Fri, 16 Aug 2024 12:21:07 +0200 Subject: [PATCH 5/5] Adjusted Heading finder P numerator to adjust for speed units --- lib/ConvoyFollower/src/DrivingState.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/ConvoyFollower/src/DrivingState.h b/lib/ConvoyFollower/src/DrivingState.h index 2cec1e61..515c81dd 100644 --- a/lib/ConvoyFollower/src/DrivingState.h +++ b/lib/ConvoyFollower/src/DrivingState.h @@ -214,7 +214,7 @@ class DrivingState : public IState struct HEADING_FINDER_PID_FACTORS { /** The PID proportional factor numerator for the Heading Finder. */ - static const int32_t PID_P_NUMERATOR = 5; + static const int32_t PID_P_NUMERATOR = 1; /** The PID proportional factor denominator for the Heading Finder.*/ static const int32_t PID_P_DENOMINATOR = 4;