From 05711d1eba19bf9df16d4f0498e784c4ae4046ae Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Sat, 25 May 2019 19:35:15 +0200 Subject: [PATCH] Bring the anti-hunt functionality back --- Firmware/MotorControl/controller.cpp | 10 ++++++++-- Firmware/MotorControl/controller.hpp | 4 ++++ 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index d295246c..83c96fc9 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -142,6 +142,7 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s // Position control // TODO Decide if we want to use encoder or pll position here + float gain_scheduling_multiplier = 1.0f; float vel_des = vel_setpoint_; if (config_.control_mode >= CTRL_MODE_POSITION_CONTROL) { float pos_err; @@ -158,6 +159,11 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s pos_err = pos_setpoint_ - pos_estimate; } vel_des += config_.pos_gain * pos_err; + // V-shaped gain shedule based on position error + float abs_pos_err = fabsf(pos_err); + if (config_.enable_gain_scheduling && abs_pos_err <= config_.gain_scheduling_width) { + gain_scheduling_multiplier = abs_pos_err / config_.gain_scheduling_width; + } } // Velocity limiting @@ -185,7 +191,7 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s float v_err = vel_des - vel_estimate; if (config_.control_mode >= CTRL_MODE_VELOCITY_CONTROL) { - Iq += config_.vel_gain * v_err; + Iq += (config_.vel_gain * gain_scheduling_multiplier) * v_err; } // Velocity integral action before limiting @@ -212,7 +218,7 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s // TODO make decayfactor configurable vel_integrator_current_ *= 0.99f; } else { - vel_integrator_current_ += (config_.vel_integrator_gain * current_meas_period) * v_err; + vel_integrator_current_ += ((config_.vel_integrator_gain * gain_scheduling_multiplier) * current_meas_period) * v_err; } } diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 020f34d0..16674d90 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -32,6 +32,8 @@ public: float vel_limit_tolerance = 1.2f; // ratio to vel_lim. 0.0f to disable float vel_ramp_rate = 10000.0f; // [(counts/s) / s] bool setpoints_in_cpr = false; + float gain_scheduling_width = 10.0f; + bool enable_gain_scheduling = false; }; explicit Controller(Config_t& config); @@ -102,6 +104,8 @@ public: make_protocol_property("current_setpoint", ¤t_setpoint_), make_protocol_property("vel_ramp_target", &vel_ramp_target_), make_protocol_property("vel_ramp_enable", &vel_ramp_enable_), + make_protocol_property("gain_scheduling_width", &config_.gain_scheduling_width), + make_protocol_property("enable_gain_scheduling", &config_.enable_gain_scheduling), make_protocol_object("config", make_protocol_property("control_mode", &config_.control_mode), make_protocol_property("pos_gain", &config_.pos_gain),