diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 31e6a354..457d0279 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -137,7 +137,7 @@ bool Axis::run_lockin_spin() { lockin_state_ = LOCKIN_STATE_RAMP; float x = 0.0f; run_control_loop([&]() { - float phase = wrap_pm_pi(config_.lockin_ramp_time * x); + 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; if (!motor_.update(I_mag, phase)) @@ -146,7 +146,7 @@ bool Axis::run_lockin_spin() { }); // Spin states - float distance = config_.lockin_ramp_time; + float distance = config_.lockin_ramp_distance; float phase = wrap_pm_pi(distance); float vel = distance / config_.lockin_ramp_time; @@ -154,7 +154,7 @@ bool Axis::run_lockin_spin() { auto spin_done = [&](bool vel_override = false) -> bool { bool done = false; if (config_.lockin_finish_on_vel || vel_override) - done = done || vel >= config_.lockin_vel; + 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) @@ -298,7 +298,10 @@ void Axis::run_state_machine_loop() { case AXIS_STATE_ENCODER_INDEX_SEARCH: { if (!motor_.is_calibrated_) goto invalid_state_label; + if (encoder_.config_.idx_search_unidirectional && motor_.config_.direction==0) + goto invalid_state_label; + // TODO: move code body to function in Encoder encoder_.config_.use_index = true; encoder_.index_found_ = false; @@ -306,7 +309,32 @@ void Axis::run_state_machine_loop() { config_.lockin_finish_on_enc_idx = true; status = run_lockin_spin(); config_.lockin_finish_on_enc_idx = orig_setting; - // status = encoder_.run_index_search(); + } break; + + case AXIS_STATE_ENCODER_DIR_FIND: { + if (!motor_.is_calibrated_) + goto invalid_state_label; + + // TODO: move code body to function in Encoder + int32_t init_enc_val = encoder_.shadow_count_; + bool orig_setting = config_.lockin_finish_on_distance; + config_.lockin_finish_on_distance = true; + motor_.config_.direction = 1; // Must test spin forwards for direction detect logic + status = run_lockin_spin(); + config_.lockin_finish_on_distance = orig_setting; + + if (status) { + // Check response and direction + if (encoder_.shadow_count_ > init_enc_val + 8) { + // motor same dir as encoder + motor_.config_.direction = 1; + } else if (encoder_.shadow_count_ < init_enc_val - 8) { + // motor opposite dir as encoder + motor_.config_.direction = -1; + } else { + motor_.config_.direction = 0; + } + } } break; case AXIS_STATE_ENCODER_OFFSET_CALIBRATION: { @@ -316,13 +344,13 @@ void Axis::run_state_machine_loop() { } break; case AXIS_STATE_LOCKIN_SPIN: { - if (!motor_.is_calibrated_) + if (!motor_.is_calibrated_ || motor_.config_.direction==0) goto invalid_state_label; status = run_lockin_spin(); } break; case AXIS_STATE_SENSORLESS_CONTROL: { - if (!motor_.is_calibrated_) + if (!motor_.is_calibrated_ || motor_.config_.direction==0) goto invalid_state_label; status = run_lockin_spin(); // TODO: restart if desired if (status) { @@ -334,7 +362,7 @@ void Axis::run_state_machine_loop() { } break; case AXIS_STATE_CLOSED_LOOP_CONTROL: { - if (!motor_.is_calibrated_) + if (!motor_.is_calibrated_ || motor_.config_.direction==0) goto invalid_state_label; if (!encoder_.is_ready_) goto invalid_state_label; diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 909b0e66..3d89d3a0 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -33,6 +33,7 @@ public: AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7, // config_.overspeed_fault_ratio * axis_->controller_.config_.vel_limit) { + set_error(ERROR_OVERSPEED); + return false; + } + } + //// run encoder count interpolation int32_t corrected_enc = count_in_cpr_ - config_.offset; // if we are stopped, make sure we don't randomly drift diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index ff0d82b7..38889c12 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -15,6 +15,7 @@ public: ERROR_UNSUPPORTED_ENCODER_MODE = 0x08, ERROR_ILLEGAL_HALL_STATE = 0x10, ERROR_INDEX_NOT_FOUND_YET = 0x20, + ERROR_OVERSPEED = 0x40, }; enum Mode_t { @@ -36,7 +37,9 @@ public: float offset_float = 0.0f; // Sub-count phase alignment offset float calib_range = 0.02f; float bandwidth = 1000.0f; + float overspeed_fault_ratio = 1.2f; // ratio of vel_lim, 0.0f = disabled bool find_idx_on_lockin = false; + bool idx_search_unidirectional = false; }; Encoder(const EncoderHardwareConfig_t& hw_config, @@ -98,6 +101,7 @@ public: make_protocol_property("mode", &config_.mode), make_protocol_property("use_index", &config_.use_index), make_protocol_property("pre_calibrated", &config_.pre_calibrated), + make_protocol_property("overspeed_fault_ratio", &config_.overspeed_fault_ratio), make_protocol_property("idx_search_speed", &config_.idx_search_speed), make_protocol_property("cpr", &config_.cpr), make_protocol_property("offset", &config_.offset), @@ -105,7 +109,8 @@ public: make_protocol_property("bandwidth", &config_.bandwidth, [](void* ctx) { static_cast(ctx)->update_pll_gains(); }, this), make_protocol_property("calib_range", &config_.calib_range), - make_protocol_property("find_idx_on_lockin", &config_.find_idx_on_lockin) + make_protocol_property("find_idx_on_lockin", &config_.find_idx_on_lockin), + make_protocol_property("idx_search_unidirectional", &config_.idx_search_unidirectional) ) ); } diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index ebfd3a12..b67d4874 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -58,7 +58,7 @@ public: float resistance_calib_max_voltage = 2.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor. float phase_inductance = 0.0f; // to be set by measure_phase_inductance float phase_resistance = 0.0f; // to be set by measure_phase_resistance - int32_t direction = 1; // 1 or -1 + int32_t direction = 0; // 1 or -1 (0 = unspecified) MotorType_t motor_type = MOTOR_TYPE_HIGH_CURRENT; // Read out max_allowed_current to see max supported value for current_lim. // float current_lim = 70.0f; //[A] diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index 677bcbed..ec374bdc 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -11,6 +11,7 @@ AXIS_STATE_ENCODER_INDEX_SEARCH = 6 AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7 AXIS_STATE_CLOSED_LOOP_CONTROL = 8 AXIS_STATE_LOCKIN_SPIN = 9 +AXIS_STATE_ENCODER_DIR_FIND = 10 AXIS_ERROR_NONE = 0 AXIS_ERROR_INVALID_STATE = 1