diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index d295246c..1e20c1f4 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -1,6 +1,6 @@ #include "odrive_main.h" - +#include Controller::Controller(Config_t& config) : config_(config) @@ -173,6 +173,19 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s } } + // TODO: Change to controller working in torque units + // Torque per amp gain scheduling (ACIM) + float vel_gain = config_.vel_gain; + float vel_integrator_gain = config_.vel_integrator_gain; + if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_ACIM) { + float effective_flux = axis_->motor_.current_control_.acim_rotor_flux; + float minflux = axis_->motor_.config_.acim_gain_min_flux; + if (fabsf(effective_flux) < minflux) + effective_flux = std::copysignf(minflux, effective_flux); + vel_gain /= effective_flux; + vel_integrator_gain /= effective_flux; + } + // Velocity control float Iq = current_setpoint_; @@ -185,13 +198,15 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s float v_err = vel_des - vel_estimate; if (config_.control_mode >= CTRL_MODE_VELOCITY_CONTROL) { - Iq += config_.vel_gain * v_err; + Iq += vel_gain * v_err; } // Velocity integral action before limiting Iq += vel_integrator_current_; // Current limiting + // TODO: Change to controller working in torque units + // and get the torque limits from a function of the motor bool limited = false; float Ilim = axis_->motor_.effective_current_lim(); if (Iq > Ilim) { @@ -212,7 +227,7 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s // TODO make decayfactor configurable vel_integrator_current_ *= 0.99f; } else { - vel_integrator_current_ += (config_.vel_integrator_gain * current_meas_period) * v_err; + vel_integrator_current_ += (vel_integrator_gain * current_meas_period) * v_err; } } diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 17306e59..da98359e 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -50,6 +50,7 @@ bool Motor::arm() { void Motor::reset_current_control() { current_control_.v_current_control_integral_d = 0.0f; current_control_.v_current_control_integral_q = 0.0f; + current_control_.acim_rotor_flux = 0.0f; } // @brief Tune the current controller based on phase resistance and inductance @@ -284,7 +285,8 @@ bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) { bool Motor::run_calibration() { float R_calib_max_voltage = config_.resistance_calib_max_voltage; - if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) { + if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT + || config_.motor_type == MOTOR_TYPE_ACIM) { if (!measure_phase_resistance(config_.calibration_current, R_calib_max_voltage)) return false; if (!measure_phase_inductance(-R_calib_max_voltage, R_calib_max_voltage)) @@ -444,17 +446,50 @@ bool Motor::update(float current_setpoint, float phase, float phase_vel) { phase *= config_.direction; phase_vel *= config_.direction; + // TODO: 2-norm vs independent clamping (current could be sqrt(2) bigger) + float ilim = effective_current_lim(); + // TODO: use std::clamp (C++17) + float id = MACRO_MIN(MACRO_MAX(current_control_.Id_setpoint, -ilim), ilim); + float iq = MACRO_MIN(MACRO_MAX(current_setpoint, -ilim), ilim); + + if (config_.motor_type == MOTOR_TYPE_ACIM) { + // Note that the effect of the current commands on the real currents is actually 1.5 PWM cycles later + // However the rotor time constant is (usually) so slow that it doesn't matter + // So we elect to write it as if the effect is immediate, to have cleaner code + + // acim_rotor_flux is normalized to units of [A] tracking Id; rotor inductance is unspecified + float dflux_by_dt = config_.acim_slip_velocity * (id - current_control_.acim_rotor_flux); + current_control_.acim_rotor_flux += dflux_by_dt * current_meas_period; + float slip_velocity = config_.acim_slip_velocity * (iq / current_control_.acim_rotor_flux); + // Check for issues with small denominator. Polarity of check to catch NaN too + bool acceptable_vel = fabsf(slip_velocity) <= 0.1f * (float)current_meas_hz; + if (!acceptable_vel) + slip_velocity = 0.0f; + phase_vel += slip_velocity; + // reporting only: + current_control_.async_phase_vel = slip_velocity; + + current_control_.async_phase_offset += slip_velocity * current_meas_period; + current_control_.async_phase_offset = wrap_pm_pi(current_control_.async_phase_offset); + phase += current_control_.async_phase_offset; + phase = wrap_pm_pi(phase); + } + float pwm_phase = phase + 1.5f * current_meas_period * phase_vel; // Execute current command // TODO: move this into the mot if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) { - if(!FOC_current(0.0f, current_setpoint, phase, pwm_phase)){ + if(!FOC_current(id, iq, phase, pwm_phase)){ + return false; + } + } else if (config_.motor_type == MOTOR_TYPE_ACIM) { + if(!FOC_current(id, iq, phase, pwm_phase)){ return false; } } else if (config_.motor_type == MOTOR_TYPE_GIMBAL) { //In gimbal motor mode, current is reinterptreted as voltage. - if(!FOC_voltage(0.0f, current_setpoint, pwm_phase)) + if(!FOC_voltage(id, iq, pwm_phase)) return false; } else { set_error(ERROR_NOT_IMPLEMENTED_MOTOR_TYPE); diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index f0783584..168b29ba 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -29,7 +29,8 @@ public: enum MotorType_t { MOTOR_TYPE_HIGH_CURRENT = 0, // MOTOR_TYPE_LOW_CURRENT = 1, //Not yet implemented - MOTOR_TYPE_GIMBAL = 2 + MOTOR_TYPE_GIMBAL = 2, + MOTOR_TYPE_ACIM = 3, }; struct Iph_BC_t { @@ -46,12 +47,16 @@ public: // Voltage applied at end of cycle: float final_v_alpha; // [V] float final_v_beta; // [V] + float Id_setpoint; // [A] float Iq_setpoint; // [A] float Iq_measured; // [A] float Id_measured; // [A] float I_measured_report_filter_k; float max_allowed_current; // [A] float overcurrent_trip_level; // [A] + float acim_rotor_flux; // [A] + float async_phase_vel; // [rad/s electrical] + float async_phase_offset; // [rad electrical] }; // NOTE: for gimbal motors, all units of A are instead V. @@ -75,6 +80,8 @@ public: float current_control_bandwidth = 1000.0f; // [rad/s] float inverter_temp_limit_lower = 100; float inverter_temp_limit_upper = 120; + float acim_slip_velocity = 14.706f; // [rad/s electrical] = 1/rotor_tau + float acim_gain_min_flux = 10; // [A] }; enum TimingLog_t { @@ -162,12 +169,16 @@ public: .Ibus = 0.0f, .final_v_alpha = 0.0f, .final_v_beta = 0.0f, + .Id_setpoint = 0.0f, .Iq_setpoint = 0.0f, .Iq_measured = 0.0f, .Id_measured = 0.0f, .I_measured_report_filter_k = 1.0f, .max_allowed_current = 0.0f, .overcurrent_trip_level = 0.0f, + .acim_rotor_flux = 0.0f, + .async_phase_vel = 0.0f, + .async_phase_offset = 0.0f, }; DRV8301_FaultType_e drv_fault_ = DRV8301_FaultType_NoFault; DRV_SPI_8301_Vars_t gate_driver_regs_; //Local view of DRV registers (initialized by DRV8301_setup) @@ -194,12 +205,16 @@ public: make_protocol_property("Ibus", ¤t_control_.Ibus), make_protocol_property("final_v_alpha", ¤t_control_.final_v_alpha), make_protocol_property("final_v_beta", ¤t_control_.final_v_beta), - make_protocol_property("Iq_setpoint", ¤t_control_.Iq_setpoint), + make_protocol_property("Id_setpoint", ¤t_control_.Id_setpoint), + make_protocol_ro_property("Iq_setpoint", ¤t_control_.Iq_setpoint), make_protocol_property("Iq_measured", ¤t_control_.Iq_measured), make_protocol_property("Id_measured", ¤t_control_.Id_measured), make_protocol_property("I_measured_report_filter_k", ¤t_control_.I_measured_report_filter_k), make_protocol_ro_property("max_allowed_current", ¤t_control_.max_allowed_current), - make_protocol_ro_property("overcurrent_trip_level", ¤t_control_.overcurrent_trip_level) + make_protocol_ro_property("overcurrent_trip_level", ¤t_control_.overcurrent_trip_level), + make_protocol_property("acim_rotor_flux", ¤t_control_.acim_rotor_flux), + make_protocol_ro_property("async_phase_vel", ¤t_control_.async_phase_vel), + make_protocol_property("async_phase_offset", ¤t_control_.async_phase_offset) ), make_protocol_object("gate_driver", make_protocol_ro_property("drv_fault", &drv_fault_) @@ -234,7 +249,9 @@ public: make_protocol_property("inverter_temp_limit_upper", &config_.inverter_temp_limit_upper), make_protocol_property("requested_current_range", &config_.requested_current_range), make_protocol_property("current_control_bandwidth", &config_.current_control_bandwidth, - [](void* ctx) { static_cast(ctx)->update_current_controller_gains(); }, this) + [](void* ctx) { static_cast(ctx)->update_current_controller_gains(); }, this), + make_protocol_property("acim_slip_velocity", &config_.acim_slip_velocity), + make_protocol_property("acim_gain_min_flux", &config_.acim_gain_min_flux) ) ); } diff --git a/Firmware/fibre/python/fibre/usbbulk_transport.py b/Firmware/fibre/python/fibre/usbbulk_transport.py index dd32b106..f8a8905a 100644 --- a/Firmware/fibre/python/fibre/usbbulk_transport.py +++ b/Firmware/fibre/python/fibre/usbbulk_transport.py @@ -187,7 +187,7 @@ def discover_channels(path, serial_number, callback, cancellation_token, channel return True while not cancellation_token.is_set(): - logger.debug("USB discover loop") + # logger.debug("USB discover loop") devices = usb.core.find(find_all=True, custom_match=device_matcher) for usb_device in devices: try: diff --git a/ODrive_Workspace.code-workspace b/ODrive_Workspace.code-workspace index 89da475f..2928216c 100644 --- a/ODrive_Workspace.code-workspace +++ b/ODrive_Workspace.code-workspace @@ -25,6 +25,7 @@ "c-cpp-flylint.cppcheck.standard": ["c99","c++14"], "files.associations": { + "*.config": "yaml", "memory": "cpp", "utility": "cpp", "deque": "cpp", @@ -58,7 +59,8 @@ "chrono": "cpp", "condition_variable": "cpp", "future": "cpp", - "arm_math.h": "c" + "arm_math.h": "c", + "cmath": "cpp" } } } diff --git a/analysis/motor_analysis/ac_induction_motor.py b/analysis/motor_analysis/ac_induction_motor.py index e467d365..9cf15228 100644 --- a/analysis/motor_analysis/ac_induction_motor.py +++ b/analysis/motor_analysis/ac_induction_motor.py @@ -148,8 +148,8 @@ def plot_data(t, y, ref, title): ax1.plot(t, np.imag(y[0]), label='Stator current (q)') ax2.plot(t, np.real(y[2]), label='Rotor current (d)') ax2.plot(t, np.imag(y[2]), label='Rotor current (q)') - ax1b.plot(t, 1000*np.real(y[1]), 'C2', label='Rotor flux (d)') - ax1b.plot(t, 1000*np.imag(y[1]), 'C3', label='Rotor flux (q)') + ax1b.plot(t, 1000*np.real(y[1]), 'C3', label='Rotor flux (d)') + ax1b.plot(t, 1000*np.imag(y[1]), 'C4', label='Rotor flux (q)') ax1.set_xlabel('time [s]') ax1.set_ylabel('Current [A]') ax1b.set_ylabel('Flux [mWb]')