From 90f5c1d0b15b4ae9088a21332fedeed01cfc2f03 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 1 May 2020 16:32:09 +0200 Subject: [PATCH] change velocity clamping in input filter mode as per: https://discordapp.com/channels/369667319280173067/369678934985408524/705227337230188595 --- Firmware/MotorControl/controller.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 3b450277..f4b7f84b 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -110,7 +110,8 @@ bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) } void Controller::update_filter_gains() { - input_filter_ki_ = 2.0f * config_.input_filter_bandwidth; // basic conversion to discrete time + float bandwidth = std::min(config_.input_filter_bandwidth, 0.25f * current_meas_hz); + input_filter_ki_ = 2.0f * bandwidth; // basic conversion to discrete time input_filter_kp_ = 0.25f * (input_filter_ki_ * input_filter_ki_); // Critically damped } @@ -175,7 +176,7 @@ bool Controller::update(float* current_setpoint_output) { float delta_vel = input_vel_ - vel_setpoint_; // Vel error float accel = input_filter_kp_*delta_pos + input_filter_ki_*delta_vel; // Feedback current_setpoint_ = accel * config_.inertia; // Accel - vel_setpoint_ += std::clamp(current_meas_period * accel, 2.0f * std::abs(delta_vel), -2.0f * std::abs(delta_vel)); // delta vel + vel_setpoint_ += current_meas_period * accel; // delta vel pos_setpoint_ += current_meas_period * vel_setpoint_; // Delta pos } break; case INPUT_MODE_MIRROR: {