Constrain speed setpoints to maximum motor speed

BlueAndi · Oct 29, 2023 · 0d7cd96 · 0d7cd96
1 parent 30c5cf8
commit 0d7cd96
Showing 1 changed file with 3 additions and 3 deletions.
diff --git a/lib/Service/DifferentialDrive.cpp b/lib/Service/DifferentialDrive.cpp
@@ -81,7 +81,7 @@ int16_t DifferentialDrive::getLinearSpeed() const
 
 void DifferentialDrive::setLinearSpeed(int16_t linearSpeed)
 {
-    m_linearSpeedCenterSetPoint = linearSpeed;
+    m_linearSpeedCenterSetPoint = constrain(linearSpeed, -m_maxMotorSpeed, m_maxMotorSpeed);
     calculateLinearSpeedLeftRight(m_linearSpeedCenterSetPoint, m_angularSpeedSetPoint, m_linearSpeedLeftSetPoint,
                                   m_linearSpeedRightSetPoint);
 }
@@ -94,8 +94,8 @@ void DifferentialDrive::getLinearSpeed(int16_t& linearSpeedLeft, int16_t& linear
 
 void DifferentialDrive::setLinearSpeed(int16_t linearSpeedLeft, int16_t linearSpeedRight)
 {
-    m_linearSpeedLeftSetPoint  = linearSpeedLeft;
-    m_linearSpeedRightSetPoint = linearSpeedRight;
+    m_linearSpeedLeftSetPoint  = constrain(linearSpeedLeft, -m_maxMotorSpeed, m_maxMotorSpeed);
+    m_linearSpeedRightSetPoint = constrain(linearSpeedRight, -m_maxMotorSpeed, m_maxMotorSpeed);
     calculateLinearAndAngularSpeedCenter(m_linearSpeedLeftSetPoint, m_linearSpeedRightSetPoint,
                                          m_linearSpeedCenterSetPoint, m_angularSpeedSetPoint);
 }