From f69685df13b954f2cd087010ef7ef04c74deeff4 Mon Sep 17 00:00:00 2001 From: Ben Wang Date: Wed, 29 May 2019 16:26:07 -0400 Subject: [PATCH 1/4] refactor lockin mode into separate variables for sensorless spinup, encoder calibration and general (user initiated) mode --- Firmware/MotorControl/axis.cpp | 78 ++++++++++++++++++++++++------- Firmware/MotorControl/axis.hpp | 30 ++++++++++-- Firmware/MotorControl/encoder.cpp | 15 +++--- docs/commands.md | 1 + docs/encoders.md | 6 +++ 5 files changed, 101 insertions(+), 29 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 480ed4c1..cbcc1e63 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -33,6 +33,48 @@ Axis::Axis(int axis_num, update_watchdog_settings(); } +Axis::LockinConfig_t Axis::default_calibration() { + Axis::LockinConfig_t config; + config.current = 10.0f; // [A] + config.ramp_time = 0.4f; // [s] + config.ramp_distance = 1 * M_PI; // [rad] + config.accel = 20.0f; // [rad/s^2] + config.vel = 40.0f; // [rad/s] + config.finish_distance = 100.0f * 2.0f * M_PI; // [rad] + config.finish_on_vel = false; + config.finish_on_distance = true; + config.finish_on_enc_idx = true; + return config; +} + +Axis::LockinConfig_t Axis::default_sensorless() { + Axis::LockinConfig_t config; + config.current = 10.0f; // [A] + config.ramp_time = 0.4f; // [s] + config.ramp_distance = 1 * M_PI; // [rad] + config.accel = 200.0f; // [rad/s^2] + config.vel = 400.0f; // [rad/s] + config.finish_distance = 100.0f; // [rad] + config.finish_on_vel = true; + config.finish_on_distance = false; + config.finish_on_enc_idx = false; + return config; +} + +Axis::LockinConfig_t Axis::default_lockin() { + Axis::LockinConfig_t config; + config.current = 10.0f; // [A] + config.ramp_time = 0.4f; // [s] + config.ramp_distance = 1 * M_PI; // [rad] + config.accel = 20.0f; // [rad/s^2] + config.vel = 400.0f; // [rad/s] + config.finish_distance = 100.0f; // [rad] + config.finish_on_vel = false; + config.finish_on_distance = false; + config.finish_on_enc_idx = false; + return config; +} + static void step_cb_wrapper(void* ctx) { reinterpret_cast(ctx)->step_cb(); } @@ -179,32 +221,32 @@ bool Axis::watchdog_check() { } } -bool Axis::run_lockin_spin() { +bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config) { // Spiral up current for softer rotor lock-in lockin_state_ = LOCKIN_STATE_RAMP; float x = 0.0f; run_control_loop([&]() { - float phase = wrap_pm_pi(config_.lockin.ramp_distance * x); - float I_mag = config_.lockin.current * x; - x += current_meas_period / config_.lockin.ramp_time; + float phase = wrap_pm_pi(lockin_config.ramp_distance * x); + float I_mag = lockin_config.current * x; + x += current_meas_period / lockin_config.ramp_time; if (!motor_.update(I_mag, phase, 0.0f)) return false; return x < 1.0f; }); // Spin states - float distance = config_.lockin.ramp_distance; + float distance = lockin_config.ramp_distance; float phase = wrap_pm_pi(distance); - float vel = distance / config_.lockin.ramp_time; + float vel = distance / lockin_config.ramp_time; // Function of states to check if we are done auto spin_done = [&](bool vel_override = false) -> bool { bool done = false; - if (config_.lockin.finish_on_vel || vel_override) - done = done || fabsf(vel) >= fabsf(config_.lockin.vel); - if (config_.lockin.finish_on_distance) - done = done || fabsf(distance) >= fabsf(config_.lockin.finish_distance); - if (config_.lockin.finish_on_enc_idx) + if (lockin_config.finish_on_vel || vel_override) + done = done || fabsf(vel) >= fabsf(lockin_config.vel); + if (lockin_config.finish_on_distance) + done = done || fabsf(distance) >= fabsf(lockin_config.finish_distance); + if (lockin_config.finish_on_enc_idx) done = done || encoder_.index_found_; return done; }; @@ -212,11 +254,11 @@ bool Axis::run_lockin_spin() { // Accelerate lockin_state_ = LOCKIN_STATE_ACCELERATE; run_control_loop([&]() { - vel += config_.lockin.accel * current_meas_period; + vel += lockin_config.accel * current_meas_period; distance += vel * current_meas_period; phase = wrap_pm_pi(phase + vel * current_meas_period); - if (!motor_.update(config_.lockin.current, phase, vel)) + if (!motor_.update(lockin_config.current, phase, vel)) return false; return !spin_done(true); //vel_override to go to next phase }); @@ -227,12 +269,12 @@ bool Axis::run_lockin_spin() { // Constant speed if (!spin_done()) { lockin_state_ = LOCKIN_STATE_CONST_VEL; - vel = config_.lockin.vel; // reset to actual specified vel to avoid small integration error + vel = lockin_config.vel; // reset to actual specified vel to avoid small integration error run_control_loop([&]() { distance += vel * current_meas_period; phase = wrap_pm_pi(phase + vel * current_meas_period); - if (!motor_.update(config_.lockin.current, phase, vel)) + if (!motor_.update(lockin_config.current, phase, vel)) return false; return !spin_done(); }); @@ -371,17 +413,17 @@ void Axis::run_state_machine_loop() { case AXIS_STATE_LOCKIN_SPIN: { if (!motor_.is_calibrated_ || motor_.config_.direction==0) goto invalid_state_label; - status = run_lockin_spin(); + status = run_lockin_spin(config_.lockin); } break; case AXIS_STATE_SENSORLESS_CONTROL: { if (!motor_.is_calibrated_ || motor_.config_.direction==0) goto invalid_state_label; - status = run_lockin_spin(); // TODO: restart if desired + status = run_lockin_spin(config_.sensorless_ramp); // TODO: restart if desired if (status) { // call to controller.reset() that happend when arming means that vel_setpoint // is zeroed. So we make the setpoint the spinup target for smooth transition. - controller_.vel_setpoint_ = config_.lockin.vel; + controller_.vel_setpoint_ = config_.sensorless_ramp.vel; status = run_sensorless_control_loop(); } } break; diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 179ae1ae..b4c69ca7 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -49,6 +49,10 @@ public: bool finish_on_enc_idx = false; }; + static LockinConfig_t default_calibration(); + static LockinConfig_t default_sensorless(); + static LockinConfig_t default_lockin(); + struct Config_t { bool startup_motor_calibration = false; //(ctx)->decode_step_dir_pins(); }, this), make_protocol_property("dir_gpio_pin", &config_.dir_gpio_pin, [](void* ctx) { static_cast(ctx)->decode_step_dir_pins(); }, this), - make_protocol_object("lockin", + make_protocol_object("calibration_lockin", + make_protocol_property("current", &config_.calibration_lockin.current), + make_protocol_property("ramp_time", &config_.calibration_lockin.ramp_time), + make_protocol_property("ramp_distance", &config_.calibration_lockin.ramp_distance), + make_protocol_property("accel", &config_.calibration_lockin.accel), + make_protocol_property("vel", &config_.calibration_lockin.vel) + ), + make_protocol_object("sensorless_ramp", + make_protocol_property("current", &config_.sensorless_ramp.current), + make_protocol_property("ramp_time", &config_.sensorless_ramp.ramp_time), + make_protocol_property("ramp_distance", &config_.sensorless_ramp.ramp_distance), + make_protocol_property("accel", &config_.sensorless_ramp.accel), + make_protocol_property("vel", &config_.sensorless_ramp.vel), + make_protocol_property("finish_distance", &config_.sensorless_ramp.finish_distance), + make_protocol_property("finish_on_vel", &config_.sensorless_ramp.finish_on_vel), + make_protocol_property("finish_on_distance", &config_.sensorless_ramp.finish_on_distance), + make_protocol_property("finish_on_enc_idx", &config_.sensorless_ramp.finish_on_enc_idx) + ), + make_protocol_object("general_lockin", make_protocol_property("current", &config_.lockin.current), make_protocol_property("ramp_time", &config_.lockin.ramp_time), make_protocol_property("ramp_distance", &config_.lockin.ramp_distance), diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 374d882b..7dcdd57a 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -93,6 +93,8 @@ void Encoder::set_linear_count(int32_t count) { // Update states shadow_count_ = count; pos_estimate_ = (float)count; + tim_cnt_sample_ = count; + //Write hardware last hw_config_.timer->Instance->CNT = count; @@ -125,20 +127,17 @@ bool Encoder::run_index_search() { } set_idx_subscribe(); - bool orig_finish_on_enc_idx = axis_->config_.lockin.finish_on_enc_idx; - axis_->config_.lockin.finish_on_enc_idx = true; - bool status = axis_->run_lockin_spin(); - axis_->config_.lockin.finish_on_enc_idx = orig_finish_on_enc_idx; + bool status = axis_->run_lockin_spin(axis_->config_.calibration_lockin); return status; } bool Encoder::run_direction_find() { int32_t init_enc_val = shadow_count_; - bool orig_finish_on_distance = axis_->config_.lockin.finish_on_distance; - axis_->config_.lockin.finish_on_distance = true; + bool orig_finish_on_distance = axis_->config_.calibration_lockin.finish_on_distance; + axis_->config_.calibration_lockin.finish_on_distance = true; axis_->motor_.config_.direction = 1; // Must test spin forwards for direction detect logic - bool status = axis_->run_lockin_spin(); - axis_->config_.lockin.finish_on_distance = orig_finish_on_distance; + bool status = axis_->run_lockin_spin(axis_->config_.calibration_lockin); + axis_->config_.calibration_lockin.finish_on_distance = orig_finish_on_distance; if (status) { // Check response and direction diff --git a/docs/commands.md b/docs/commands.md index e6ddaa09..a5f4675e 100644 --- a/docs/commands.md +++ b/docs/commands.md @@ -121,6 +121,7 @@ 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.vel_setpoint = 400 +odrv0.axis0.motor.config.direction = 1 odrv0.axis0.sensorless_estimator.config.pm_flux_linkage = 5.51328895422 / ( * ) ``` diff --git a/docs/encoders.md b/docs/encoders.md index c3d42fb0..19251d7e 100644 --- a/docs/encoders.md +++ b/docs/encoders.md @@ -86,6 +86,12 @@ and now see if after a reboot you can run: without getting errors. +### Reversing index search +Sometimes you would like the index search to only happen in a particular direction (the reverse of the default), instead of swapping the motor leads, you can run the following commands +* `.config.calibration_lockin.vel = -.config.calibration_lockin.vel` +* `.config.calibration_lockin.accel = -.config.calibration_lockin.accel` +* `.config.calibration_lockin.ramp_distance = -.config.calibration_lockin.ramp_distance` + ## What happens if calibration fails There are subtle ways that encoder problems will impact your ODrive. For example, ODrive may not complete the calibrate sequence when you go to: * `.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE` From 526a193524f910fc30358a016e6a3a2432ac0529 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 2 Jun 2019 19:12:54 -0700 Subject: [PATCH 2/4] Update encoders.md --- docs/encoders.md | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/docs/encoders.md b/docs/encoders.md index 19251d7e..c7008fb3 100644 --- a/docs/encoders.md +++ b/docs/encoders.md @@ -40,9 +40,15 @@ Below are the steps to do the one-time calibration and configuration. Note that That's it, now on every reboot the motor will turn in one direction until it finds the encoder index. -* If you wish to scan for the index pulse in the other direction, that feature is currently undocumented. * If your motor has problems reaching the index location due to the mechanical load, you can increase `.motor.config.calibration_current`. +### Reversing index search +Sometimes you would like the index search to only happen in a particular direction (the reverse of the default), instead of swapping the motor leads, you can run the following commands +* `.config.calibration_lockin.vel = -.config.calibration_lockin.vel` +* `.config.calibration_lockin.accel = -.config.calibration_lockin.accel` +* `.config.calibration_lockin.ramp_distance = -.config.calibration_lockin.ramp_distance` + + *IMPORTANT:* Your motor should find the same rotational position when the ODrive performs an index search if the index signal is working properly. This means that the motor should spin, and stop at the same position if you have set .config.startup_encoder_index_search so the search starts on reboot, or you if call the command:.requested_state = AXIS_STATE_ENCODER_INDEX_SEARCH after reboot. You can test this. Send the reboot() command, and while it's rebooting turn your motor, then make sure the motor returns back to the correct position each time when it comes out of reboot. Try this procedure a couple of times to be sure. ### Startup sequence notes @@ -86,12 +92,6 @@ and now see if after a reboot you can run: without getting errors. -### Reversing index search -Sometimes you would like the index search to only happen in a particular direction (the reverse of the default), instead of swapping the motor leads, you can run the following commands -* `.config.calibration_lockin.vel = -.config.calibration_lockin.vel` -* `.config.calibration_lockin.accel = -.config.calibration_lockin.accel` -* `.config.calibration_lockin.ramp_distance = -.config.calibration_lockin.ramp_distance` - ## What happens if calibration fails There are subtle ways that encoder problems will impact your ODrive. For example, ODrive may not complete the calibrate sequence when you go to: * `.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE` From a3755b848de92fbb0085bb63e55503b3e28264c3 Mon Sep 17 00:00:00 2001 From: Ben Wang Date: Mon, 3 Jun 2019 12:06:50 -0400 Subject: [PATCH 3/4] use default constructor for general lockin struct --- Firmware/MotorControl/axis.cpp | 14 -------------- Firmware/MotorControl/axis.hpp | 2 +- 2 files changed, 1 insertion(+), 15 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index cbcc1e63..e5f25b16 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -61,20 +61,6 @@ Axis::LockinConfig_t Axis::default_sensorless() { return config; } -Axis::LockinConfig_t Axis::default_lockin() { - Axis::LockinConfig_t config; - config.current = 10.0f; // [A] - config.ramp_time = 0.4f; // [s] - config.ramp_distance = 1 * M_PI; // [rad] - config.accel = 20.0f; // [rad/s^2] - config.vel = 400.0f; // [rad/s] - config.finish_distance = 100.0f; // [rad] - config.finish_on_vel = false; - config.finish_on_distance = false; - config.finish_on_enc_idx = false; - return config; -} - static void step_cb_wrapper(void* ctx) { reinterpret_cast(ctx)->step_cb(); } diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index b4c69ca7..c23fb2f1 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -71,8 +71,8 @@ public: uint16_t dir_gpio_pin = 0; LockinConfig_t calibration_lockin = default_calibration(); - LockinConfig_t lockin = default_lockin(); LockinConfig_t sensorless_ramp = default_sensorless(); + LockinConfig_t lockin; }; enum thread_signals { From 68d760e86adbdf9ae67381a0ac7a0d16fe26db7d Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 21 Jun 2019 11:27:59 -0700 Subject: [PATCH 4/4] Update encoders.md --- docs/encoders.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/docs/encoders.md b/docs/encoders.md index c7008fb3..c9a8aaab 100644 --- a/docs/encoders.md +++ b/docs/encoders.md @@ -43,10 +43,10 @@ That's it, now on every reboot the motor will turn in one direction until it fin * If your motor has problems reaching the index location due to the mechanical load, you can increase `.motor.config.calibration_current`. ### Reversing index search -Sometimes you would like the index search to only happen in a particular direction (the reverse of the default), instead of swapping the motor leads, you can run the following commands -* `.config.calibration_lockin.vel = -.config.calibration_lockin.vel` -* `.config.calibration_lockin.accel = -.config.calibration_lockin.accel` -* `.config.calibration_lockin.ramp_distance = -.config.calibration_lockin.ramp_distance` +Sometimes you would like the index search to only happen in a particular direction (the reverse of the default), instead of swapping the motor leads, you can ensure the following three values are negative: +* `.config.calibration_lockin.vel` +* `.config.calibration_lockin.accel` +* `.config.calibration_lockin.ramp_distance` *IMPORTANT:* Your motor should find the same rotational position when the ODrive performs an index search if the index signal is working properly. This means that the motor should spin, and stop at the same position if you have set .config.startup_encoder_index_search so the search starts on reboot, or you if call the command:.requested_state = AXIS_STATE_ENCODER_INDEX_SEARCH after reboot. You can test this. Send the reboot() command, and while it's rebooting turn your motor, then make sure the motor returns back to the correct position each time when it comes out of reboot. Try this procedure a couple of times to be sure.