From 63bebaa8dea9adcf8e2fbcd64eb420a4e4948e3b Mon Sep 17 00:00:00 2001 From: Unknown Date: Sat, 24 Aug 2019 21:44:23 -0400 Subject: [PATCH] Add "enable" switches for controller options --- Firmware/MotorControl/controller.cpp | 14 ++++++++------ Firmware/MotorControl/controller.hpp | 16 ++++++++++++---- 2 files changed, 20 insertions(+), 10 deletions(-) diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index a4d1355b..db7d3642 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -222,12 +222,14 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s // Velocity limiting float vel_lim = config_.vel_limit; - if (vel_des > vel_lim) vel_des = vel_lim; - if (vel_des < -vel_lim) vel_des = -vel_lim; + if (config_.enable_vel_limit) { + if (vel_des > vel_lim) vel_des = vel_lim; + if (vel_des < -vel_lim) vel_des = -vel_lim; + } // Check for overspeed fault (done in this module (controller) for cohesion with vel_lim) - if (config_.vel_limit_tolerance > 0.0f) { // 0.0f to disable - if (fabsf(vel_estimate) > config_.vel_limit_tolerance * vel_lim) { + if (config_.enable_overspeed_error) { // 0.0f to disable + if (std::abs(vel_estimate) > config_.vel_limit_tolerance * vel_lim) { set_error(ERROR_OVERSPEED); return false; } @@ -239,7 +241,7 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s // Anti-cogging is enabled after calibration // We get the current position and apply a current feed-forward // ensuring that we handle negative encoder positions properly (-1 == motor->encoder.encoder_cpr - 1) - if (anticogging_valid_) { + if (anticogging_valid_ && config_.anticogging.enable) { Iq += config_.anticogging.cogging_map[std::clamp(mod(static_cast(anticogging_pos), 3600), 0, 3600)]; } @@ -252,7 +254,7 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s Iq += vel_integrator_current_; // Velocity limiting in current mode - if (config_.control_mode < CTRL_MODE_VELOCITY_CONTROL && config_.vel_limit > 0.0f) { + if (config_.control_mode < CTRL_MODE_VELOCITY_CONTROL && config_.enable_current_vel_limit) { Iq = limitVel(config_.vel_limit, vel_estimate, config_.vel_gain, Iq); } diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 502b097a..a2370f49 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -40,6 +40,7 @@ class Controller { float calib_pos_threshold = 1.0f; float calib_vel_threshold = 1.0f; float cogging_ratio = 1.0f; + bool enable = true; } Anticogging_t; struct Config_t { @@ -57,8 +58,11 @@ class Controller { float input_filter_bandwidth = 2.0f; // [1/s] float homing_speed = 2000.0f; // [counts/s] Anticogging_t anticogging; - float gain_scheduling_width = 10.0f; - bool enable_gain_scheduling = false; + float gain_scheduling_width = 10.0f; + bool enable_gain_scheduling = false; + bool enable_vel_limit = true; + bool enable_overspeed_error = true; + bool enable_current_vel_limit = true; }; explicit Controller(Config_t& config); @@ -125,8 +129,11 @@ class Controller { make_protocol_property("vel_integrator_current", &vel_integrator_current_), make_protocol_property("anticogging_valid", &anticogging_valid_), 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("enable_vel_limit", &config_.enable_vel_limit), + make_protocol_property("enable_current_mode_vel_limit", &config_.enable_current_vel_limit), + make_protocol_property("enable_gain_scheduling", &config_.enable_gain_scheduling), + make_protocol_property("enable_overspeed_error", &config_.enable_overspeed_error), make_protocol_property("control_mode", &config_.control_mode), make_protocol_property("input_mode", &config_.input_mode), make_protocol_property("pos_gain", &config_.pos_gain), @@ -145,7 +152,8 @@ class Controller { make_protocol_ro_property("calib_anticogging", &config_.anticogging.calib_anticogging), make_protocol_property("calib_pos_threshold", &config_.anticogging.calib_pos_threshold), make_protocol_property("calib_vel_threshold", &config_.anticogging.calib_vel_threshold), - make_protocol_ro_property("cogging_ratio", &config_.anticogging.cogging_ratio))), + make_protocol_ro_property("cogging_ratio", &config_.anticogging.cogging_ratio), + make_protocol_property("anticogging_enabled", &config_.anticogging.enable))), make_protocol_function("move_incremental", *this, &Controller::move_incremental, "displacement", "from_goal_point"), make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration), make_protocol_function("home_axis", *this, &Controller::home_axis));