diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 4c00ca6c..020046c3 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -3,13 +3,13 @@ Encoder::Encoder(const EncoderHardwareConfig_t& hw_config, - EncoderConfig_t& config) : + Config_t& config) : hw_config_(hw_config), config_(config) { // Calculate encoder pll gains // This calculation is currently identical to the PLL in SensorlessEstimator - float pll_bandwidth = 1000.0f; // [rad/s] + float pll_bandwidth = 100.0f; // [rad/s] pll_kp_ = 2.0f * pll_bandwidth; // Critically damped @@ -26,32 +26,6 @@ void Encoder::setup() { enc_index_cb_wrapper, this); } -int16_t Encoder::get_low_level_count() { - switch (mode_) { - case MODE_INCREMENTAL: { - return (int16_t)hw_config_.timer->Instance->CNT; - } break; - case MODE_HALL: { - switch (hall_state_) { - case 0b001: return 0; - case 0b011: return 1; - case 0b010: return 2; - case 0b110: return 3; - case 0b100: return 4; - case 0b101: return 5; - default: { - error_ |= ERROR_ILLEGAL_HALL_STATE; - return 0; - } - } - } break; - default: { - error_ |= ERROR_UNSUPPORTED_ENCODER_MODE; - return 0; - } - } -} - //-------------------- // Hardware Dependent //-------------------- @@ -120,6 +94,8 @@ bool Encoder::run_index_search() { index_found_ = false; float phase = 0.0f; axis_->run_control_loop([&](){ + update(nullptr, nullptr, nullptr); + phase = wrap_pm_pi(phase + omega * current_meas_period); float v_alpha = voltage_magnitude * arm_cos_f32(phase); @@ -160,6 +136,8 @@ bool Encoder::run_offset_calibration() { // go to motor zero phase for start_lock_duration to get ready to scan int i = 0; axis_->run_control_loop([&](){ + update(nullptr, nullptr, nullptr); + if (!axis_->motor_.enqueue_voltage_timings(voltage_magnitude, 0.0f)) return false; // error set inside enqueue_voltage_timings axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB); @@ -168,13 +146,13 @@ bool Encoder::run_offset_calibration() { if (axis_->error_ != Axis::ERROR_NO_ERROR) return false; - int32_t init_enc_val = get_low_level_count(); + int32_t init_enc_val = shadow_count_; int64_t encvaluesum = 0; // scan forward i = 0; axis_->run_control_loop([&](){ - axis_->encoder_.update(nullptr, nullptr, nullptr); + update(nullptr, nullptr, nullptr); float phase = wrap_pm_pi(scan_distance * (float)i / (float)num_steps - scan_distance / 2.0f); float v_alpha = voltage_magnitude * arm_cos_f32(phase); @@ -183,7 +161,7 @@ bool Encoder::run_offset_calibration() { return false; // error set inside enqueue_voltage_timings axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB); - encvaluesum += get_low_level_count(); + encvaluesum += shadow_count_; return ++i < num_steps; }); @@ -193,17 +171,17 @@ bool Encoder::run_offset_calibration() { //TODO avoid recomputing elec_rad_per_enc every time float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr)); float expected_encoder_delta = scan_distance / elec_rad_per_enc; - float actual_encoder_delta_abs = fabsf(get_low_level_count()-init_enc_val); + float actual_encoder_delta_abs = fabsf(shadow_count_-init_enc_val); if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > config_.calib_range) { error_ |= ERROR_CPR_OUT_OF_RANGE; return false; } // check direction - if (get_low_level_count() > init_enc_val + 8) { + if (shadow_count_ > init_enc_val + 8) { // motor same dir as encoder axis_->motor_.config_.direction = 1; - } else if (get_low_level_count() < init_enc_val - 8) { + } else if (shadow_count_ < init_enc_val - 8) { // motor opposite dir as encoder axis_->motor_.config_.direction = -1; } else { @@ -215,6 +193,8 @@ bool Encoder::run_offset_calibration() { // scan backwards i = 0; axis_->run_control_loop([&](){ + update(nullptr, nullptr, nullptr); + float phase = wrap_pm_pi(-scan_distance * (float)i / (float)num_steps + scan_distance / 2.0f); float v_alpha = voltage_magnitude * arm_cos_f32(phase); float v_beta = voltage_magnitude * arm_sin_f32(phase); @@ -222,7 +202,7 @@ bool Encoder::run_offset_calibration() { return false; // error set inside enqueue_voltage_timings axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB); - encvaluesum += get_low_level_count(); + encvaluesum += shadow_count_; return ++i < num_steps; }); @@ -235,6 +215,18 @@ bool Encoder::run_offset_calibration() { return true; } +static bool decode_hall(uint8_t hall_state, int32_t* hall_cnt) { + switch (hall_state) { + case 0b001: *hall_cnt = 0; return true; + case 0b011: *hall_cnt = 1; return true; + case 0b010: *hall_cnt = 2; return true; + case 0b110: *hall_cnt = 3; return true; + case 0b100: *hall_cnt = 4; return true; + case 0b101: *hall_cnt = 5; return true; + default: return false; + } +} + bool Encoder::update(float* pos_estimate, float* vel_estimate, float* phase_output) { // Check that we don't get problems with discrete time approximation if (!(current_meas_period * pll_kp_ < 1.0f)) { @@ -242,9 +234,35 @@ bool Encoder::update(float* pos_estimate, float* vel_estimate, float* phase_outp return false; } - // update internal encoder state - int16_t delta_enc_16 = get_low_level_count() - (int16_t)shadow_count_; - int32_t delta_enc = (int32_t)delta_enc_16; //sign extend + // update internal encoder state. + int32_t delta_enc = 0; + switch (config_.mode) { + case MODE_INCREMENTAL: { + //TODO: use count_in_cpr_ instead as shadow_count_ can overflow + //or use 64 bit + int16_t delta_enc_16 = (int16_t)hw_config_.timer->Instance->CNT - (int16_t)shadow_count_; + delta_enc = (int32_t)delta_enc_16; //sign extend + } break; + + case MODE_HALL: { + int32_t hall_cnt; + if (decode_hall(hall_state_, &hall_cnt)) { + delta_enc = hall_cnt - count_in_cpr_; + delta_enc = mod(delta_enc, 6); + if (delta_enc > 3) + delta_enc -= 6; + } else { + error_ |= ERROR_ILLEGAL_HALL_STATE; + return false; + } + } break; + + default: { + error_ |= ERROR_UNSUPPORTED_ENCODER_MODE; + return 0; + } break; + } + shadow_count_ += delta_enc; count_in_cpr_ += delta_enc; count_in_cpr_ = mod(count_in_cpr_, config_.cpr); @@ -261,14 +279,14 @@ bool Encoder::update(float* pos_estimate, float* vel_estimate, float* phase_outp // run pll (for now pll is in units of encoder counts) // Predict current pos pos_estimate_ += current_meas_period * pll_vel_; - pos_cpr_ += current_meas_period * pll_vel_; + pos_cpr_ += current_meas_period * pll_vel_; // discrete phase detector float delta_pos = (float)(shadow_count_ - (int32_t)floorf(pos_estimate_)); float delta_pos_cpr = (float)(count_in_cpr_ - (int32_t)floorf(pos_cpr_)); delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * (float)(config_.cpr)); // pll feedback pos_estimate_ += current_meas_period * pll_kp_ * delta_pos; - pos_cpr_ += current_meas_period * pll_kp_ * delta_pos_cpr; + pos_cpr_ += current_meas_period * pll_kp_ * delta_pos_cpr; pos_cpr_ = fmodf_pos(pos_cpr_, (float)(config_.cpr)); pll_vel_ += current_meas_period * pll_ki_ * delta_pos_cpr; if (fabsf(pll_vel_) < 0.5f * current_meas_period * pll_ki_) diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 7a67ffb9..c7524302 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -5,20 +5,6 @@ #error "This file should not be included directly. Include odrive_main.h instead." #endif -struct EncoderConfig_t { - bool use_index = false; - bool pre_calibrated = false; // If true, this means the offset stored in - // configuration is valid and does not need - // be determined by run_offset_calibration. - // In this case the encoder will enter ready - // state as soon as the index is found. - float idx_search_speed = 10.0f; // [rad/s electrical] - int32_t cpr = (2048 * 4); // Default resolution of CUI-AMT102 encoder, - int32_t offset = 0; // If pre_calibrated is true, this is copied into encoder.offset_ once - // index search succeeds - float calib_range = 0.02f; -}; - class Encoder { public: enum Error_t { @@ -35,14 +21,28 @@ public: MODE_HALL }; + struct Config_t { + Encoder::Mode_t mode = Encoder::MODE_INCREMENTAL; + bool use_index = false; + bool pre_calibrated = false; // If true, this means the offset stored in + // configuration is valid and does not need + // be determined by run_offset_calibration. + // In this case the encoder will enter ready + // state as soon as the index is found. + float idx_search_speed = 10.0f; // [rad/s electrical] + int32_t cpr = (2048 * 4); // Default resolution of CUI-AMT102 encoder, + int32_t offset = 0; // If pre_calibrated is true, this is copied into encoder.offset_ once + // index search succeeds + float calib_range = 0.02f; + }; + Encoder(const EncoderHardwareConfig_t& hw_config, - EncoderConfig_t& config); + Config_t& config); void setup(); void enc_index_cb(); - int16_t get_low_level_count(); void set_linear_count(int32_t count); void set_circular_count(int32_t count); bool calib_enc_offset(float voltage_magnitude); @@ -53,11 +53,10 @@ public: bool update(float* pos_estimate, float* vel_estimate, float* phase); const EncoderHardwareConfig_t& hw_config_; - EncoderConfig_t& config_; + Config_t& config_; Axis* axis_ = nullptr; // set by Axis constructor Error_t error_ = ERROR_NONE; - Mode_t mode_ = MODE_INCREMENTAL; bool index_found_ = false; bool is_ready_ = false; int32_t shadow_count_ = 0; @@ -90,6 +89,7 @@ public: make_protocol_property("pll_kp", &pll_kp_), make_protocol_property("pll_ki", &pll_ki_), make_protocol_object("config", + make_protocol_property("mode", &config_.mode), make_protocol_property("use_index", &config_.use_index), make_protocol_property("pre_calibrated", &config_.pre_calibrated), make_protocol_property("idx_search_speed", &config_.idx_search_speed), diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 2d744d5b..1e18a744 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -9,7 +9,7 @@ #include BoardConfig_t board_config; -EncoderConfig_t encoder_configs[AXIS_COUNT]; +Encoder::Config_t encoder_configs[AXIS_COUNT]; ControllerConfig_t controller_configs[AXIS_COUNT]; MotorConfig_t motor_configs[AXIS_COUNT]; AxisConfig_t axis_configs[AXIS_COUNT]; @@ -21,7 +21,7 @@ Axis *axes[AXIS_COUNT]; typedef Config< BoardConfig_t, - EncoderConfig_t[AXIS_COUNT], + Encoder::Config_t[AXIS_COUNT], ControllerConfig_t[AXIS_COUNT], MotorConfig_t[AXIS_COUNT], AxisConfig_t[AXIS_COUNT]> ConfigFormat; @@ -49,7 +49,7 @@ void load_configuration(void) { //If loading failed, restore defaults board_config = BoardConfig_t(); for (size_t i = 0; i < AXIS_COUNT; ++i) { - encoder_configs[i] = EncoderConfig_t(); + encoder_configs[i] = Encoder::Config_t(); controller_configs[i] = ControllerConfig_t(); motor_configs[i] = MotorConfig_t(); axis_configs[i] = AxisConfig_t();