Skip to content

Commit

Permalink
Sync with release
Browse files Browse the repository at this point in the history
  • Loading branch information
gabryelreyes committed Aug 21, 2024
2 parents 3ac917b + 718b03c commit 40b6bcc
Show file tree
Hide file tree
Showing 2 changed files with 55 additions and 19 deletions.
43 changes: 43 additions & 0 deletions doc/architecture/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
31 changes: 12 additions & 19 deletions lib/Service/src/DifferentialDrive.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<int32_t>(linearSpeedCenter); /* [steps/s] */
int32_t angularSpeed32 = static_cast<int32_t>(angularSpeed); /* [mrad/s] */
int32_t wheelBase32 = static_cast<int32_t>(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<int32_t>(linearSpeedCenter); /* [steps/s] */
int32_t halfLinearSpeedCenter32 = linearSpeedCenter32 / 2; /* [steps/s] */
int32_t angularSpeed32 = static_cast<int32_t>(angularSpeed); /* [mrad/s] */
int32_t wheelBase32 = static_cast<int32_t>(RobotConstants::WHEEL_BASE); /* [mm] */
int32_t encoderStepsPerM32 = static_cast<int32_t>(RobotConstants::ENCODER_STEPS_PER_M); /* [steps/m] */
int32_t linearSpeedTurnInPlace32 =
(angularSpeed32 * wheelBase32 * encoderStepsPerM32) / static_cast<int32_t>(2000000); /* [steps/s] */

linearSpeedLeft = halfLinearSpeedCenter32 - linearSpeedTurnInPlace32; /* [steps/s] */
linearSpeedRight = halfLinearSpeedCenter32 + linearSpeedTurnInPlace32; /* [steps/s] */
}

void DifferentialDrive::calculateLinearAndAngularSpeedCenter(int16_t linearSpeedLeft, int16_t linearSpeedRight,
Expand All @@ -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<int16_t>(linearSpeedCenter32);
angularSpeed = static_cast<int16_t>(angularSpeed32);
linearSpeedCenter = static_cast<int16_t>(linearSpeedCenter32); /* [steps/s] */
angularSpeed = static_cast<int16_t>(angularSpeed32); /* [mrad/s] */
}

/******************************************************************************
Expand Down

0 comments on commit 40b6bcc

Please sign in to comment.