DiffDrive:: Angular speed calculation fixed

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)

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<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,
@@ -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] */
 }
 
 /******************************************************************************