From 28b40c9bd47fc3d218987a6566e03243fdd33271 Mon Sep 17 00:00:00 2001 From: PAJohnson Date: Thu, 10 Sep 2020 19:48:36 -0400 Subject: [PATCH] Fix for high commanded currents during lockin ramp. --- Firmware/MotorControl/axis.cpp | 8 ++++---- Firmware/MotorControl/sensorless_estimator.cpp | 6 ++++-- Firmware/MotorControl/sensorless_estimator.hpp | 3 ++- docs/commands.md | 8 ++++---- 4 files changed, 14 insertions(+), 11 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 310a7a8a..27e30faa 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -234,9 +234,9 @@ bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config) { float x = 0.0f; run_control_loop([&]() { float phase = wrap_pm_pi(lockin_config.ramp_distance * x); - float I_mag = lockin_config.current * x; + float torque = lockin_config.current * motor_.config_.torque_constant * x; x += current_meas_period / lockin_config.ramp_time; - if (!motor_.update(I_mag, phase, 0.0f)) + if (!motor_.update(torque, phase, 0.0f)) return false; return x < 1.0f; }); @@ -265,7 +265,7 @@ bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config) { distance += vel * current_meas_period; phase = wrap_pm_pi(phase + vel * current_meas_period); - if (!motor_.update(lockin_config.current, phase, vel)) + if (!motor_.update(lockin_config.current * motor_.config_.torque_constant, phase, vel)) return false; return !spin_done(true); //vel_override to go to next phase }); @@ -281,7 +281,7 @@ bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config) { distance += vel * current_meas_period; phase = wrap_pm_pi(phase + vel * current_meas_period); - if (!motor_.update(lockin_config.current, phase, vel)) + if (!motor_.update(lockin_config.current * motor_.config_.torque_constant, phase, vel)) return false; return !spin_done(); }); diff --git a/Firmware/MotorControl/sensorless_estimator.cpp b/Firmware/MotorControl/sensorless_estimator.cpp index aebbc09b..4c521b34 100644 --- a/Firmware/MotorControl/sensorless_estimator.cpp +++ b/Firmware/MotorControl/sensorless_estimator.cpp @@ -70,13 +70,15 @@ bool SensorlessEstimator::update() { } // predict PLL phase with velocity - pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * vel_estimate_); + pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * vel_estimate_erad_); // update PLL phase with observer permanent magnet phase phase_ = fast_atan2(eta[1], eta[0]); float delta_phase = wrap_pm_pi(phase_ - pll_pos_); pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * pll_kp * delta_phase); // update PLL velocity - vel_estimate_ += current_meas_period * pll_ki * delta_phase; + vel_estimate_erad_ += current_meas_period * pll_ki * delta_phase; + // convert to mechanical turns/s for controller usage. + vel_estimate_ = vel_estimate_erad_ / (std::max((float)axis_->motor_.config_.pole_pairs, 1.0f) * 2.0f * M_PI); vel_estimate_valid_ = true; return true; diff --git a/Firmware/MotorControl/sensorless_estimator.hpp b/Firmware/MotorControl/sensorless_estimator.hpp index 95992ae0..27f6f1c9 100644 --- a/Firmware/MotorControl/sensorless_estimator.hpp +++ b/Firmware/MotorControl/sensorless_estimator.hpp @@ -20,7 +20,8 @@ public: Error error_ = ERROR_NONE; float phase_ = 0.0f; // [rad] float pll_pos_ = 0.0f; // [rad] - float vel_estimate_ = 0.0f; // [rad/s] + float vel_estimate_ = 0.0f; // [turn/s] + float vel_estimate_erad_ = 0.0f; // [rad/s] bool vel_estimate_valid_ = false; // float pll_kp_ = 0.0f; // [rad/s / rad] // float pll_ki_ = 0.0f; // [(rad/s^2) / rad] diff --git a/docs/commands.md b/docs/commands.md index 1b41b30f..a621d578 100644 --- a/docs/commands.md +++ b/docs/commands.md @@ -82,15 +82,15 @@ All variables that are part of a `[...].config` object can be saved to non-volat ## Setting up sensorless The ODrive can run without encoder/hall feedback, but there is a minimum speed, usually around a few hunderd RPM. -To give an example, suppose you have a motor with 7 pole pairs, and you want to spin it at 3000 RPM. Then you would set the `input_vel` to `3000 * 2*pi/60 * 7 = 2199 rad/s electrical`. - -Below are some suggested starting parameters that you can use. Note that you _must_ set the `pm_flux_linkage` correctly for sensorless mode to work. +Below are some suggested starting parameters that you can use. Note that you _must_ set the `pm_flux_linkage` correctly for sensorless mode to work. Motor calibration and setup must also be completed before sensorless mode will work. ``` odrv0.axis0.controller.config.vel_gain = 0.01 odrv0.axis0.controller.config.vel_integrator_gain = 0.05 odrv0.axis0.controller.config.control_mode = 2 -odrv0.axis0.controller.input_vel = 400 +odrv0.axis0.controller.input_vel = 10 +odrv0.axis0.controller.config.vel_limit = +odrv0.axis0.motor.config.current_lim = 2 * odrv0.axis0.config.sensorless_ramp.current odrv0.axis0.motor.config.direction = 1 odrv0.axis0.sensorless_estimator.config.pm_flux_linkage = 5.51328895422 / ( * ) ```