diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 0cf13549..49605017 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -39,9 +39,8 @@ bool Encoder::do_checks(){ // Triggered when an encoder passes over the "Index" pin // TODO: only arm index edge interrupt when we know encoder has powered up // (maybe by attaching the interrupt on start search, synergistic with following) -// TODO: disable interrupt once we found the index void Encoder::enc_index_cb() { - if (config_.use_index && !index_found_) { + if (config_.use_index) { set_circular_count(0, false); if (config_.zero_count_on_find_idx) set_linear_count(0); // Avoid position control transient after search @@ -71,6 +70,23 @@ void Encoder::set_idx_subscribe(bool override_enable) { } } +void Encoder::update_pll_gains() { + pll_kp_ = 2.0f * config_.bandwidth; // basic conversion to discrete time + pll_ki_ = 0.25f * (pll_kp_ * pll_kp_); // Critically damped + + // Check that we don't get problems with discrete time approximation + if (!(current_meas_period * pll_kp_ < 1.0f)) { + set_error(ERROR_UNSTABLE_GAIN); + } +} + +void Encoder::check_pre_calibrated() { + if (!is_ready_) + config_.pre_calibrated = false; + if (config_.mode == MODE_INCREMENTAL && !index_found_) + config_.pre_calibrated = false; +} + // Function that sets the current encoder count to a desired 32-bit value. void Encoder::set_linear_count(int32_t count) { // Disable interrupts to make a critical section to avoid race condition @@ -261,16 +277,6 @@ static bool decode_hall(uint8_t hall_state, int32_t* hall_cnt) { } } -void Encoder::update_pll_gains() { - pll_kp_ = 2.0f * config_.bandwidth; // basic conversion to discrete time - pll_ki_ = 0.25f * (pll_kp_ * pll_kp_); // Critically damped - - // Check that we don't get problems with discrete time approximation - if (!(current_meas_period * pll_kp_ < 1.0f)) { - set_error(ERROR_UNSTABLE_GAIN); - } -} - void Encoder::sample_now() { switch (config_.mode) { case MODE_INCREMENTAL: { diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index dff98f28..b3a0e3bd 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -52,6 +52,8 @@ public: void enc_index_cb(); void set_idx_subscribe(bool override_enable = false); + void update_pll_gains(); + void check_pre_calibrated(); void set_linear_count(int32_t count); void set_circular_count(int32_t count, bool update_offset); @@ -63,7 +65,7 @@ public: void sample_now(); bool update(); - void update_pll_gains(); + const EncoderHardwareConfig_t& hw_config_; Config_t& config_; @@ -92,8 +94,8 @@ public: auto make_protocol_definitions() { return make_protocol_member_list( make_protocol_property("error", &error_), - make_protocol_ro_property("is_ready", &is_ready_), - make_protocol_ro_property("index_found", const_cast(&index_found_)), + make_protocol_property("is_ready", &is_ready_), + make_protocol_property("index_found", const_cast(&index_found_)), make_protocol_property("shadow_count", &shadow_count_), make_protocol_property("count_in_cpr", &count_in_cpr_), make_protocol_property("interpolation", &interpolation_), @@ -110,7 +112,8 @@ public: [](void* ctx) { static_cast(ctx)->set_idx_subscribe(); }, this), make_protocol_property("find_idx_on_lockin_only", &config_.find_idx_on_lockin_only, [](void* ctx) { static_cast(ctx)->set_idx_subscribe(); }, this), - make_protocol_property("pre_calibrated", &config_.pre_calibrated), + make_protocol_property("pre_calibrated", &config_.pre_calibrated, + [](void* ctx) { static_cast(ctx)->check_pre_calibrated(); }, this), make_protocol_property("zero_count_on_find_idx", &config_.zero_count_on_find_idx), make_protocol_property("cpr", &config_.cpr), make_protocol_property("offset", &config_.offset),