diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp
index 2dcd90a4..0080c789 100644
--- a/Firmware/MotorControl/controller.cpp
+++ b/Firmware/MotorControl/controller.cpp
@@ -138,14 +138,10 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s
             current_setpoint_ = input_current_;
         } break;
         case INPUT_MODE_VEL_RAMP: {
-            float max_step_size = current_meas_period * config_.vel_ramp_rate;
+            float max_step_size = std::abs(current_meas_period * config_.vel_ramp_rate);
             float full_step = input_vel_ - vel_setpoint_;
-            float step;
-            if (fabsf(full_step) > max_step_size) {
-                step = std::copysignf(max_step_size, full_step);
-            } else {
-                step = full_step;
-            }
+            float step = std::clamp(full_step, -max_step_size, max_step_size);
+
             vel_setpoint_ += step;
             current_setpoint_ = step / current_meas_period * config_.inertia;
         } break;
diff --git a/Firmware/Tests/test_runner.cpp b/Firmware/Tests/test_runner.cpp
index 71450787..0b116fc6 100644
--- a/Firmware/Tests/test_runner.cpp
+++ b/Firmware/Tests/test_runner.cpp
@@ -274,4 +274,46 @@ using doctest::Approx;
         CHECK(limitVel(20000.0f, 1000.0f, 5.0E-4f, 30.0f) == 9.5f);
         CHECK(limitVel(20000.0f, -1000.0f, 5.0E-4f, 30.0f) == Approx(10.5f));
     }
+}
+
+TEST_SUITE("vel_ramp") {
+    float vel_ramp_old(float input_vel_, float vel_setpoint_, float vel_ramp_rate) {
+        float max_step_size = 0.000125f * vel_ramp_rate;
+        float full_step     = input_vel_ - vel_setpoint_;
+        float step;
+        if (fabsf(full_step) > max_step_size) {
+            step = std::copysignf(max_step_size, full_step);
+        } else {
+            step = full_step;
+        }
+        return step;
+    }
+
+    float vel_ramp_new(float input_vel_, float vel_setpoint_, float vel_ramp_rate){
+        float max_step_size = 0.000125f * vel_ramp_rate;
+        float full_step     = input_vel_ - vel_setpoint_;
+        return std::clamp(full_step, -max_step_size, max_step_size);
+    }
+
+    TEST_CASE("Blah") {
+        float vel_setpoint = 0.0f;
+        float vel_ramp_rate = 8000;
+        float input_vel = 0.0f;
+        CHECK(vel_ramp_old(input_vel, vel_setpoint, vel_ramp_rate) == vel_ramp_new(input_vel, vel_setpoint, vel_ramp_rate));
+
+        input_vel = 10.0f;
+        CHECK(vel_ramp_old(input_vel, vel_setpoint, vel_ramp_rate) == vel_ramp_new(input_vel, vel_setpoint, vel_ramp_rate));
+
+        input_vel = 10000.0f;
+        CHECK(vel_ramp_old(input_vel, vel_setpoint, vel_ramp_rate) == vel_ramp_new(input_vel, vel_setpoint, vel_ramp_rate));
+
+        input_vel = -10000.0f;
+        CHECK(vel_ramp_old(input_vel, vel_setpoint, vel_ramp_rate) == vel_ramp_new(input_vel, vel_setpoint, vel_ramp_rate));
+
+        input_vel = -0.1234f;
+        CHECK(vel_ramp_old(input_vel, vel_setpoint, vel_ramp_rate) == vel_ramp_new(input_vel, vel_setpoint, vel_ramp_rate));
+
+        input_vel = 0.1234f;
+        CHECK(vel_ramp_old(input_vel, vel_setpoint, vel_ramp_rate) == vel_ramp_new(input_vel, vel_setpoint, vel_ramp_rate));
+    }
 }
\ No newline at end of file