diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 05140c06..31e6a354 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -291,29 +291,37 @@ void Axis::run_state_machine_loop() { // Handlers should exit if requested_state != AXIS_STATE_UNDEFINED bool status; switch (current_state_) { - case AXIS_STATE_MOTOR_CALIBRATION: + case AXIS_STATE_MOTOR_CALIBRATION: { status = motor_.run_calibration(); - break; + } break; - case AXIS_STATE_ENCODER_INDEX_SEARCH: + case AXIS_STATE_ENCODER_INDEX_SEARCH: { if (!motor_.is_calibrated_) goto invalid_state_label; - status = encoder_.run_index_search(); - break; - case AXIS_STATE_ENCODER_OFFSET_CALIBRATION: + encoder_.config_.use_index = true; + encoder_.index_found_ = false; + + bool orig_setting = config_.lockin_finish_on_enc_idx; + 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_OFFSET_CALIBRATION: { if (!motor_.is_calibrated_) goto invalid_state_label; status = encoder_.run_offset_calibration(); - break; + } break; - case AXIS_STATE_LOCKIN_SPIN: + case AXIS_STATE_LOCKIN_SPIN: { if (!motor_.is_calibrated_) goto invalid_state_label; status = run_lockin_spin(); - break; + } break; - case AXIS_STATE_SENSORLESS_CONTROL: + case AXIS_STATE_SENSORLESS_CONTROL: { if (!motor_.is_calibrated_) goto invalid_state_label; status = run_lockin_spin(); // TODO: restart if desired @@ -323,20 +331,20 @@ void Axis::run_state_machine_loop() { controller_.vel_setpoint_ = config_.lockin_vel; status = run_sensorless_control_loop(); } - break; + } break; - case AXIS_STATE_CLOSED_LOOP_CONTROL: + case AXIS_STATE_CLOSED_LOOP_CONTROL: { if (!motor_.is_calibrated_) goto invalid_state_label; if (!encoder_.is_ready_) goto invalid_state_label; status = run_closed_loop_control_loop(); - break; + } break; - case AXIS_STATE_IDLE: + case AXIS_STATE_IDLE: { run_idle_loop(); status = motor_.arm(); // done with idling - try to arm the motor - break; + } break; default: invalid_state_label: diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 2755ba43..909b0e66 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -50,13 +50,13 @@ public: // Spinup settings float lockin_current = 10.0f; // [A] float lockin_ramp_time = 0.4f; // [s] - float lockin_ramp_distance = 4 * M_PI; // [rad] - float lockin_accel = 400.0f; // [rad/s^2] - float lockin_vel = 400.0f; // [rad/s] - bool lockin_finish_on_vel = true; + float lockin_ramp_distance = 1 * M_PI; // [rad] + float lockin_accel = 10.0f; // [rad/s^2] + float lockin_vel = 100.0f; // [rad/s] + float lockin_finish_distance = 1000.0f; // [rad] + bool lockin_finish_on_vel = false; bool lockin_finish_on_distance = false; bool lockin_finish_on_enc_idx = false; - float lockin_finish_distance = 1000.0f; // [rad] }; enum thread_signals { diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 45885c29..781ea1d2 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -91,38 +91,6 @@ void Encoder::set_circular_count(int32_t count, bool update_offset) { cpu_exit_critical(prim); } - -// @brief Slowly turns the motor in one direction until the -// encoder index is found. -// TODO: Do the scan with current, not voltage! -bool Encoder::run_index_search() { - float voltage_magnitude; - if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_HIGH_CURRENT) - voltage_magnitude = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance; - else if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL) - voltage_magnitude = axis_->motor_.config_.calibration_current; - else - return false; - - float omega = (float)(axis_->motor_.config_.direction) * config_.idx_search_speed; - - index_found_ = false; - float phase = 0.0f; - axis_->run_control_loop([&](){ - phase = wrap_pm_pi(phase + omega * current_meas_period); - - float v_alpha = voltage_magnitude * our_arm_cos_f32(phase); - float v_beta = voltage_magnitude * our_arm_sin_f32(phase); - if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta)) - return false; // error set inside enqueue_voltage_timings - axis_->motor_.log_timing(Motor::TIMING_LOG_IDX_SEARCH); - - // continue until the index is found - return !index_found_; - }); - return true; -} - // @brief Turns the motor in one direction for a bit and then in the other // direction in order to find the offset between the electrical phase 0 // and the encoder state 0. diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index baa32ad0..ff0d82b7 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -51,7 +51,6 @@ public: void set_linear_count(int32_t count); void set_circular_count(int32_t count, bool update_offset); bool calib_enc_offset(float voltage_magnitude); - bool scan_for_enc_idx(float omega, float voltage_magnitude); bool run_index_search(); bool run_offset_calibration(); diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index ca73b315..677bcbed 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -7,10 +7,10 @@ AXIS_STATE_STARTUP_SEQUENCE = 2 AXIS_STATE_FULL_CALIBRATION_SEQUENCE = 3 AXIS_STATE_MOTOR_CALIBRATION = 4 AXIS_STATE_SENSORLESS_CONTROL = 5 -AXIS_STATE_LOCKIN_SPIN = 6 -AXIS_STATE_ENCODER_INDEX_SEARCH = 7 -AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 8 -AXIS_STATE_CLOSED_LOOP_CONTROL = 9 +AXIS_STATE_ENCODER_INDEX_SEARCH = 6 +AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7 +AXIS_STATE_CLOSED_LOOP_CONTROL = 8 +AXIS_STATE_LOCKIN_SPIN = 9 AXIS_ERROR_NONE = 0 AXIS_ERROR_INVALID_STATE = 1