From a2378dbf79f9ef6ec6ab2760ccdefeb4f017d7da Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 23 Mar 2018 16:53:18 -0700 Subject: [PATCH] make encoder index search and encoder offset calibration independent These two activities are now separate states of the axis state machine. Each of them can be invoked independently at any time (provided the motor is calibrated). --- Firmware/MotorControl/axis.cpp | 20 ++++-- Firmware/MotorControl/axis.hpp | 46 ++++++++------ Firmware/MotorControl/encoder.cpp | 102 +++++++++++++++++------------- Firmware/MotorControl/encoder.hpp | 20 ++++-- Firmware/MotorControl/motor.hpp | 5 +- 5 files changed, 114 insertions(+), 79 deletions(-) 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),