force offset cal after idx search, index now homes axis

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

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]

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<typename T>
     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

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;

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<bool*>(&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_),