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

make stuff working again, improve state machine design, add underscore to member names

Browse Source
This commit is contained in:
Samuel Sadok
2018-03-09 13:37:07 -08:00
parent f5081352b3
commit 9486f9ed15
21 changed files with 972 additions and 1069 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/controller.cpp
+49 -42
View File
@@ -3,7 +3,7 @@
Controller::Controller(ControllerConfig_t& config) :
config(config)
config_(config)
{}
//--------------------------------
@@ -11,32 +11,39 @@ Controller::Controller(ControllerConfig_t& config) :
//--------------------------------
void Controller::set_pos_setpoint(float pos_setpoint, float vel_feed_forward, float current_feed_forward) {
pos_setpoint = pos_setpoint;
vel_setpoint = vel_feed_forward;
current_setpoint = current_feed_forward;
config.control_mode = CTRL_MODE_POSITION_CONTROL;
pos_setpoint_ = pos_setpoint;
vel_setpoint_ = vel_feed_forward;
current_setpoint_ = current_feed_forward;
config_.control_mode = CTRL_MODE_POSITION_CONTROL;
#ifdef DEBUG_PRINT
printf("POSITION_CONTROL %6.0f %3.3f %3.3f\n", motor->pos_setpoint, motor->vel_setpoint, motor->current_setpoint);
printf("POSITION_CONTROL %6.0f %3.3f %3.3f\n", pos_setpoint, vel_setpoint_, current_setpoint_);
#endif
}
void Controller::set_vel_setpoint(float vel_setpoint, float current_feed_forward) {
vel_setpoint = vel_setpoint;
current_setpoint = current_feed_forward;
config.control_mode = CTRL_MODE_VELOCITY_CONTROL;
vel_setpoint_ = vel_setpoint;
current_setpoint_ = current_feed_forward;
config_.control_mode = CTRL_MODE_VELOCITY_CONTROL;
#ifdef DEBUG_PRINT
printf("VELOCITY_CONTROL %3.3f %3.3f\n", motor->vel_setpoint, motor->current_setpoint);
printf("VELOCITY_CONTROL %3.3f %3.3f\n", vel_setpoint_, motor->current_setpoint_);
#endif
}
void Controller::set_current_setpoint(float current_setpoint) {
current_setpoint = current_setpoint;
config.control_mode = CTRL_MODE_CURRENT_CONTROL;
current_setpoint_ = current_setpoint;
config_.control_mode = CTRL_MODE_CURRENT_CONTROL;
#ifdef DEBUG_PRINT
printf("CURRENT_CONTROL %3.3f\n", motor->current_setpoint);
printf("CURRENT_CONTROL %3.3f\n", current_setpoint_);
#endif
}
void Controller::start_anticogging_calibration() {
// Ensure the cogging map was correctly allocated earlier and that the motor is capable of calibrating
if (anticogging_.cogging_map != NULL && axis_->error_ == Axis::ERROR_NO_ERROR) {
anticogging_.calib_anticogging = true;
}
}
/*
* This anti-cogging implementation iterates through each encoder position,
* waits for zero velocity & position error,
@@ -44,21 +51,21 @@ void Controller::set_current_setpoint(float current_setpoint) {
*
* This holding current is added as a feedforward term in the control loop.
*/
bool Controller::anti_cogging_calibration(float pos_estimate, float vel_estimate) {
if (anticogging.calib_anticogging && anticogging.cogging_map != NULL) {
float pos_err = anticogging.index - pos_estimate;
if (fabsf(pos_err) <= anticogging.calib_pos_threshold &&
fabsf(vel_estimate) < anticogging.calib_vel_threshold) {
anticogging.cogging_map[anticogging.index++] = vel_integrator_current;
bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) {
if (anticogging_.calib_anticogging && anticogging_.cogging_map != NULL) {
float pos_err = anticogging_.index - pos_estimate;
if (fabsf(pos_err) <= anticogging_.calib_pos_threshold &&
fabsf(vel_estimate) < anticogging_.calib_vel_threshold) {
anticogging_.cogging_map[anticogging_.index++] = vel_integrator_current_;
}
if (anticogging.index < axis->encoder.config.cpr) { // TODO: remove the dependency on encoder CPR
set_pos_setpoint(anticogging.index, 0.0f, 0.0f);
if (anticogging_.index < axis_->encoder_.config_.cpr) { // TODO: remove the dependency on encoder CPR
set_pos_setpoint(anticogging_.index, 0.0f, 0.0f);
return false;
} else {
anticogging.index = 0;
anticogging_.index = 0;
set_pos_setpoint(0.0f, 0.0f, 0.0f); // Send the motor home
anticogging.use_anticogging = true; // We're good to go, enable anti-cogging
anticogging.calib_anticogging = false;
anticogging_.use_anticogging = true; // We're good to go, enable anti-cogging
anticogging_.calib_anticogging = false;
return true;
}
}
@@ -66,42 +73,42 @@ bool Controller::anti_cogging_calibration(float pos_estimate, float vel_estimate
}
bool Controller::update(float pos_estimate, float vel_estimate, float* current_setpoint_output) {
// Only runs if anticogging.calib_anticogging is true; non-blocking
anti_cogging_calibration(pos_estimate, vel_estimate);
// Only runs if anticogging_.calib_anticogging is true; non-blocking
anticogging_calibration(pos_estimate, vel_estimate);
// Position control
// TODO Decide if we want to use encoder or pll position here
float vel_des = vel_setpoint;
if (config.control_mode >= CTRL_MODE_POSITION_CONTROL) {
float pos_err = pos_setpoint - pos_estimate;
vel_des += config.pos_gain * pos_err;
float vel_des = vel_setpoint_;
if (config_.control_mode >= CTRL_MODE_POSITION_CONTROL) {
float pos_err = pos_setpoint_ - pos_estimate;
vel_des += config_.pos_gain * pos_err;
}
// Velocity limiting
float vel_lim = config.vel_limit;
float vel_lim = config_.vel_limit;
if (vel_des > vel_lim) vel_des = vel_lim;
if (vel_des < -vel_lim) vel_des = -vel_lim;
// Velocity control
float Iq = current_setpoint;
float Iq = current_setpoint_;
// 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.use_anticogging) {
Iq += anticogging.cogging_map[mod(pos_estimate, axis->encoder.config.cpr)];
if (anticogging_.use_anticogging) {
Iq += anticogging_.cogging_map[mod(pos_estimate, axis_->encoder_.config_.cpr)];
}
float v_err = vel_des - vel_estimate;
if (config.control_mode >= CTRL_MODE_VELOCITY_CONTROL) {
Iq += config.vel_gain * v_err;
if (config_.control_mode >= CTRL_MODE_VELOCITY_CONTROL) {
Iq += config_.vel_gain * v_err;
}
// Velocity integral action before limiting
Iq += vel_integrator_current;
Iq += vel_integrator_current_;
// Current limiting
float Ilim = std::min(axis->motor.config.current_lim, axis->motor.current_control.max_allowed_current);
float Ilim = std::min(axis_->motor_.config_.current_lim, axis_->motor_.current_control_.max_allowed_current);
bool limited = false;
if (Iq > Ilim) {
limited = true;
@@ -113,15 +120,15 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s
}
// Velocity integrator (behaviour dependent on limiting)
if (config.control_mode < CTRL_MODE_VELOCITY_CONTROL) {
if (config_.control_mode < CTRL_MODE_VELOCITY_CONTROL) {
// reset integral if not in use
vel_integrator_current = 0.0f;
vel_integrator_current_ = 0.0f;
} else {
if (limited) {
// TODO make decayfactor configurable
vel_integrator_current *= 0.99f;
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 * current_meas_period) * v_err;
}
}