diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp
index 546847ed..e17e8629 100644
--- a/Firmware/MotorControl/controller.cpp
+++ b/Firmware/MotorControl/controller.cpp
@@ -226,9 +226,9 @@ bool Controller::update() {
             autotuning_phase_ = wrap_pm_pi(autotuning_phase_ + (2.0f * M_PI * autotuning_.frequency * current_meas_period));
             float c = our_arm_cos_f32(autotuning_phase_);
             float s = our_arm_sin_f32(autotuning_phase_);
-            pos_setpoint_ = autotuning_.pos_amplitude * s; // + pos_amp_c * c
-            vel_setpoint_ = autotuning_.vel_amplitude * c;
-            torque_setpoint_ = autotuning_.torque_amplitude * -s;
+            pos_setpoint_ = input_pos_ + autotuning_.pos_amplitude * s; // + pos_amp_c * c
+            vel_setpoint_ = input_vel_ + autotuning_.vel_amplitude * c;
+            torque_setpoint_ = input_torque_ + autotuning_.torque_amplitude * -s;
         } break;
         default: {
             set_error(ERROR_INVALID_INPUT_MODE);