From e88abb56eba892b442b07e9acf256c338f4260c0 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 24 Sep 2018 22:59:36 -0700 Subject: [PATCH] pull in configs and enum into class for Axis and Motor, for consistency --- Firmware/MotorControl/axis.cpp | 2 +- Firmware/MotorControl/axis.hpp | 78 ++++++++++++------------- Firmware/MotorControl/encoder.cpp | 8 +-- Firmware/MotorControl/main.cpp | 12 ++-- Firmware/MotorControl/motor.cpp | 29 +++++----- Firmware/MotorControl/motor.hpp | 96 +++++++++++++++---------------- 6 files changed, 113 insertions(+), 112 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 52d4c125..4ff660fd 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -7,7 +7,7 @@ #include "odrive_main.h" Axis::Axis(const AxisHardwareConfig_t& hw_config, - AxisConfig_t& config, + Config_t& config, Encoder& encoder, SensorlessEstimator& sensorless_estimator, Controller& controller, diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 4eac2c4b..6063ee3a 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -5,40 +5,6 @@ #error "This file should not be included directly. Include odrive_main.h instead." #endif -// Warning: Do not reorder these enum values. -// The state machine uses ">" comparision on them. -enum AxisState_t { - AXIS_STATE_UNDEFINED = 0, //" comparision on them. + enum State_t { + AXIS_STATE_UNDEFINED = 0, //motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) + 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_TYPE_GIMBAL) + else if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL) voltage_magnitude = axis_->motor_.config_.calibration_current; else return false; @@ -142,9 +142,9 @@ bool Encoder::run_offset_calibration() { shadow_count_ = count_in_cpr_; float voltage_magnitude; - if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) + 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_TYPE_GIMBAL) + else if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL) voltage_magnitude = axis_->motor_.config_.calibration_current; else return false; diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index d9f14ebc..4550605e 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -12,8 +12,8 @@ BoardConfig_t board_config; Encoder::Config_t encoder_configs[AXIS_COUNT]; SensorlessEstimator::Config_t sensorless_configs[AXIS_COUNT]; Controller::Config_t controller_configs[AXIS_COUNT]; -MotorConfig_t motor_configs[AXIS_COUNT]; -AxisConfig_t axis_configs[AXIS_COUNT]; +Motor::Config_t motor_configs[AXIS_COUNT]; +Axis::Config_t axis_configs[AXIS_COUNT]; TrapezoidalTrajectory::Config_t trap_configs[AXIS_COUNT]; bool user_config_loaded_; @@ -26,9 +26,9 @@ typedef Config< Encoder::Config_t[AXIS_COUNT], SensorlessEstimator::Config_t[AXIS_COUNT], Controller::Config_t[AXIS_COUNT], - MotorConfig_t[AXIS_COUNT], + Motor::Config_t[AXIS_COUNT], TrapezoidalTrajectory::Config_t[AXIS_COUNT], - AxisConfig_t[AXIS_COUNT]> ConfigFormat; + Axis::Config_t[AXIS_COUNT]> ConfigFormat; void save_configuration(void) { if (ConfigFormat::safe_store_config( @@ -62,9 +62,9 @@ void load_configuration(void) { encoder_configs[i] = Encoder::Config_t(); sensorless_configs[i] = SensorlessEstimator::Config_t(); controller_configs[i] = Controller::Config_t(); - motor_configs[i] = MotorConfig_t(); + motor_configs[i] = Motor::Config_t(); trap_configs[i] = TrapezoidalTrajectory::Config_t(); - axis_configs[i] = AxisConfig_t(); + axis_configs[i] = Axis::Config_t(); } } else { user_config_loaded_ = true; diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 56817ee8..1782414c 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -6,8 +6,8 @@ Motor::Motor(const MotorHardwareConfig_t& hw_config, - const GateDriverHardwareConfig_t& gate_driver_config, - MotorConfig_t& config) : + const GateDriverHardwareConfig_t& gate_driver_config, + Config_t& config) : hw_config_(hw_config), gate_driver_config_(gate_driver_config), config_(config), @@ -298,10 +298,11 @@ bool Motor::FOC_voltage(float v_d, float v_q, float phase) { } bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { - Current_control_t* ictrl = ¤t_control_; + // Syntactic sugar + CurrentControl_t& ictrl = current_control_; // For Reporting - ictrl->Iq_setpoint = Iq_des; + ictrl.Iq_setpoint = Iq_des; // Clarke transform float Ialpha = -current_meas_.phB - current_meas_.phC; @@ -312,7 +313,7 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { float s = arm_sin_f32(phase); float Id = c * Ialpha + s * Ibeta; float Iq = c * Ibeta - s * Ialpha; - ictrl->Iq_measured = Iq; + ictrl.Iq_measured = Iq; // Current error float Ierr_d = Id_des - Id; @@ -320,8 +321,8 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { // TODO look into feed forward terms (esp omega, since PI pole maps to RL tau) // Apply PI control - float Vd = ictrl->v_current_control_integral_d + Ierr_d * ictrl->p_gain; - float Vq = ictrl->v_current_control_integral_q + Ierr_q * ictrl->p_gain; + float Vd = ictrl.v_current_control_integral_d + Ierr_d * ictrl.p_gain; + float Vq = ictrl.v_current_control_integral_q + Ierr_q * ictrl.p_gain; float mod_to_V = (2.0f / 3.0f) * vbus_voltage; float V_to_mod = 1.0f / mod_to_V; @@ -335,23 +336,23 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { mod_d *= mod_scalefactor; mod_q *= mod_scalefactor; // TODO make decayfactor configurable - ictrl->v_current_control_integral_d *= 0.99f; - ictrl->v_current_control_integral_q *= 0.99f; + ictrl.v_current_control_integral_d *= 0.99f; + ictrl.v_current_control_integral_q *= 0.99f; } else { - ictrl->v_current_control_integral_d += Ierr_d * (ictrl->i_gain * current_meas_period); - ictrl->v_current_control_integral_q += Ierr_q * (ictrl->i_gain * current_meas_period); + ictrl.v_current_control_integral_d += Ierr_d * (ictrl.i_gain * current_meas_period); + ictrl.v_current_control_integral_q += Ierr_q * (ictrl.i_gain * current_meas_period); } // Compute estimated bus current - ictrl->Ibus = mod_d * Id + mod_q * Iq; + ictrl.Ibus = mod_d * Id + mod_q * Iq; // Inverse park transform float mod_alpha = c * mod_d - s * mod_q; float mod_beta = c * mod_q + s * mod_d; // Report final applied voltage in stationary frame (for sensorles estimator) - ictrl->final_v_alpha = mod_to_V * mod_alpha; - ictrl->final_v_beta = mod_to_V * mod_beta; + ictrl.final_v_alpha = mod_to_V * mod_alpha; + ictrl.final_v_beta = mod_to_V * mod_beta; // Apply SVM if (!enqueue_modulation_timings(mod_alpha, mod_beta)) diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 07820726..235ca6a8 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -7,51 +7,6 @@ #include "drv8301.h" -typedef enum { - MOTOR_TYPE_HIGH_CURRENT = 0, - // MOTOR_TYPE_LOW_CURRENT = 1, //Not yet implemented - MOTOR_TYPE_GIMBAL = 2 -} Motor_type_t; - -typedef struct { - float phB; - float phC; -} Iph_BC_t; - -typedef struct { - float p_gain; // [V/A] - float i_gain; // [V/As] - float v_current_control_integral_d; // [V] - float v_current_control_integral_q; // [V] - float Ibus; // DC bus current [A] - // Voltage applied at end of cycle: - float final_v_alpha; // [V] - float final_v_beta; // [V] - float Iq_setpoint; - float Iq_measured; - float max_allowed_current; -} Current_control_t; - -// NOTE: for gimbal motors, all units of A are instead V. -// 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 pre_calibrated = false; // can be set to true to indicate that all values here are valid - int32_t pole_pairs = 7; - 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. - 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 - Motor_type_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] - float current_lim = 10.0f; //[A] - // Value used to compute shunt amplifier gains - float requested_current_range = 70.0f; // [A] - float current_control_bandwidth = 1000.0f; // [rad/s] -} MotorConfig_t; - class Motor { public: enum Error_t { @@ -68,6 +23,51 @@ public: ERROR_UNEXPECTED_TIMER_CALLBACK = 0x0200 }; + enum MotorType_t { + MOTOR_TYPE_HIGH_CURRENT = 0, + // MOTOR_TYPE_LOW_CURRENT = 1, //Not yet implemented + MOTOR_TYPE_GIMBAL = 2 + }; + + struct Iph_BC_t { + float phB; + float phC; + }; + + struct CurrentControl_t{ + float p_gain; // [V/A] + float i_gain; // [V/As] + float v_current_control_integral_d; // [V] + float v_current_control_integral_q; // [V] + float Ibus; // DC bus current [A] + // Voltage applied at end of cycle: + float final_v_alpha; // [V] + float final_v_beta; // [V] + float Iq_setpoint; + float Iq_measured; + float max_allowed_current; + }; + + // NOTE: for gimbal motors, all units of A are instead V. + // 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. + struct Config_t { + bool pre_calibrated = false; // can be set to true to indicate that all values here are valid + int32_t pole_pairs = 7; + 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. + 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 + 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] + float current_lim = 10.0f; //[A] + // Value used to compute shunt amplifier gains + float requested_current_range = 70.0f; // [A] + float current_control_bandwidth = 1000.0f; // [rad/s] + }; + enum TimingLog_t { TIMING_LOG_GENERAL, TIMING_LOG_ADC_CB_I, @@ -90,7 +90,7 @@ public: Motor(const MotorHardwareConfig_t& hw_config, const GateDriverHardwareConfig_t& gate_driver_config, - MotorConfig_t& config); + Config_t& config); bool arm(); void disarm(); @@ -119,7 +119,7 @@ public: const MotorHardwareConfig_t& hw_config_; const GateDriverHardwareConfig_t gate_driver_config_; - MotorConfig_t& config_; + Config_t& config_; Axis* axis_ = nullptr; // set by Axis constructor //private: @@ -144,7 +144,7 @@ public: Iph_BC_t current_meas_ = {0.0f, 0.0f}; Iph_BC_t DC_calib_ = {0.0f, 0.0f}; float phase_current_rev_gain_ = 0.0f; // Reverse gain for ADC to Amps (to be set by DRV8301_setup) - Current_control_t current_control_ = { + CurrentControl_t current_control_ = { .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement .v_current_control_integral_d = 0.0f,