From f04f02aff7e3900991487499406394abfe2007d5 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Wed, 1 Dec 2021 17:41:00 -0500 Subject: [PATCH] Add input_xxx to the setpoints for autotuning --- Firmware/MotorControl/controller.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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);