diff --git a/lib/APPCalib/src/MotorSpeedCalibrationState.cpp b/lib/APPCalib/src/MotorSpeedCalibrationState.cpp
index 1d80eef7..77d47017 100644
--- a/lib/APPCalib/src/MotorSpeedCalibrationState.cpp
+++ b/lib/APPCalib/src/MotorSpeedCalibrationState.cpp
@@ -227,8 +227,7 @@ void MotorSpeedCalibrationState::finishCalibration(StateMachine& sm)
     }
     else
     {
-        int32_t maxSpeed32 =
-            static_cast<int32_t>(maxSpeed) * 1000 / static_cast<int32_t>(RobotConstants::ENCODER_STEPS_PER_M);
+        int32_t maxSpeed32 = Util::stepsPerSecondToMillimetersPerSecond(maxSpeed);
 
         LOG_INFO_VAL("Calibrated max. speed (steps/s): ", maxSpeed);
         LOG_INFO_VAL("Calibrated max. speed (mm/s): ", maxSpeed32);
diff --git a/lib/APPLineFollower/src/MotorSpeedCalibrationState.cpp b/lib/APPLineFollower/src/MotorSpeedCalibrationState.cpp
index f9547662..133ad99c 100644
--- a/lib/APPLineFollower/src/MotorSpeedCalibrationState.cpp
+++ b/lib/APPLineFollower/src/MotorSpeedCalibrationState.cpp
@@ -227,7 +227,7 @@ void MotorSpeedCalibrationState::finishCalibration(StateMachine& sm)
     }
     else
     {
-        int32_t maxSpeed32 = static_cast<int32_t>(maxSpeed) * 1000 / static_cast<int32_t>(RobotConstants::ENCODER_STEPS_PER_M);
+        int32_t maxSpeed32 = Util::stepsPerSecondToMillimetersPerSecond(maxSpeed);
 
         LOG_INFO_VAL("Calibrated max. speed (steps/s): ", maxSpeed);
         LOG_INFO_VAL("Calibrated max. speed (mm/s): ", maxSpeed32);
diff --git a/lib/Service/src/Util.cpp b/lib/Service/src/Util.cpp
index 84d7d608..621ecd86 100644
--- a/lib/Service/src/Util.cpp
+++ b/lib/Service/src/Util.cpp
@@ -33,6 +33,7 @@
  * Includes
  *****************************************************************************/
 #include <Util.h>
+#include <RobotConstants.h>
 
 /******************************************************************************
  * Compiler Switches
@@ -210,6 +211,17 @@ bool Util::isButtonTriggered(IButton& button, bool& lastState)
     return isTriggered;
 }
 
+int32_t Util::stepsPerSecondToMillimetersPerSecond(int16_t speedStepsPerSec)
+{
+    return (static_cast<int32_t>(speedStepsPerSec) * 1000 / static_cast<int32_t>(RobotConstants::ENCODER_STEPS_PER_M));
+}
+
+int16_t Util::millimetersPerSecondToStepsPerSecond(int32_t speedMmPerSec)
+{
+    int32_t speedStepsPerSec = speedMmPerSec * static_cast<int32_t>(RobotConstants::ENCODER_STEPS_PER_M);
+    return (static_cast<int16_t>(speedStepsPerSec / 1000));
+}
+
 /******************************************************************************
  * Local Functions
  *****************************************************************************/
diff --git a/lib/Service/src/Util.h b/lib/Service/src/Util.h
index df96fa15..3a94d1fa 100644
--- a/lib/Service/src/Util.h
+++ b/lib/Service/src/Util.h
@@ -85,20 +85,20 @@ namespace Util
 
     /**
      * Divide and round.
-     * 
+     *
      * @param[in] numerator     The numerator.
      * @param[in] denominator   The denominator.
-     * 
+     *
      * @return Result
      */
     uint32_t divRoundUp(uint32_t numerator, uint32_t denominator);
 
     /**
      * Divide and round.
-     * 
+     *
      * @param[in] numerator     The numerator.
      * @param[in] denominator   The denominator.
-     * 
+     *
      * @return Result
      */
     int32_t divRoundUp(int32_t numerator, int32_t denominator);
@@ -106,14 +106,32 @@ namespace Util
     /**
      * Is button triggered?
      * Triggered means a pressed/released change.
-     * 
+     *
      * @param[in]       button      The button.
      * @param[inout]    lastState   The last button state.
-     * 
+     *
      * @return If button is triggered, it will return true otherwise false.
      */
     bool isButtonTriggered(IButton& button, bool& lastState);
 
+    /**
+     * Convert a speed in encoder steps per second to a speed in mm/s.
+     *
+     * @param[in]   speedStepsPerSec Speed in encoder steps per second
+     *
+     * @return Speed in mm/s
+     */
+    int32_t stepsPerSecondToMillimetersPerSecond(int16_t speedStepsPerSec);
+
+    /**
+     * Convert a speed in mm/s to a speed in encoder steps per second.
+     *
+     * @param[in]   speedMmPerSec Speed in mm/s
+     *
+     * @return Speed in encoder steps per second
+     */
+    int16_t millimetersPerSecondToStepsPerSecond(int32_t speedMmPerSec);
+
 } /* namespace Util */
 
 #endif /* UTIL_H */