diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index e186e864..d6c718e4 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -214,8 +214,10 @@ void Axis::run_state_machine_loop() { if (requested_state_ == AXIS_STATE_STARTUP_SEQUENCE) { if (config_.startup_motor_calibration) task_chain_[pos++] = AXIS_STATE_MOTOR_CALIBRATION; - if (config_.startup_encoder_calibration) - task_chain_[pos++] = AXIS_STATE_ENCODER_CALIBRATION; + if (config_.startup_encoder_index_search && encoder_.config_.use_index) + task_chain_[pos++] = AXIS_STATE_ENCODER_INDEX_SEARCH; + if (config_.startup_encoder_offset_calibration) + task_chain_[pos++] = AXIS_STATE_ENCODER_OFFSET_CALIBRATION; if (config_.startup_closed_loop_control) task_chain_[pos++] = AXIS_STATE_CLOSED_LOOP_CONTROL; else if (config_.startup_sensorless_control) @@ -223,7 +225,9 @@ void Axis::run_state_machine_loop() { task_chain_[pos++] = AXIS_STATE_IDLE; } else if (requested_state_ == AXIS_STATE_FULL_CALIBRATION_SEQUENCE) { task_chain_[pos++] = AXIS_STATE_MOTOR_CALIBRATION; - task_chain_[pos++] = AXIS_STATE_ENCODER_CALIBRATION; + if (encoder_.config_.use_index) + task_chain_[pos++] = AXIS_STATE_ENCODER_INDEX_SEARCH; + task_chain_[pos++] = AXIS_STATE_ENCODER_OFFSET_CALIBRATION; task_chain_[pos++] = AXIS_STATE_IDLE; } else if (requested_state_ != AXIS_STATE_UNDEFINED) { task_chain_[pos++] = requested_state_; @@ -239,7 +243,7 @@ void Axis::run_state_machine_loop() { // Validate the state before running it if (current_state_ > AXIS_STATE_MOTOR_CALIBRATION && !motor_.is_calibrated_) current_state_ = AXIS_STATE_UNDEFINED; - if (current_state_ > AXIS_STATE_ENCODER_CALIBRATION && !encoder_.is_calibrated_) + if (current_state_ > AXIS_STATE_ENCODER_OFFSET_CALIBRATION && !encoder_.is_ready_) current_state_ = AXIS_STATE_UNDEFINED; // Run the specified state @@ -250,8 +254,12 @@ void Axis::run_state_machine_loop() { status = motor_.run_calibration(); break; - case AXIS_STATE_ENCODER_CALIBRATION: - status = encoder_.run_calibration(); + case AXIS_STATE_ENCODER_INDEX_SEARCH: + status = encoder_.run_index_search(); + break; + + case AXIS_STATE_ENCODER_OFFSET_CALIBRATION: + status = encoder_.run_offset_calibration(); break; case AXIS_STATE_SENSORLESS_CONTROL: diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index d4feadbd..65ef9d97 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -8,19 +8,22 @@ // Warning: Do not reorder these enum values. // The state machine uses ">" comparision on them. enum AxisState_t { - AXIS_STATE_UNDEFINED, //Instance->CNT = count; pll_pos_ = (float)count; @@ -53,14 +59,59 @@ void Encoder::set_count(int32_t count) { } +// @brief Slowly turns the motor in one direction until the +// encoder index is found. // TODO: Do the scan with current, not voltage! -// TODO: add check_timing -bool Encoder::calib_enc_offset(float voltage_magnitude) { +bool Encoder::run_index_search() { + float voltage_magnitude; + if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) + voltage_magnitude = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance; + else if (axis_->motor_.config_.motor_type == 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 * arm_cos_f32(phase); + float v_beta = voltage_magnitude * arm_sin_f32(phase); + axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta); + axis_->motor_.log_timing(Motor::TIMING_LOG_IDX_SEARCH); + + // continue until the index is found + return !index_found_; + }); + return axis_->error_ != Axis::ERROR_NO_ERROR; +} + +// @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. +// TODO: Do the scan with current, not voltage! +bool Encoder::run_offset_calibration() { static const float start_lock_duration = 1.0f; static const float scan_omega = 4.0f * M_PI; static const float scan_distance = 16.0f * M_PI; static const int num_steps = scan_distance / scan_omega * current_meas_hz; + // Temporarily disable index search so it doesn't mess + // with the offset calibration + bool old_use_index = config_.use_index; + config_.use_index = true; + + float voltage_magnitude; + if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) + voltage_magnitude = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance; + else if (axis_->motor_.config_.motor_type == MOTOR_TYPE_GIMBAL) + voltage_magnitude = axis_->motor_.config_.calibration_current; + else + return false; + // go to motor zero phase for start_lock_duration to get ready to scan int i = 0; axis_->run_control_loop([&](){ @@ -128,46 +179,9 @@ bool Encoder::calib_enc_offset(float voltage_magnitude) { if (axis_->error_ != Axis::ERROR_NO_ERROR) return false; - int offset = encvaluesum / (num_steps * 2); - config_.offset = offset; - is_calibrated_ = true; - return true; -} - -bool Encoder::scan_for_enc_idx(float omega, float voltage_magnitude) { - 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 * arm_cos_f32(phase); - float v_beta = voltage_magnitude * arm_sin_f32(phase); - axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta); - axis_->motor_.log_timing(Motor::TIMING_LOG_IDX_SEARCH); - - // continue until the index is found - return !index_found_; - }); - return axis_->error_ == Axis::ERROR_NO_ERROR; -} - -bool Encoder::run_calibration() { - float enc_calibration_voltage; - if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) - enc_calibration_voltage = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance; - else if (axis_->motor_.config_.motor_type == MOTOR_TYPE_GIMBAL) - enc_calibration_voltage = axis_->motor_.config_.calibration_current; - else - return false; - - if (config_.use_index && !index_found_) - if (!scan_for_enc_idx( - (float)(axis_->motor_.config_.direction) * config_.idx_search_speed, - enc_calibration_voltage)) - return false; - if (!config_.hand_calibrated) // TODO: discuss what logic we want here - if (!calib_enc_offset(enc_calibration_voltage)) - return false; + offset_ = encvaluesum / (num_steps * 2); + is_ready_ = true; + config_.use_index = old_use_index; return true; } @@ -184,7 +198,7 @@ bool Encoder::update(float* pos_estimate, float* vel_estimate, float* phase_outp // compute electrical phase int corrected_enc = state_ % config_.cpr; - corrected_enc -= config_.offset; + corrected_enc -= offset_; //corrected_enc *= axis_->motor_.config_.direction; TODO: verify if this still works //TODO avoid recomputing elec_rad_per_enc every time float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr)); diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index cc1701a0..f3dbc78c 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -7,10 +7,15 @@ struct EncoderConfig_t { bool use_index = false; - bool hand_calibrated = false; + bool pre_calibrated = false; // If true, this means the offset stored in + // configuration is valid and does not need + // be determined by run_offset_calibration. + // In this case the encoder will enter ready + // state as soon as the index is found. float idx_search_speed = 10.0f; // [rad/s electrical] int32_t cpr = (2048 * 4); // Default resolution of CUI-AMT102 encoder, - int32_t offset = 0; + int32_t offset = 0; // If pre_calibrated is true, this is copied into encoder.offset_ once + // index search succeeds float calib_range = 0.02; }; @@ -34,8 +39,9 @@ public: 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(); bool update(float* pos_estimate, float* vel_estimate, float* phase); - bool run_calibration(); const EncoderHardwareConfig_t& hw_config_; EncoderConfig_t& config_; @@ -43,8 +49,9 @@ public: Error_t error_ = ERROR_NONE; bool index_found_ = false; - bool is_calibrated_ = config_.hand_calibrated; + bool is_ready_ = false; int32_t state_ = 0; + int32_t offset_ = 0; float phase_ = 0.0f; // [rad] float pll_pos_ = 0.0f; // [rad] float pll_vel_ = 0.0f; // [rad/s] @@ -55,9 +62,10 @@ public: auto make_protocol_definitions() { return make_protocol_member_list( make_protocol_property("error", &error_), - make_protocol_ro_property("is_calibrated", &is_calibrated_), + make_protocol_ro_property("is_ready", &is_ready_), make_protocol_ro_property("index_found", const_cast(&index_found_)), make_protocol_property("state", &state_), + make_protocol_property("offset", &offset_), make_protocol_property("phase", &phase_), make_protocol_property("pll_pos", &pll_pos_), make_protocol_property("pll_vel", &pll_vel_), @@ -65,7 +73,7 @@ public: make_protocol_property("pll_ki", &pll_ki_), make_protocol_object("config", make_protocol_property("use_index", &config_.use_index), - make_protocol_property("hand_calibrated", &config_.hand_calibrated), + make_protocol_property("pre_calibrated", &config_.pre_calibrated), make_protocol_property("idx_search_speed", &config_.idx_search_speed), make_protocol_property("cpr", &config_.cpr), make_protocol_property("offset", &config_.offset), diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index caa133e7..2e771b27 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -36,7 +36,7 @@ typedef struct { // example: vel_gain is [V/(count/s)] instead of [A/(count/s)] // example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor. typedef struct { - bool hand_calibrated = false; // can be set to true to indicate that all values here are valid + bool pre_calibrated = false; // can be set to true to indicate that all values here are valid int32_t pole_pairs = 7; // This value is correct for N5065 motors and Turnigy SK3 series. float calibration_current = 10.0f; // [A] float resistance_calib_max_voltage = 1.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor. @@ -118,7 +118,7 @@ public: // variables exposed on protocol Error_t error_ = ERROR_NO_ERROR; - bool is_calibrated_ = config_.hand_calibrated; + bool is_calibrated_ = config_.pre_calibrated; Iph_BC_t current_meas_ = {0.0f, 0.0f}; Iph_BC_t DC_calib_ = {0.0f, 0.0f}; const float shunt_conductance_ = 1.0f / SHUNT_RESISTANCE; //[S] @@ -180,6 +180,7 @@ public: make_protocol_ro_property("TIMING_LOG_FOC_CURRENT", &timing_log_[TIMING_LOG_FOC_CURRENT]) ), make_protocol_object("config", + make_protocol_property("pre_calibrated", &config_.pre_calibrated), make_protocol_property("pole_pairs", &config_.pole_pairs), make_protocol_property("calibration_current", &config_.calibration_current), make_protocol_property("resistance_calib_max_voltage", &config_.resistance_calib_max_voltage),