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

Refactor control loop.

Browse Source 
Please see https://github.com/madcowswe/ODrive/issues/472 for a detailed description.
This commit is contained in:
Samuel Sadok
2020-09-01 00:03:01 +02:00
parent 86be15086f
commit 5661a3b5ed
67 changed files with 2587 additions and 2075 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/encoder.cpp
+113 -66
View File
@@ -64,7 +64,6 @@ void Encoder::set_error(Error error) {
vel_estimate_valid_ = false;
pos_estimate_valid_ = false;
error_ |= error;
axis_->error_ |= Axis::ERROR_ENCODER_FAILED;
}
bool Encoder::do_checks(){
@@ -166,9 +165,6 @@ void Encoder::set_circular_count(int32_t count, bool update_offset) {
bool Encoder::run_index_search() {
config_.use_index = true;
index_found_ = false;
if (!config_.idx_search_unidirectional && axis_->motor_.config_.direction == 0) {
axis_->motor_.config_.direction = 1;
}
set_idx_subscribe();
bool status = axis_->run_lockin_spin(axis_->config_.calibration_lockin);
@@ -177,7 +173,6 @@ bool Encoder::run_index_search() {
bool Encoder::run_direction_find() {
int32_t init_enc_val = shadow_count_;
axis_->motor_.config_.direction = 1; // Must test spin forwards for direction detect logic
Axis::LockinConfig_t lockin_config = axis_->config_.calibration_lockin;
lockin_config.finish_distance = lockin_config.vel * 3.0f; // run for 3 seconds
@@ -190,12 +185,12 @@ bool Encoder::run_direction_find() {
// Check response and direction
if (shadow_count_ > init_enc_val + 8) {
// motor same dir as encoder
axis_->motor_.config_.direction = 1;
config_.direction = 1;
} else if (shadow_count_ < init_enc_val - 8) {
// motor opposite dir as encoder
axis_->motor_.config_.direction = -1;
config_.direction = -1;
} else {
axis_->motor_.config_.direction = 0;
config_.direction = 0;
}
}
@@ -205,10 +200,8 @@ bool Encoder::run_direction_find() {
// @brief Turns the motor in one direction for a bit and then in the other
// direction in order to find the offset between the electrical phase 0
// and the encoder state 0.
// TODO: Do the scan with current, not voltage!
bool Encoder::run_offset_calibration() {
const float start_lock_duration = 1.0f;
const int num_steps = (int)(config_.calib_scan_distance / config_.calib_scan_omega * (float)current_meas_hz);
// Require index found if enabled
if (config_.use_index && !index_found_) {
@@ -220,55 +213,85 @@ bool Encoder::run_offset_calibration() {
// Therefore we have to sync them for calibration
shadow_count_ = count_in_cpr_;
float voltage_magnitude;
if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_HIGH_CURRENT)
voltage_magnitude = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance;
else if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL)
voltage_magnitude = axis_->motor_.config_.calibration_current;
else
return false;
CRITICAL_SECTION() {
// Reset state variables
axis_->open_loop_controller_.Id_setpoint_ = NAN;
axis_->open_loop_controller_.Iq_setpoint_ = NAN;
axis_->open_loop_controller_.Vd_setpoint_ = NAN;
axis_->open_loop_controller_.Vq_setpoint_ = NAN;
axis_->open_loop_controller_.phase_ = 0.0f;
axis_->open_loop_controller_.phase_vel_ = NAN;
float max_current_ramp = axis_->motor_.config_.calibration_current / start_lock_duration * 2.0f;
axis_->open_loop_controller_.max_current_ramp_ = max_current_ramp;
axis_->open_loop_controller_.max_voltage_ramp_ = max_current_ramp;
axis_->open_loop_controller_.max_phase_vel_ramp_ = INFINITY;
axis_->open_loop_controller_.target_current_ = axis_->motor_.config_.motor_type != Motor::MOTOR_TYPE_GIMBAL ? axis_->motor_.config_.calibration_current : 0.0f;
axis_->open_loop_controller_.target_voltage_ = axis_->motor_.config_.motor_type != Motor::MOTOR_TYPE_GIMBAL ? 0.0f : axis_->motor_.config_.calibration_current;
axis_->open_loop_controller_.target_vel_ = 0.0f;
axis_->open_loop_controller_.total_distance_ = 0.0f;
axis_->motor_.current_control_.enable_current_control_src_ = (axis_->motor_.config_.motor_type != Motor::MOTOR_TYPE_GIMBAL);
axis_->motor_.current_control_.Id_setpoint_src_ = &axis_->open_loop_controller_.Id_setpoint_;
axis_->motor_.current_control_.Iq_setpoint_src_ = &axis_->open_loop_controller_.Iq_setpoint_;
axis_->motor_.current_control_.Vd_setpoint_src_ = &axis_->open_loop_controller_.Vd_setpoint_;
axis_->motor_.current_control_.Vq_setpoint_src_ = &axis_->open_loop_controller_.Vq_setpoint_;
axis_->motor_.current_control_.phase_src_ =
axis_->async_estimator_.rotor_phase_src_ =
&axis_->open_loop_controller_.phase_;
axis_->motor_.phase_vel_src_ =
axis_->motor_.current_control_.phase_vel_src_ =
axis_->async_estimator_.rotor_phase_vel_src_ =
&axis_->open_loop_controller_.phase_vel_;
}
axis_->wait_for_control_iteration();
axis_->motor_.arm(&axis_->motor_.current_control_);
// go to motor zero phase for start_lock_duration to get ready to scan
int i = 0;
axis_->run_control_loop([&](){
if (!axis_->motor_.enqueue_voltage_timings(voltage_magnitude, 0.0f))
return false; // error set inside enqueue_voltage_timings
axis_->motor_.log_timing(TIMING_LOG_ENC_CALIB);
return ++i < start_lock_duration * current_meas_hz;
});
if (axis_->error_ != Axis::ERROR_NONE)
return false;
for (size_t i = 0; i < (size_t)(start_lock_duration * 1000.0f); ++i) {
if (!axis_->motor_.is_armed_) {
return false; // TODO: return "disarmed" error code
}
if (axis_->requested_state_ != Axis::AXIS_STATE_UNDEFINED) {
axis_->motor_.disarm();
return false; // TODO: return "aborted" error code
}
osDelay(1);
}
int32_t init_enc_val = shadow_count_;
uint32_t num_steps = 0;
int64_t encvaluesum = 0;
// scan forward
i = 0;
axis_->run_control_loop([&]() {
float phase = wrap_pm_pi(config_.calib_scan_distance * (float)i / (float)num_steps - config_.calib_scan_distance / 2.0f);
float v_alpha = voltage_magnitude * our_arm_cos_f32(phase);
float v_beta = voltage_magnitude * our_arm_sin_f32(phase);
if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
return false; // error set inside enqueue_voltage_timings
axis_->motor_.log_timing(TIMING_LOG_ENC_CALIB);
CRITICAL_SECTION() {
axis_->open_loop_controller_.target_vel_ = config_.calib_scan_omega;
axis_->open_loop_controller_.total_distance_ = 0.0f;
}
// scan forward
while ((axis_->requested_state_ == Axis::AXIS_STATE_UNDEFINED) && axis_->motor_.is_armed_) {
bool reached_target_dist = axis_->open_loop_controller_.total_distance_ >= config_.calib_scan_distance;
if (reached_target_dist) {
break;
}
encvaluesum += shadow_count_;
return ++i < num_steps;
});
if (axis_->error_ != Axis::ERROR_NONE)
return false;
num_steps++;
osDelay(1);
}
// Check response and direction
if (shadow_count_ > init_enc_val + 8) {
// motor same dir as encoder
axis_->motor_.config_.direction = 1;
config_.direction = 1;
} else if (shadow_count_ < init_enc_val - 8) {
// motor opposite dir as encoder
axis_->motor_.config_.direction = -1;
config_.direction = -1;
} else {
// Encoder response error
set_error(ERROR_NO_RESPONSE);
axis_->motor_.disarm();
return false;
}
@@ -279,25 +302,31 @@ bool Encoder::run_offset_calibration() {
calib_scan_response_ = std::abs(shadow_count_ - init_enc_val);
if (std::abs(calib_scan_response_ - expected_encoder_delta) / expected_encoder_delta > config_.calib_range) {
set_error(ERROR_CPR_POLEPAIRS_MISMATCH);
axis_->motor_.disarm();
return false;
}
// scan backwards
i = 0;
axis_->run_control_loop([&]() {
float phase = wrap_pm_pi(-config_.calib_scan_distance * (float)i / (float)num_steps + config_.calib_scan_distance / 2.0f);
float v_alpha = voltage_magnitude * our_arm_cos_f32(phase);
float v_beta = voltage_magnitude * our_arm_sin_f32(phase);
if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
return false; // error set inside enqueue_voltage_timings
axis_->motor_.log_timing(TIMING_LOG_ENC_CALIB);
CRITICAL_SECTION() {
axis_->open_loop_controller_.target_vel_ = -config_.calib_scan_omega;
}
// scan backwards
while ((axis_->requested_state_ == Axis::AXIS_STATE_UNDEFINED) && axis_->motor_.is_armed_) {
bool reached_target_dist = axis_->open_loop_controller_.total_distance_ <= 0.0f;
if (reached_target_dist) {
break;
}
encvaluesum += shadow_count_;
return ++i < num_steps;
});
if (axis_->error_ != Axis::ERROR_NONE)
num_steps++;
osDelay(1);
}
// Motor disarmed because of an error
if (!axis_->motor_.is_armed_) {
return false;
}
axis_->motor_.disarm();
config_.offset = encvaluesum / (num_steps * 2);
int32_t residual = encvaluesum - ((int64_t)config_.offset * (int64_t)(num_steps * 2));
@@ -339,7 +368,7 @@ void Encoder::sample_now() {
case MODE_SPI_ABS_AEAT:
case MODE_SPI_ABS_RLS:
{
axis_->motor_.log_timing(TIMING_LOG_SAMPLE_NOW);
abs_spi_start_transaction();
// Do nothing
} break;
@@ -368,10 +397,8 @@ void Encoder::decode_hall_samples() {
| (read_sampled_gpio(hallC_gpio_) ? 4 : 0);
}
bool Encoder::abs_spi_start_transaction(){
bool Encoder::abs_spi_start_transaction() {
if (mode_ & MODE_FLAG_ABS){
axis_->motor_.log_timing(TIMING_LOG_SPI_START);
if (Stm32SpiArbiter::acquire_task(&spi_task_)) {
spi_task_.ncs_gpio = abs_spi_cs_gpio_;
spi_task_.tx_buf = (uint8_t*)abs_spi_dma_tx_;
@@ -411,8 +438,6 @@ void Encoder::abs_spi_cb(bool success) {
goto done;
}
axis_->motor_.log_timing(TIMING_LOG_SPI_END);
switch (mode_) {
case MODE_SPI_ABS_AMS: {
uint16_t rawVal = abs_spi_dma_rx_[0];
@@ -476,6 +501,7 @@ bool Encoder::update() {
} break;
case MODE_HALL: {
decode_hall_samples();
int32_t hall_cnt;
if (decode_hall(hall_state_, &hall_cnt)) {
delta_enc = hall_cnt - count_in_cpr_;
@@ -485,6 +511,11 @@ bool Encoder::update() {
} else {
if (!config_.ignore_illegal_hall_state) {
set_error(ERROR_ILLEGAL_HALL_STATE);
pos_estimate_ = NAN;
pos_cpr_ = NAN;
vel_estimate_ = NAN;
phase_ = NAN;
phase_vel_ = NAN;
return false;
}
}
@@ -508,8 +539,15 @@ bool Encoder::update() {
if (abs_spi_pos_updated_ == false) {
// Low pass filter the error
spi_error_rate_ += current_meas_period * (1.0f - spi_error_rate_);
if (spi_error_rate_ > 0.005f)
if (spi_error_rate_ > 0.005f) {
set_error(ERROR_ABS_SPI_COM_FAIL);
pos_estimate_ = NAN;
pos_cpr_ = NAN;
vel_estimate_ = NAN;
phase_ = NAN;
phase_vel_ = NAN;
return false;
}
} else {
// Low pass filter the error
spi_error_rate_ += current_meas_period * (0.0f - spi_error_rate_);
@@ -524,7 +562,12 @@ bool Encoder::update() {
}break;
default: {
set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
pos_estimate_ = NAN;
pos_cpr_ = NAN;
vel_estimate_ = NAN;
phase_ = NAN;
phase_vel_ = NAN;
return false;
} break;
}
@@ -589,9 +632,13 @@ bool Encoder::update() {
float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
float ph = elec_rad_per_enc * (interpolated_enc - config_.offset_float);
// ph = fmodf(ph, 2*M_PI);
phase_ = wrap_pm_pi(ph);
if (is_ready_) {
phase_ = wrap_pm_pi(ph) * config_.direction;
phase_vel_ = (2*M_PI) * vel_estimate_ * axis_->motor_.config_.pole_pairs * config_.direction;
} else {
phase_ = NAN;
phase_vel_ = NAN;
}
vel_estimate_valid_ = true;
pos_estimate_valid_ = true;
return true;
}