From 1df025cedafb46fcd70714321efd3b9d71ce87b3 Mon Sep 17 00:00:00 2001 From: BlueAndi Date: Wed, 21 Aug 2024 00:49:48 +0200 Subject: [PATCH 1/2] Calculation of the robot angular speed from the linear speed left and right fixed. #148 --- doc/architecture/README.md | 43 +++++++++++++++++++++++++++ lib/Service/src/DifferentialDrive.cpp | 31 ++++++++----------- 2 files changed, 55 insertions(+), 19 deletions(-) diff --git a/doc/architecture/README.md b/doc/architecture/README.md index 5c0786c5..bc747270 100644 --- a/doc/architecture/README.md +++ b/doc/architecture/README.md @@ -58,6 +58,49 @@ The following applications are supported: ![differentialDrive](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/DifferentialDrive.plantuml) +Robot angular speed vs wheel linear speed\ +$v [\frac{mm}{s}] = \frac{w_r [\frac{rad}{s}] \cdot W [mm]}{2}$ + +* $v$: The linear speed of one wheel relative to the other $[\frac{mm}{s}]$. +* $w_r$: The robot's angular speed $[\frac{rad}{s}]$. +* $W$: The robot's wheel base $[mm]$. +* $v_L = -v_R$ by rotation about midpoint of the wheel axis. + +Linear speed left\ +$v_L [\frac{mm}{s}] = -\frac{w_r [\frac{rad}{s}] \cdot W [mm]}{2}$ + +Linear speed right\ +$v_R [\frac{mm}{s}] = \frac{w_r [\frac{rad}{s}] \cdot W [mm]}{2}$ + +Consider encoder steps per m\ +$v [\frac{steps}{s}] = \frac{w_r [\frac{rad}{s}] \cdot W [mm]}{2} \cdot \frac{ENC [\frac{steps}{m}]}{1000}$ + +* $v$: The linear speed of one wheel relative to the other $[\frac{steps}{s}]$. +* $w_r$: The robot's angular speed $[\frac{rad}{s}]$. +* $W$: The robot's wheel base $[mm]$. +* $ENC$: The number of encoder steps per m $[\frac{steps}{m}]$. + +Linear speed left\ +$v_L [\frac{steps}{s}] = -\frac{w_r [\frac{rad}{s}] \cdot W [mm]}{2} \cdot \frac{ENC [\frac{steps}{m}]}{1000}$ + +Linear speed right\ +$v_R [\frac{steps}{s}] = \frac{w_r [\frac{rad}{s}] \cdot W [mm]}{2} \cdot \frac{ENC [\frac{steps}{m}]}{1000}$ + +Consider robot linear speed center\ + +Linear speed left\ +$v_L [\frac{steps}{s}] = \frac{v_{Linear}}{2} [\frac{steps}{s}] -\frac{w_r [\frac{rad}{s}] \cdot W [mm]}{2} \cdot \frac{ENC [\frac{steps}{m}]}{1000}$ + +Linear speed right\ +$v_R [\frac{steps}{s}] = \frac{v_{Linear}}{2} [\frac{steps}{s}] + \frac{w_r [\frac{rad}{s}] \cdot W [mm]}{2} \cdot \frac{ENC [\frac{steps}{m}]}{1000}$ + +Consider angular speed in mrad per s\ +Linear speed left\ +$v_L [\frac{steps}{s}] = \frac{v_{Linear}}{2} [\frac{steps}{s}] -\frac{w_r [\frac{mrad}{s}] \cdot W [mm]}{2 \cdot 1000} \cdot \frac{ENC [\frac{steps}{m}]}{1000}$ + +Linear speed right\ +$v_R [\frac{steps}{s}] = \frac{v_{Linear}}{2} [\frac{steps}{s}] + \frac{w_r [\frac{mrad}{s}] \cdot W [mm]}{2 \cdot 1000} \cdot \frac{ENC [\frac{steps}{m}]}{1000}$ + #### Odometry ![odometry](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/architecture/uml/LogicalView/Odometry.plantuml) diff --git a/lib/Service/src/DifferentialDrive.cpp b/lib/Service/src/DifferentialDrive.cpp index c8699c23..069ad5fe 100644 --- a/lib/Service/src/DifferentialDrive.cpp +++ b/lib/Service/src/DifferentialDrive.cpp @@ -245,18 +245,16 @@ DifferentialDrive::DifferentialDrive() : void DifferentialDrive::calculateLinearSpeedLeftRight(int16_t linearSpeedCenter, int16_t angularSpeed, int16_t& linearSpeedLeft, int16_t& linearSpeedRight) { - int32_t linearSpeedCenter32 = static_cast(linearSpeedCenter); /* [steps/s] */ - int32_t angularSpeed32 = static_cast(angularSpeed); /* [mrad/s] */ - int32_t wheelBase32 = static_cast(RobotConstants::WHEEL_BASE); /* [mm] */ - - /* angular speed = 2 * (linear speed right - linear speed left ) / wheel base - * linear speed right - linear speed left = angular speed * wheel base / 2 - * - * linear speed right = - linear speed left - */ - - linearSpeedLeft = linearSpeedCenter32 - (angularSpeed32 * wheelBase32) / 2; - linearSpeedRight = linearSpeedCenter32 + (angularSpeed32 * wheelBase32) / 2; + int32_t linearSpeedCenter32 = static_cast(linearSpeedCenter); /* [steps/s] */ + int32_t halfLinearSpeedCenter32 = linearSpeedCenter32 / 2; /* [steps/s] */ + int32_t angularSpeed32 = static_cast(angularSpeed); /* [mrad/s] */ + int32_t wheelBase32 = static_cast(RobotConstants::WHEEL_BASE); /* [mm] */ + int32_t encoderStepsPerM32 = static_cast(RobotConstants::ENCODER_STEPS_PER_M); /* [steps/m] */ + int32_t linearSpeedTurnInPlace32 = + (angularSpeed32 * wheelBase32 * encoderStepsPerM32) / (2 * 1000 * 1000); /* [steps/s] */ + + linearSpeedLeft = halfLinearSpeedCenter32 - linearSpeedTurnInPlace32; /* [steps/s] */ + linearSpeedRight = halfLinearSpeedCenter32 + linearSpeedTurnInPlace32; /* [steps/s] */ } void DifferentialDrive::calculateLinearAndAngularSpeedCenter(int16_t linearSpeedLeft, int16_t linearSpeedRight, @@ -268,13 +266,8 @@ void DifferentialDrive::calculateLinearAndAngularSpeedCenter(int16_t linearSpeed int32_t linearSpeedCenter32 = (linearSpeedRight32 + linearSpeedLeft32) / 2; /* [steps/s] */ int32_t angularSpeed32 = (2 * (linearSpeedRight32 - linearSpeedLeft32)) / wheelBase32; /* [mrad/s] */ - /* linear speed = (linear speed right + linear speed left) / 2 - * - * angular speed = 2 * (linear speed right - linear speed left ) / wheel base - */ - - linearSpeedCenter = static_cast(linearSpeedCenter32); - angularSpeed = static_cast(angularSpeed32); + linearSpeedCenter = static_cast(linearSpeedCenter32); /* [steps/s] */ + angularSpeed = static_cast(angularSpeed32); /* [mrad/s] */ } /****************************************************************************** From 8a411e49f0e013377525ac6f2a93a35cb258695a Mon Sep 17 00:00:00 2001 From: BlueAndi Date: Wed, 21 Aug 2024 00:57:04 +0200 Subject: [PATCH 2/2] Use int32_t number to avoid integer overflow. --- lib/Service/src/DifferentialDrive.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/Service/src/DifferentialDrive.cpp b/lib/Service/src/DifferentialDrive.cpp index 069ad5fe..2ce2c077 100644 --- a/lib/Service/src/DifferentialDrive.cpp +++ b/lib/Service/src/DifferentialDrive.cpp @@ -251,7 +251,7 @@ void DifferentialDrive::calculateLinearSpeedLeftRight(int16_t linearSpeedCenter, int32_t wheelBase32 = static_cast(RobotConstants::WHEEL_BASE); /* [mm] */ int32_t encoderStepsPerM32 = static_cast(RobotConstants::ENCODER_STEPS_PER_M); /* [steps/m] */ int32_t linearSpeedTurnInPlace32 = - (angularSpeed32 * wheelBase32 * encoderStepsPerM32) / (2 * 1000 * 1000); /* [steps/s] */ + (angularSpeed32 * wheelBase32 * encoderStepsPerM32) / static_cast(2000000); /* [steps/s] */ linearSpeedLeft = halfLinearSpeedCenter32 - linearSpeedTurnInPlace32; /* [steps/s] */ linearSpeedRight = halfLinearSpeedCenter32 + linearSpeedTurnInPlace32; /* [steps/s] */