diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp
index 61a17a38..56e2257d 100644
--- a/Firmware/MotorControl/controller.cpp
+++ b/Firmware/MotorControl/controller.cpp
@@ -116,12 +116,9 @@ void Controller::update_filter_gains() {
 
 namespace {
 float limitVel(float vel_limit, float vel_estimate, float vel_gain, float Iq, float current_lim) {
-    float Imax = (vel_limit - std::abs(vel_estimate)) * vel_gain;
-    if (vel_estimate > 0.0f) {
-        return std::clamp(Iq, -current_lim, Imax);
-    } else {
-        return std::clamp(Iq, -Imax, current_lim);
-    }
+    float Imax = std::min((vel_limit - vel_estimate) * vel_gain, current_lim);
+    float Imin = std::max((-vel_limit - vel_estimate) * vel_gain, -current_lim);
+    return std::clamp(Iq, Imin, Imax);
 }
 }  // namespace