From 1761272cad99b66385c4e772286ac4abf9ac9da7 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 1 Apr 2019 21:22:27 -0700 Subject: [PATCH] add encoder offset calib debug var calib_scan_response --- Firmware/MotorControl/encoder.cpp | 14 ++++++-------- Firmware/MotorControl/encoder.hpp | 10 ++++++++-- 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index d7235655..5a4fe833 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -163,9 +163,7 @@ bool Encoder::run_direction_find() { // TODO: Do the scan with current, not voltage! bool Encoder::run_offset_calibration() { static const float start_lock_duration = 1.0f; - static const float scan_omega = 4.0f * M_PI; - static const float scan_distance = 16.0f * M_PI; - static const int num_steps = (int)(scan_distance / scan_omega * (float)current_meas_hz); + static 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_) { @@ -202,7 +200,7 @@ bool Encoder::run_offset_calibration() { // scan forward i = 0; axis_->run_control_loop([&](){ - float phase = wrap_pm_pi(scan_distance * (float)i / (float)num_steps - scan_distance / 2.0f); + 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)) @@ -232,9 +230,9 @@ bool Encoder::run_offset_calibration() { //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) + float expected_encoder_delta = config_.calib_scan_distance / elec_rad_per_enc; + calib_scan_response_ = fabsf(shadow_count_-init_enc_val); + if(fabsf(calib_scan_response_ - expected_encoder_delta)/expected_encoder_delta > config_.calib_range) { set_error(ERROR_CPR_OUT_OF_RANGE); return false; @@ -243,7 +241,7 @@ bool Encoder::run_offset_calibration() { // scan backwards i = 0; axis_->run_control_loop([&](){ - float phase = wrap_pm_pi(-scan_distance * (float)i / (float)num_steps + scan_distance / 2.0f); + 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)) diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index ecc6d4f1..c2d32841 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -37,6 +37,8 @@ public: float offset_float = 0.0f; // Sub-count phase alignment offset bool enable_phase_interpolation = true; // Use velocity to interpolate inside the count state float calib_range = 0.02f; // Accuracy required to pass encoder cpr check + float calib_scan_distance = 16.0f * M_PI; // rad electrical + float calib_scan_omega = 4.0f * M_PI; // rad/s electrical float bandwidth = 1000.0f; bool find_idx_on_lockin_only = false; // Only be sensitive during lockin scan constant vel state bool idx_search_unidirectional = false; // Only allow index search in known direction @@ -83,6 +85,7 @@ public: float vel_estimate_ = 0.0f; // [count/s] float pll_kp_ = 0.0f; // [count/s / count] float pll_ki_ = 0.0f; // [(count/s^2) / count] + float calib_scan_response_ = 0.0f; // debug report from offset calib int16_t tim_cnt_sample_ = 0; // // Updated by low_level pwm_adc_cb @@ -99,11 +102,12 @@ public: make_protocol_property("shadow_count", &shadow_count_), make_protocol_property("count_in_cpr", &count_in_cpr_), make_protocol_property("interpolation", &interpolation_), - make_protocol_property("phase", &phase_), + make_protocol_ro_property("phase", &phase_), make_protocol_property("pos_estimate", &pos_estimate_), make_protocol_property("pos_cpr", &pos_cpr_), - make_protocol_property("hall_state", &hall_state_), + make_protocol_ro_property("hall_state", &hall_state_), make_protocol_property("vel_estimate", &vel_estimate_), + make_protocol_ro_property("calib_scan_response", &calib_scan_response_), // make_protocol_property("pll_kp", &pll_kp_), // make_protocol_property("pll_ki", &pll_ki_), make_protocol_object("config", @@ -122,6 +126,8 @@ public: make_protocol_property("bandwidth", &config_.bandwidth, [](void* ctx) { static_cast(ctx)->update_pll_gains(); }, this), make_protocol_property("calib_range", &config_.calib_range), + make_protocol_property("calib_scan_distance", &config_.calib_scan_distance), + make_protocol_property("calib_scan_omega", &config_.calib_scan_omega), make_protocol_property("idx_search_unidirectional", &config_.idx_search_unidirectional), make_protocol_property("ignore_illegal_hall_state", &config_.ignore_illegal_hall_state) ),