From 19e3c779aefdf1cda17ad748a2c87a05812741b5 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 17 Aug 2018 23:39:43 -0700 Subject: [PATCH] force offset cal after idx search, index now homes axis --- CHANGELOG.md | 10 ++++++ Firmware/MotorControl/axis.cpp | 30 +++++++++++----- Firmware/MotorControl/axis.hpp | 11 ++++-- Firmware/MotorControl/encoder.cpp | 58 ++++++++++++++++++------------- Firmware/MotorControl/encoder.hpp | 8 ++--- 5 files changed, 77 insertions(+), 40 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 9795d51d..d885d6e2 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,6 +1,16 @@ # Unreleased Features Please add a note of your changes below this heading if you make a Pull Request. +## Unreleased +### Added +* Encoder position count "homed" to zero when index is found. + +### Changed +* We now enforce encoder offset calibration must happen after index is found (if using index) + +### Fixed +* Once you got an axis error `ERROR_INVALID_STATE` you could never clear it + # Releases ## [0.4.2] - 2018-08-04 ### Added diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 270ab15f..b5c7fbe5 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -93,6 +93,19 @@ void Axis::set_step_dir_enabled(bool enable) { } } +bool Axis::check_for_errors() { + // Maybe we should update this to only trigger on new errors? + // The danger with that is we could fail to bail on uncleared errors that still prevent + // correct opreation. + + // For now: we treat ERROR_INVALID_STATE in idle loop special, or we could never stay + // in idle after this kind of error. + if (current_state_ == AXIS_STATE_IDLE) + return (error_ & ~ERROR_INVALID_STATE) == ERROR_NONE; + else + return error_ == ERROR_NONE; +} + // @brief Do axis level checks and call subcomponent do_checks // Returns true if everything is ok. bool Axis::do_checks() { @@ -112,7 +125,7 @@ bool Axis::do_checks() { // sensorless_estimator_.do_checks(); // controller_.do_checks(); - return error_ == ERROR_NONE; + return check_for_errors(); } // @brief Update all esitmators @@ -120,7 +133,7 @@ bool Axis::do_updates() { // Sub-components should use set_error which will propegate to this error_ encoder_.update(); sensorless_estimator_.update(); - return error_ == ERROR_NONE; + return check_for_errors(); } float Axis::get_temp() { @@ -159,7 +172,7 @@ bool Axis::run_sensorless_spin_up() { // is zeroed. So we make the setpoint the spinup target for smooth transition. controller_.vel_setpoint_ = config_.spin_up_target_vel; - return error_ == ERROR_NONE; + return check_for_errors(); } // Note run_sensorless_control_loop and run_closed_loop_control_loop are very similar and differ only in where we get the estimate from. @@ -178,7 +191,7 @@ bool Axis::run_sensorless_control_loop() { return true; }); set_step_dir_enabled(false); - return error_ == ERROR_NONE; + return check_for_errors(); } bool Axis::run_closed_loop_control_loop() { @@ -193,7 +206,7 @@ bool Axis::run_closed_loop_control_loop() { return true; }); set_step_dir_enabled(false); - return error_ == ERROR_NONE; + return check_for_errors(); } bool Axis::run_idle_loop() { @@ -203,7 +216,7 @@ bool Axis::run_idle_loop() { run_control_loop([this](){ return true; }); - return error_ == ERROR_NONE; + return check_for_errors(); } // Infinite loop that does calibration and enters main control loop as appropriate @@ -249,9 +262,10 @@ void Axis::run_state_machine_loop() { task_chain_[pos++] = requested_state_; task_chain_[pos++] = AXIS_STATE_IDLE; } - task_chain_[pos++] = AXIS_STATE_UNDEFINED; - // TODO: bounds checking + task_chain_[pos++] = AXIS_STATE_UNDEFINED; // TODO: bounds checking requested_state_ = AXIS_STATE_UNDEFINED; + // Auto-clear any invalid state error + error_ &= ~ERROR_INVALID_STATE; } // Note that current_state is a reference to task_chain_[0] diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index cfa1bd28..111f9e26 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -79,6 +79,7 @@ public: bool check_PSU_brownout(); bool do_checks(); bool do_updates(); + bool check_for_errors(); float get_temp(); // @brief Runs the specified update handler at the frequency of the current measurements. @@ -104,9 +105,13 @@ public: template void run_control_loop(const T& update_handler) { while (requested_state_ == AXIS_STATE_UNDEFINED) { - if (!do_checks()) // look for errors at axis level and also all subcomponents - break; - if (!do_updates()) // Update all estimators + // look for errors at axis level and also all subcomponents + bool checks_ok = do_checks(); + // Update all estimators + // Note: updates run even if checks fail + bool updates_ok = do_updates(); + + if (!checks_ok || !updates_ok) break; // Run main loop function, defer quitting for after wait diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 9a6210ce..ccd6ee2d 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -8,7 +8,6 @@ Encoder::Encoder(const EncoderHardwareConfig_t& hw_config, config_(config) { if (config.pre_calibrated && (config.mode == Encoder::MODE_HALL)) { - offset_ = config.offset; is_ready_ = true; } } @@ -38,13 +37,17 @@ 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_) { set_circular_count(0); + set_linear_count(0); // Avoid position control transient after search if (config_.pre_calibrated) { - offset_ = config_.offset; is_ready_ = true; + } else { + // Invalidate offset calibration that may have happened before idx search + is_ready_ = false; } index_found_ = true; } @@ -73,8 +76,13 @@ void Encoder::set_circular_count(int32_t count) { __disable_irq(); // Offset and state must be shifted by the same amount - offset_ += count - count_in_cpr_; - offset_ = mod(offset_, config_.cpr); + // Note that if the linear count is also cleared before running an update, + // the offset will drift by at least one count. Therefore we shouldn't rely + // on this during index search callback. + // Hence we invalidate calibration in enc_index_cb + config_.offset += count - count_in_cpr_; + config_.offset = mod(config_.offset, config_.cpr); + // Update states count_in_cpr_ = mod(count, config_.cpr); pos_cpr_ = (float)count_in_cpr_; @@ -124,10 +132,11 @@ bool Encoder::run_offset_calibration() { static const float scan_distance = 16.0f * M_PI; static const int num_steps = (int)(scan_distance / scan_omega * (float)current_meas_hz); - // Temporarily disable index search so it doesn't mess - // with the offset calibration - bool old_use_index = config_.use_index; - config_.use_index = false; + // Require index found if enabled + if (config_.use_index && !index_found_) { + set_error(ERROR_INDEX_NOT_FOUND_YET); + return false; + } // We use shadow_count_ to do the calibration, but the offset is used by count_in_cpr_ // Therefore we have to sync them for calibration @@ -172,16 +181,7 @@ bool Encoder::run_offset_calibration() { if (axis_->error_ != Axis::ERROR_NONE) return false; - //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(shadow_count_-init_enc_val); - if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > config_.calib_range) - { - set_error(ERROR_CPR_OUT_OF_RANGE); - return false; - } - // check direction + // Check response and direction if (shadow_count_ > init_enc_val + 8) { // motor same dir as encoder axis_->motor_.config_.direction = 1; @@ -190,7 +190,18 @@ bool Encoder::run_offset_calibration() { axis_->motor_.config_.direction = -1; } else { // Encoder response error - set_error(ERROR_RESPONSE); + set_error(ERROR_NO_RESPONSE); + return false; + } + + //TODO avoid recomputing elec_rad_per_enc every time + // Check CPR + 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(shadow_count_-init_enc_val); + if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > config_.calib_range) + { + set_error(ERROR_CPR_OUT_OF_RANGE); return false; } @@ -211,12 +222,11 @@ bool Encoder::run_offset_calibration() { if (axis_->error_ != Axis::ERROR_NONE) return false; - offset_ = encvaluesum / (num_steps * 2); - config_.offset = offset_; - int32_t residual = encvaluesum - ((int64_t)offset_ * (int64_t)(num_steps * 2)); + config_.offset = encvaluesum / (num_steps * 2); + int32_t residual = encvaluesum - ((int64_t)config_.offset * (int64_t)(num_steps * 2)); config_.offset_float = (float)residual / (float)(num_steps * 2) + 0.5f; // add 0.5 to center-align state to phase + is_ready_ = true; - config_.use_index = old_use_index; return true; } @@ -296,7 +306,7 @@ bool Encoder::update() { } //// run encoder count interpolation - int32_t corrected_enc = count_in_cpr_ - offset_; + int32_t corrected_enc = count_in_cpr_ - config_.offset; // if we are stopped, make sure we don't randomly drift if (snap_to_zero_vel) { interpolation_ = 0.5f; diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 86bc9b73..531bbf7b 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -11,9 +11,10 @@ public: ERROR_NONE = 0, ERROR_UNSTABLE_GAIN = 0x01, ERROR_CPR_OUT_OF_RANGE = 0x02, - ERROR_RESPONSE = 0x04, + ERROR_NO_RESPONSE = 0x04, ERROR_UNSUPPORTED_ENCODER_MODE = 0x08, ERROR_ILLEGAL_HALL_STATE = 0x10, + ERROR_INDEX_NOT_FOUND_YET = 0x20, }; enum Mode_t { @@ -31,8 +32,7 @@ public: // 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 + int32_t offset = 0; // Offset between encoder count and rotor electrical phase float offset_float = 0.0f; // Sub-count phase alignment offset float calib_range = 0.02f; float bandwidth = 1000.0f; @@ -65,7 +65,6 @@ public: bool is_ready_ = false; int32_t shadow_count_ = 0; int32_t count_in_cpr_ = 0; - int32_t offset_ = 0; float interpolation_ = 0.0f; float phase_ = 0.0f; // [rad] float pos_estimate_ = 0.0f; // [rad] @@ -85,7 +84,6 @@ public: make_protocol_ro_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("offset", &offset_), make_protocol_property("interpolation", &interpolation_), make_protocol_property("phase", &phase_), make_protocol_property("pos_estimate", &pos_estimate_),