From f69685df13b954f2cd087010ef7ef04c74deeff4 Mon Sep 17 00:00:00 2001 From: Ben Wang Date: Wed, 29 May 2019 16:26:07 -0400 Subject: [PATCH] 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`