From e9d5f55570c367ccc0bd15cfb4964250d786729b Mon Sep 17 00:00:00 2001 From: PAJohnson Date: Thu, 29 Oct 2020 00:20:39 -0400 Subject: [PATCH] Fixed the encoder offset calibration to work correctly when calib_scan_distance is not a multiple of 4pi --- CHANGELOG.md | 2 +- Firmware/MotorControl/encoder.cpp | 7 +++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 2b294c67..ea1c76d9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,7 +5,7 @@ Please add a note of your changes below this heading if you make a Pull Request. * Added periodic sending of encoder position on CAN ### Changed - +* Modified encoder offset calibration to work correctly when calib_scan_distance is not a multiple of 4pi * Moved thermistors from being a top level object to belonging to Motor objects. Also changed errors: thermistor errors rolled into motor errors * Use DMA for DRV8301 setup * Make NVM configuration code more dynamic so that the layout doesn't have to be known at compile time. diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index d2ce992b..cc5c2d1d 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -228,10 +228,13 @@ bool Encoder::run_offset_calibration() { else return false; - // go to motor zero phase for start_lock_duration to get ready to scan + // go to start position of forward scan for start_lock_duration to get ready to scan int i = 0; axis_->run_control_loop([&](){ - if (!axis_->motor_.enqueue_voltage_timings(voltage_magnitude, 0.0f)) + float phase = wrap_pm_pi(0 - 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)) return false; // error set inside enqueue_voltage_timings axis_->motor_.log_timing(TIMING_LOG_ENC_CALIB); return ++i < start_lock_duration * current_meas_hz;