Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-21 15:34:33 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge anti_hunt

Browse Source
This commit is contained in:
Paul Guenette
2019-05-25 19:38:52 +02:00
parent 9e2476ed65 05711d1eba
commit 73df226fc3
2 changed files with 12 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/controller.cpp
+8 -2
View File
@@ -181,6 +181,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;
@@ -197,6 +198,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
@@ -224,7 +230,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
@@ -251,7 +257,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;
}
}
Firmware/MotorControl/controller.hpp
+4
View File
@@ -56,6 +56,8 @@ public:
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;
};
explicit Controller(Config_t& config);
@@ -120,6 +122,8 @@ public:
make_protocol_ro_property("trajectory_done", &trajectory_done_),
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("control_mode", &config_.control_mode),
make_protocol_property("input_mode", &config_.input_mode),