From 436d392a130f227e7100ad1ba054df78e0b771ac Mon Sep 17 00:00:00 2001 From: greyes Date: Fri, 16 Aug 2024 10:42:46 +0200 Subject: [PATCH 1/3] Added util to convert encoders and millimeters per second --- .../src/MotorSpeedCalibrationState.cpp | 3 +- .../src/MotorSpeedCalibrationState.cpp | 2 +- lib/Service/src/Util.cpp | 12 ++++++++ lib/Service/src/Util.h | 30 +++++++++++++++---- 4 files changed, 38 insertions(+), 9 deletions(-) diff --git a/lib/APPCalib/src/MotorSpeedCalibrationState.cpp b/lib/APPCalib/src/MotorSpeedCalibrationState.cpp index 1d80eef7..77d47017 100644 --- a/lib/APPCalib/src/MotorSpeedCalibrationState.cpp +++ b/lib/APPCalib/src/MotorSpeedCalibrationState.cpp @@ -227,8 +227,7 @@ void MotorSpeedCalibrationState::finishCalibration(StateMachine& sm) } else { - int32_t maxSpeed32 = - static_cast(maxSpeed) * 1000 / static_cast(RobotConstants::ENCODER_STEPS_PER_M); + int32_t maxSpeed32 = Util::stepsPerSecondToMillimetersPerSecond(maxSpeed); LOG_INFO_VAL("Calibrated max. speed (steps/s): ", maxSpeed); LOG_INFO_VAL("Calibrated max. speed (mm/s): ", maxSpeed32); diff --git a/lib/APPLineFollower/src/MotorSpeedCalibrationState.cpp b/lib/APPLineFollower/src/MotorSpeedCalibrationState.cpp index f9547662..133ad99c 100644 --- a/lib/APPLineFollower/src/MotorSpeedCalibrationState.cpp +++ b/lib/APPLineFollower/src/MotorSpeedCalibrationState.cpp @@ -227,7 +227,7 @@ void MotorSpeedCalibrationState::finishCalibration(StateMachine& sm) } else { - int32_t maxSpeed32 = static_cast(maxSpeed) * 1000 / static_cast(RobotConstants::ENCODER_STEPS_PER_M); + int32_t maxSpeed32 = Util::stepsPerSecondToMillimetersPerSecond(maxSpeed); LOG_INFO_VAL("Calibrated max. speed (steps/s): ", maxSpeed); LOG_INFO_VAL("Calibrated max. speed (mm/s): ", maxSpeed32); diff --git a/lib/Service/src/Util.cpp b/lib/Service/src/Util.cpp index 84d7d608..621ecd86 100644 --- a/lib/Service/src/Util.cpp +++ b/lib/Service/src/Util.cpp @@ -33,6 +33,7 @@ * Includes *****************************************************************************/ #include +#include /****************************************************************************** * Compiler Switches @@ -210,6 +211,17 @@ bool Util::isButtonTriggered(IButton& button, bool& lastState) return isTriggered; } +int32_t Util::stepsPerSecondToMillimetersPerSecond(int16_t speedStepsPerSec) +{ + return (static_cast(speedStepsPerSec) * 1000 / static_cast(RobotConstants::ENCODER_STEPS_PER_M)); +} + +int16_t Util::millimetersPerSecondToStepsPerSecond(int32_t speedMmPerSec) +{ + int32_t speedStepsPerSec = speedMmPerSec * static_cast(RobotConstants::ENCODER_STEPS_PER_M); + return (static_cast(speedStepsPerSec / 1000)); +} + /****************************************************************************** * Local Functions *****************************************************************************/ diff --git a/lib/Service/src/Util.h b/lib/Service/src/Util.h index df96fa15..3a94d1fa 100644 --- a/lib/Service/src/Util.h +++ b/lib/Service/src/Util.h @@ -85,20 +85,20 @@ namespace Util /** * Divide and round. - * + * * @param[in] numerator The numerator. * @param[in] denominator The denominator. - * + * * @return Result */ uint32_t divRoundUp(uint32_t numerator, uint32_t denominator); /** * Divide and round. - * + * * @param[in] numerator The numerator. * @param[in] denominator The denominator. - * + * * @return Result */ int32_t divRoundUp(int32_t numerator, int32_t denominator); @@ -106,14 +106,32 @@ namespace Util /** * Is button triggered? * Triggered means a pressed/released change. - * + * * @param[in] button The button. * @param[inout] lastState The last button state. - * + * * @return If button is triggered, it will return true otherwise false. */ bool isButtonTriggered(IButton& button, bool& lastState); + /** + * Convert a speed in encoder steps per second to a speed in mm/s. + * + * @param[in] speedStepsPerSec Speed in encoder steps per second + * + * @return Speed in mm/s + */ + int32_t stepsPerSecondToMillimetersPerSecond(int16_t speedStepsPerSec); + + /** + * Convert a speed in mm/s to a speed in encoder steps per second. + * + * @param[in] speedMmPerSec Speed in mm/s + * + * @return Speed in encoder steps per second + */ + int16_t millimetersPerSecondToStepsPerSecond(int32_t speedMmPerSec); + } /* namespace Util */ #endif /* UTIL_H */ From 8784e9f516d2af8357a0e54cd17c95d76be8229a Mon Sep 17 00:00:00 2001 From: greyes Date: Fri, 16 Aug 2024 11:05:03 +0200 Subject: [PATCH 2/3] Applications send a receive speeds in mm/s instead of steps/s --- lib/APPConvoyFollower/src/App.cpp | 17 ++++++++++++----- lib/APPConvoyFollower/src/SerialMuxChannels.h | 14 +++++++------- lib/APPConvoyLeader/src/App.cpp | 16 +++++++++++----- lib/APPConvoyLeader/src/SerialMuxChannels.h | 14 +++++++------- lib/APPRemoteControl/src/App.cpp | 13 +++++++++---- lib/APPRemoteControl/src/SerialMuxChannels.h | 14 +++++++------- 6 files changed, 53 insertions(+), 35 deletions(-) diff --git a/lib/APPConvoyFollower/src/App.cpp b/lib/APPConvoyFollower/src/App.cpp index 76253f32..eb480a5f 100644 --- a/lib/APPConvoyFollower/src/App.cpp +++ b/lib/APPConvoyFollower/src/App.cpp @@ -42,6 +42,7 @@ #include #include #include +#include /****************************************************************************** * Compiler Switches @@ -189,7 +190,8 @@ void App::handleRemoteCommand(const Command& cmd) break; case SMPChannelPayload::CmdId::CMD_ID_GET_MAX_SPEED: - rsp.maxMotorSpeed = Board::getInstance().getSettings().getMaxSpeed(); + rsp.maxMotorSpeed = + Util::stepsPerSecondToMillimetersPerSecond(Board::getInstance().getSettings().getMaxSpeed()); break; case SMPChannelPayload::CmdId::CMD_ID_SET_INIT_POS: @@ -252,6 +254,9 @@ void App::reportVehicleData() uint8_t averageCounts = 0U; uint8_t leftCounts = 0U; uint8_t rightCounts = 0U; + int16_t leftSpeed = speedometer.getLinearSpeedLeft(); + int16_t rightSpeed = speedometer.getLinearSpeedRight(); + int16_t centerSpeed = speedometer.getLinearSpeedCenter(); proximitySensors.read(); leftCounts = proximitySensors.countsFrontWithLeftLeds(); @@ -265,9 +270,9 @@ void App::reportVehicleData() payload.xPos = xPos; payload.yPos = yPos; payload.orientation = odometry.getOrientation(); - payload.left = speedometer.getLinearSpeedLeft(); - payload.right = speedometer.getLinearSpeedRight(); - payload.center = speedometer.getLinearSpeedCenter(); + payload.left = Util::stepsPerSecondToMillimetersPerSecond(leftSpeed); + payload.right = Util::stepsPerSecondToMillimetersPerSecond(rightSpeed); + payload.center = Util::stepsPerSecondToMillimetersPerSecond(centerSpeed); payload.proximity = static_cast(averageCounts); /* Ignoring return value, as error handling is not available. */ @@ -359,7 +364,9 @@ void App_motorSpeedSetpointsChannelCallback(const uint8_t* payload, const uint8_ if ((nullptr != payload) && (SPEED_SETPOINT_CHANNEL_DLC == payloadSize)) { const SpeedData* motorSpeedData = reinterpret_cast(payload); - DrivingState::getInstance().setTargetSpeeds(motorSpeedData->left, motorSpeedData->right); + int16_t leftSpeed = Util::millimetersPerSecondToStepsPerSecond(motorSpeedData->left); + int16_t rightSpeed = Util::millimetersPerSecondToStepsPerSecond(motorSpeedData->right); + DrivingState::getInstance().setTargetSpeeds(leftSpeed, rightSpeed); } } diff --git a/lib/APPConvoyFollower/src/SerialMuxChannels.h b/lib/APPConvoyFollower/src/SerialMuxChannels.h index c79b9754..29838c30 100644 --- a/lib/APPConvoyFollower/src/SerialMuxChannels.h +++ b/lib/APPConvoyFollower/src/SerialMuxChannels.h @@ -169,16 +169,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. */ @@ -187,9 +187,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; diff --git a/lib/APPConvoyLeader/src/App.cpp b/lib/APPConvoyLeader/src/App.cpp index b4ad13aa..a09da528 100644 --- a/lib/APPConvoyLeader/src/App.cpp +++ b/lib/APPConvoyLeader/src/App.cpp @@ -42,6 +42,7 @@ #include #include #include +#include /****************************************************************************** * Compiler Switches @@ -182,7 +183,8 @@ void App::handleRemoteCommand(const Command& cmd) switch (cmd.commandId) { case SMPChannelPayload::CmdId::CMD_ID_GET_MAX_SPEED: - rsp.maxMotorSpeed = ParameterSets::getInstance().getParameterSet().topSpeed; + rsp.maxMotorSpeed = + Util::stepsPerSecondToMillimetersPerSecond(ParameterSets::getInstance().getParameterSet().topSpeed); break; case SMPChannelPayload::CmdId::CMD_ID_SET_INIT_POS: @@ -245,6 +247,9 @@ void App::reportVehicleData() uint8_t averageCounts = 0U; uint8_t leftCounts = 0U; uint8_t rightCounts = 0U; + int16_t leftSpeed = speedometer.getLinearSpeedLeft(); + int16_t rightSpeed = speedometer.getLinearSpeedRight(); + int16_t centerSpeed = speedometer.getLinearSpeedCenter(); proximitySensors.read(); leftCounts = proximitySensors.countsFrontWithLeftLeds(); @@ -258,9 +263,9 @@ void App::reportVehicleData() payload.xPos = xPos; payload.yPos = yPos; payload.orientation = odometry.getOrientation(); - payload.left = speedometer.getLinearSpeedLeft(); - payload.right = speedometer.getLinearSpeedRight(); - payload.center = speedometer.getLinearSpeedCenter(); + payload.left = Util::stepsPerSecondToMillimetersPerSecond(leftSpeed); + payload.right = Util::stepsPerSecondToMillimetersPerSecond(rightSpeed); + payload.center = Util::stepsPerSecondToMillimetersPerSecond(centerSpeed); payload.proximity = static_cast(averageCounts); /* Ignoring return value, as error handling is not available. */ @@ -331,7 +336,8 @@ void App_motorSpeedSetpointsChannelCallback(const uint8_t* payload, const uint8_ if ((nullptr != payload) && (SPEED_SETPOINT_CHANNEL_DLC == payloadSize)) { const SpeedData* motorSpeedData = reinterpret_cast(payload); - DrivingState::getInstance().setTopSpeed(motorSpeedData->center); + int16_t topSpeed = Util::millimetersPerSecondToStepsPerSecond(motorSpeedData->center); + DrivingState::getInstance().setTopSpeed(topSpeed); } } diff --git a/lib/APPConvoyLeader/src/SerialMuxChannels.h b/lib/APPConvoyLeader/src/SerialMuxChannels.h index a4440d77..396586e4 100644 --- a/lib/APPConvoyLeader/src/SerialMuxChannels.h +++ b/lib/APPConvoyLeader/src/SerialMuxChannels.h @@ -163,16 +163,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. */ @@ -181,9 +181,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; diff --git a/lib/APPRemoteControl/src/App.cpp b/lib/APPRemoteControl/src/App.cpp index 76253f32..28675f2d 100644 --- a/lib/APPRemoteControl/src/App.cpp +++ b/lib/APPRemoteControl/src/App.cpp @@ -42,6 +42,7 @@ #include #include #include +#include /****************************************************************************** * Compiler Switches @@ -189,7 +190,8 @@ void App::handleRemoteCommand(const Command& cmd) break; case SMPChannelPayload::CmdId::CMD_ID_GET_MAX_SPEED: - rsp.maxMotorSpeed = Board::getInstance().getSettings().getMaxSpeed(); + rsp.maxMotorSpeed = + Util::stepsPerSecondToMillimetersPerSecond(Board::getInstance().getSettings().getMaxSpeed()); break; case SMPChannelPayload::CmdId::CMD_ID_SET_INIT_POS: @@ -252,6 +254,9 @@ void App::reportVehicleData() uint8_t averageCounts = 0U; uint8_t leftCounts = 0U; uint8_t rightCounts = 0U; + int16_t leftSpeed = speedometer.getLinearSpeedLeft(); + int16_t rightSpeed = speedometer.getLinearSpeedRight(); + int16_t centerSpeed = speedometer.getLinearSpeedCenter(); proximitySensors.read(); leftCounts = proximitySensors.countsFrontWithLeftLeds(); @@ -265,9 +270,9 @@ void App::reportVehicleData() payload.xPos = xPos; payload.yPos = yPos; payload.orientation = odometry.getOrientation(); - payload.left = speedometer.getLinearSpeedLeft(); - payload.right = speedometer.getLinearSpeedRight(); - payload.center = speedometer.getLinearSpeedCenter(); + payload.left = Util::stepsPerSecondToMillimetersPerSecond(leftSpeed); + payload.right = Util::stepsPerSecondToMillimetersPerSecond(rightSpeed); + payload.center = Util::stepsPerSecondToMillimetersPerSecond(centerSpeed); payload.proximity = static_cast(averageCounts); /* Ignoring return value, as error handling is not available. */ diff --git a/lib/APPRemoteControl/src/SerialMuxChannels.h b/lib/APPRemoteControl/src/SerialMuxChannels.h index c79b9754..29838c30 100644 --- a/lib/APPRemoteControl/src/SerialMuxChannels.h +++ b/lib/APPRemoteControl/src/SerialMuxChannels.h @@ -169,16 +169,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. */ @@ -187,9 +187,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 9570fe8d9eacba0773c79d2f25dfa2bc8b7f7c23 Mon Sep 17 00:00:00 2001 From: greyes Date: Fri, 16 Aug 2024 13:06:26 +0200 Subject: [PATCH 3/3] Moved utils from source to header file to aid compiler optimization --- lib/Service/src/Util.cpp | 12 ------------ lib/Service/src/Util.h | 13 +++++++++++-- 2 files changed, 11 insertions(+), 14 deletions(-) diff --git a/lib/Service/src/Util.cpp b/lib/Service/src/Util.cpp index 621ecd86..84d7d608 100644 --- a/lib/Service/src/Util.cpp +++ b/lib/Service/src/Util.cpp @@ -33,7 +33,6 @@ * Includes *****************************************************************************/ #include -#include /****************************************************************************** * Compiler Switches @@ -211,17 +210,6 @@ bool Util::isButtonTriggered(IButton& button, bool& lastState) return isTriggered; } -int32_t Util::stepsPerSecondToMillimetersPerSecond(int16_t speedStepsPerSec) -{ - return (static_cast(speedStepsPerSec) * 1000 / static_cast(RobotConstants::ENCODER_STEPS_PER_M)); -} - -int16_t Util::millimetersPerSecondToStepsPerSecond(int32_t speedMmPerSec) -{ - int32_t speedStepsPerSec = speedMmPerSec * static_cast(RobotConstants::ENCODER_STEPS_PER_M); - return (static_cast(speedStepsPerSec / 1000)); -} - /****************************************************************************** * Local Functions *****************************************************************************/ diff --git a/lib/Service/src/Util.h b/lib/Service/src/Util.h index 3a94d1fa..566a3e55 100644 --- a/lib/Service/src/Util.h +++ b/lib/Service/src/Util.h @@ -46,6 +46,7 @@ #include #include #include +#include /** * Utilities @@ -121,7 +122,11 @@ namespace Util * * @return Speed in mm/s */ - int32_t stepsPerSecondToMillimetersPerSecond(int16_t speedStepsPerSec); + inline int32_t stepsPerSecondToMillimetersPerSecond(int16_t speedStepsPerSec) + { + return (static_cast(speedStepsPerSec) * 1000 / + static_cast(RobotConstants::ENCODER_STEPS_PER_M)); + } /** * Convert a speed in mm/s to a speed in encoder steps per second. @@ -130,7 +135,11 @@ namespace Util * * @return Speed in encoder steps per second */ - int16_t millimetersPerSecondToStepsPerSecond(int32_t speedMmPerSec); + inline int16_t millimetersPerSecondToStepsPerSecond(int32_t speedMmPerSec) + { + int32_t speedStepsPerSec = speedMmPerSec * static_cast(RobotConstants::ENCODER_STEPS_PER_M); + return (static_cast(speedStepsPerSec / 1000)); + } } /* namespace Util */