diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 97d97170..2afb00b3 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -39,6 +39,34 @@ Axis::Axis(int axis_num, max_endstop_.axis_ = this; } +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; +} + static void step_cb_wrapper(void* ctx) { reinterpret_cast(ctx)->step_cb(); } @@ -193,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; }; @@ -226,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 }); @@ -241,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(); }); @@ -405,17 +433,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 7ce82ace..2f100d83 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -61,6 +61,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 c67dd228..0cc64979 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -98,6 +98,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; @@ -130,20 +132,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..c9a8aaab 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 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. ### Startup sequence notes