Revert wrong calculations

BlueAndi · Aug 21, 2024 · 3ac917b · 3ac917b
1 parent 6f167f7
commit 3ac917b
Showing 1 changed file with 5 additions and 6 deletions.
diff --git a/lib/Service/src/DifferentialDrive.cpp b/lib/Service/src/DifferentialDrive.cpp
@@ -245,19 +245,18 @@ 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] */
-    int32_t encoderSteps32      = static_cast<int32_t>(RobotConstants::ENCODER_STEPS_PER_M); /* [steps/m] */
+    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 / 2) - ((angularSpeed32 / 1000) * (wheelBase32) * (encoderSteps32 / 1000));
-    linearSpeedRight = (linearSpeedCenter32 / 2) + ((angularSpeed32 / 1000) * (wheelBase32) * (encoderSteps32 / 1000));
+    linearSpeedLeft  = linearSpeedCenter32 - (angularSpeed32 * wheelBase32) / 2;
+    linearSpeedRight = linearSpeedCenter32 + (angularSpeed32 * wheelBase32) / 2;
 }
 
 void DifferentialDrive::calculateLinearAndAngularSpeedCenter(int16_t linearSpeedLeft, int16_t linearSpeedRight,