diff --git a/CHANGELOG.md b/CHANGELOG.md index eb0d96f6..bf7ea37f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,6 +2,9 @@ Please add a note of your changes below this heading if you make a Pull Request. ### Added +* AC Induction Motor support. + * Tracking of rotor flux through rotor time constant + * Automatic d axis current for Maximum Torque Per Amp (MTPA) * Simplified control interface ("Input Filter" branch) * New input variables: `input_pos`, `input_vel`, and `input_current` * New setting `input_mode` to switch between different input behaviours @@ -22,11 +25,12 @@ Please add a note of your changes below this heading if you make a Pull Request. * Using an STM32F405 .svd file allows CortexDebug to view registers during debugging ### Changed +* Changed ratiometric `motor.config.current_lim_tolerance` to absolute `motor.config.current_lim_margin` +* Moved `controller.vel_ramp_enable` into `controller.config`. * Anticogging map is temporarily forced to 0.1 deg precision, but saves with the config * Some Encoder settings have been made read-only * Cleaned up VSCode C/C++ Configuration settings on Windows with recursive includePath * Now compiling with C++17 - # Releases ## [0.4.11] - 2019-07-25 ### Added diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 107fa5ed..b0ddab90 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -1,5 +1,6 @@ #include "odrive_main.h" +#include #include @@ -271,6 +272,21 @@ bool Controller::update(float* current_setpoint_output) { } } + // 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; + // TODO: also scale the integral value which is also changing units. + // (or again just do control in torque units) + } + // Velocity control float Iq = current_setpoint_; @@ -305,6 +321,8 @@ bool Controller::update(float* current_setpoint_output) { } // 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) { diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 07508f60..e69fcffb 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -114,6 +114,7 @@ public: // float vel_setpoint = 800.0f; float vel_integrator_current_ = 0.0f; // [A] float current_setpoint_ = 0.0f; // [A] + float vel_ramp_target_ = 0.0f; float input_pos_ = 0.0f; float input_vel_ = 0.0f; @@ -141,6 +142,7 @@ public: make_protocol_ro_property("current_setpoint", ¤t_setpoint_), make_protocol_ro_property("trajectory_done", &trajectory_done_), make_protocol_property("vel_integrator_current", &vel_integrator_current_), + make_protocol_property("vel_ramp_target", &vel_ramp_target_), make_protocol_property("anticogging_valid", &anticogging_valid_), make_protocol_property("gain_scheduling_width", &config_.gain_scheduling_width), make_protocol_object("config", @@ -155,6 +157,7 @@ public: make_protocol_property("vel_integrator_gain", &config_.vel_integrator_gain), make_protocol_property("vel_limit", &config_.vel_limit), make_protocol_property("vel_limit_tolerance", &config_.vel_limit_tolerance), + make_protocol_property("vel_ramp_enable", &config_.vel_ramp_enable), make_protocol_property("vel_ramp_rate", &config_.vel_ramp_rate), make_protocol_property("current_ramp_rate", &config_.current_ramp_rate), make_protocol_property("homing_speed", &config_.homing_speed), diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index c0208db0..4b55bceb 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -3,14 +3,17 @@ Encoder::Encoder(const EncoderHardwareConfig_t& hw_config, - Config_t& config) : + Config_t& config, Motor::Config_t motor_config) : hw_config_(hw_config), config_(config) { update_pll_gains(); - if (config.pre_calibrated && (config.mode == Encoder::MODE_HALL || config.mode == Encoder::MODE_SINCOS)) { - is_ready_ = true; + if (config.pre_calibrated) { + if (config.mode == Encoder::MODE_HALL || config.mode == Encoder::MODE_SINCOS) + is_ready_ = true; + if (motor_config.motor_type == Motor::MOTOR_TYPE_ACIM) + is_ready_ = true; } } @@ -93,7 +96,8 @@ void Encoder::update_pll_gains() { } void Encoder::check_pre_calibrated() { - if (!is_ready_) + // TODO: restoring config from python backup is fragile here (ACIM motor type must be set first) + if (!is_ready_ && axis_->motor_.config_.motor_type != Motor::MOTOR_TYPE_ACIM) config_.pre_calibrated = false; if (mode_ == MODE_INCREMENTAL && !index_found_) config_.pre_calibrated = false; diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index fddf5142..34b85482 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -54,7 +54,7 @@ public: }; Encoder(const EncoderHardwareConfig_t& hw_config, - Config_t& config); + Config_t& config, Motor::Config_t motor_config); void setup(); void set_error(Error_t error); @@ -146,13 +146,13 @@ public: [](void* ctx) { static_cast(ctx)->set_idx_subscribe(); }, this), make_protocol_property("find_idx_on_lockin_only", &config_.find_idx_on_lockin_only, [](void* ctx) { static_cast(ctx)->set_idx_subscribe(); }, this), - make_protocol_property("pre_calibrated", &config_.pre_calibrated, - [](void* ctx) { static_cast(ctx)->check_pre_calibrated(); }, this), make_protocol_property("abs_spi_cs_gpio_pin", &config_.abs_spi_cs_gpio_pin, [](void* ctx) { static_cast(ctx)->abs_spi_cs_pin_init(); }, this), make_protocol_property("zero_count_on_find_idx", &config_.zero_count_on_find_idx), make_protocol_property("cpr", &config_.cpr), make_protocol_property("offset", &config_.offset), + make_protocol_property("pre_calibrated", &config_.pre_calibrated, + [](void* ctx) { static_cast(ctx)->check_pre_calibrated(); }, this), make_protocol_property("offset_float", &config_.offset_float), make_protocol_property("enable_phase_interpolation", &config_.enable_phase_interpolation), make_protocol_property("bandwidth", &config_.bandwidth, diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index 8b4a16f5..a3c341f5 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -426,11 +426,6 @@ void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { // Only one conversion in sequence, so only rank1 uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1); vbus_voltage = ADCValue * voltage_scale; - if (axes[0] && !axes[0]->error_ && axes[1] && !axes[1]->error_) { - if (oscilloscope_pos >= OSCILLOSCOPE_SIZE) - oscilloscope_pos = 0; - oscilloscope[oscilloscope_pos++] = vbus_voltage; - } } static void decode_hall_samples(Encoder& enc, uint16_t GPIO_samples[num_GPIO]) { diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 3cc077ea..57a0fef0 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -161,7 +161,7 @@ extern "C" int construct_objects(){ odCAN = new ODriveCAN(&hcan1, can_config); for (size_t i = 0; i < AXIS_COUNT; ++i) { Encoder *encoder = new Encoder(hw_configs[i].encoder_config, - encoder_configs[i]); + encoder_configs[i], motor_configs[i]); SensorlessEstimator *sensorless_estimator = new SensorlessEstimator(sensorless_configs[i]); Controller *controller = new Controller(controller_configs[i]); Motor *motor = new Motor(hw_configs[i].motor_config, diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index dd9c47ae..38b3155b 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)) @@ -357,7 +359,7 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_pha ictrl.Id_measured += ictrl.I_measured_report_filter_k * (Id - ictrl.Id_measured); // Check for violation of current limit - float I_trip = config_.current_lim_tolerance * effective_current_lim(); + float I_trip = effective_current_lim() + config_.current_lim_margin; if (SQ(Id) + SQ(Iq) > SQ(I_trip)) { set_error(ERROR_CURRENT_LIMIT_VIOLATION); return false; @@ -409,6 +411,31 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_pha return false; // error set inside enqueue_modulation_timings log_timing(TIMING_LOG_FOC_CURRENT); + if (axis_->axis_num_ == 0) { + + // Edit these to suit your capture needs + float trigger_data = ictrl.v_current_control_integral_d; + float trigger_threshold = 0.5f; + float sample_data = Ialpha; + + static bool ready = false; + static bool capturing = false; + if (trigger_data < trigger_threshold) { + ready = true; + } + if (ready && trigger_data >= trigger_threshold) { + capturing = true; + ready = false; + } + if (capturing) { + oscilloscope[oscilloscope_pos] = sample_data; + if (++oscilloscope_pos >= OSCILLOSCOPE_SIZE) { + oscilloscope_pos = 0; + capturing = false; + } + } + } + return true; } @@ -418,17 +445,58 @@ 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 + + if (config_.acim_autoflux_enable) { + float abs_iq = fabsf(iq); + float gain = abs_iq > id ? config_.acim_autoflux_attack_gain : config_.acim_autoflux_decay_gain; + id += gain * (abs_iq - id) * current_meas_period; + id = MACRO_MIN(MACRO_MAX(id, config_.acim_autoflux_min_Id), ilim); + current_control_.Id_setpoint = id; + } + + // 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 6e24d895..40fafce3 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. @@ -69,12 +74,18 @@ public: // 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] - float current_lim_tolerance = 1.25f; // multiple of current_lim + float current_lim_margin = 8.0f; // Maximum violation of current_lim // Value used to compute shunt amplifier gains float requested_current_range = 60.0f; // [A] 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] + float acim_autoflux_min_Id = 10; // [A] + bool acim_autoflux_enable = false; + float acim_autoflux_attack_gain = 10.0f; + float acim_autoflux_decay_gain = 1.0f; }; enum TimingLog_t { @@ -162,12 +173,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 +209,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_) @@ -229,12 +248,18 @@ public: make_protocol_property("direction", &config_.direction), make_protocol_property("motor_type", &config_.motor_type), make_protocol_property("current_lim", &config_.current_lim), - make_protocol_property("current_lim_tolerance", &config_.current_lim_tolerance), + make_protocol_property("current_lim_margin", &config_.current_lim_margin), make_protocol_property("inverter_temp_limit_lower", &config_.inverter_temp_limit_lower), 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), + make_protocol_property("acim_autoflux_min_Id", &config_.acim_autoflux_min_Id), + make_protocol_property("acim_autoflux_enable", &config_.acim_autoflux_enable), + make_protocol_property("acim_autoflux_attack_gain", &config_.acim_autoflux_attack_gain), + make_protocol_property("acim_autoflux_decay_gain", &config_.acim_autoflux_decay_gain) ) ); } diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index 5e1caf37..dec12035 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -110,7 +110,7 @@ extern Axis *axes[AXIS_COUNT]; extern ODriveCAN *odCAN; // if you use the oscilloscope feature you can bump up this value -#define OSCILLOSCOPE_SIZE 128 +#define OSCILLOSCOPE_SIZE 4096 extern float oscilloscope[OSCILLOSCOPE_SIZE]; extern size_t oscilloscope_pos; @@ -129,10 +129,10 @@ inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast(~static_c // ODrive specific includes #include #include +#include #include #include #include -#include #include #include #include diff --git a/Firmware/fibre/python/fibre/discovery.py b/Firmware/fibre/python/fibre/discovery.py index a3751633..6039d7e0 100644 --- a/Firmware/fibre/python/fibre/discovery.py +++ b/Firmware/fibre/python/fibre/discovery.py @@ -112,18 +112,30 @@ def find_all(path, serial_number, def find_any(path="usb", serial_number=None, search_cancellation_token=None, channel_termination_token=None, - timeout=None, logger=Logger(verbose=False)): + timeout=None, logger=Logger(verbose=False), find_multiple=False): """ Blocks until the first matching Fibre node is connected and then returns that node """ - result = [ None ] + result = [] done_signal = Event(search_cancellation_token) def did_discover_object(obj): - result[0] = obj - done_signal.set() + result.append(obj) + if find_multiple: + if len(result) >= int(find_multiple): + done_signal.set() + else: + done_signal.set() + find_all(path, serial_number, did_discover_object, done_signal, channel_termination_token, logger) try: done_signal.wait(timeout=timeout) + except TimeoutError: + if not find_multiple: + return None finally: done_signal.set() # terminate find_all - return result[0] + + if find_multiple: + return result + else: + return result[0] 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 6e9385a0..cf197f8d 100644 --- a/ODrive_Workspace.code-workspace +++ b/ODrive_Workspace.code-workspace @@ -8,11 +8,15 @@ }, { "path": "docs" + }, + { + "path": "analysis" } ], "settings": { "c-cpp-flylint.cppcheck.standard": ["c99","c++14"], "files.associations": { + "*.config": "yaml", "memory": "cpp", "utility": "cpp", "deque": "cpp", diff --git a/analysis/motor_analysis/ac_induction_motor.py b/analysis/motor_analysis/ac_induction_motor.py new file mode 100644 index 00000000..ccc06ec8 --- /dev/null +++ b/analysis/motor_analysis/ac_induction_motor.py @@ -0,0 +1,245 @@ + +import numpy as np +import matplotlib.pyplot as plt +from scipy.integrate import solve_ivp +from scipy.optimize import least_squares +from engineering_notation import EngNumber + +filename = "oscilloscope.csv" + +PLOT_INITAL = True +DO_FITTING = False +PLOT_PROGRESS = False +REPORT_PROGRESS = True +assumed_rotor_resistance = 1 +pole_pairs = 2 + +class ACMotor(): + """ + Models an induction motor based on Eq 10 in [1]. + [1] https://pdfs.semanticscholar.org/4770/15e472da4c2e05e9ff8c1b921c76a938f786.pdf + + + Note: This model refers all rotor quantities to the stator, i.e. the + quantities are as if the motor had a winding ratio of k = 1. + """ + + # parameters: (name, range) + parameter_definitions = [ + ('stator_inductance', (0, np.inf), 'H'), # aka l_s, [Henry] + ('stator_resistance', (0, np.inf), 'ohm'), # aka r_s, [Ohm] + ('rotor_inductance', (0, np.inf), 'H'), # aka l_r [Henry] + # ('rotor_resistance', (0, np.inf), 'ohm'), # aka r_r [Ohm] + ('mutual_inductance_factor', (0, 1.0), ''), #[unitless] = l_m**2 / (l_s * l_r) + ] + # parameter index lookup + pl = {r[0]:i for i, r in enumerate(parameter_definitions)} + + # states: (name, initial_value) + state_definitions = [ + ('stator_current', 0.0), # aka i_s, [A] + ('rotor_flux', 0.0), # aka Phi_r, [Wb] + ] # complex numbers + # state index lookup + sl = {r[0]:i for i, r in enumerate(state_definitions)} + + def __init__(self, params): + self.params = params + + # Assigned in run(): + # self.stator_voltage = None + # self.omega_stator = None + # self.omega_rotor = None + + def get_mutual_inductance(self): + return np.sqrt( + self.params[ACMotor.pl['mutual_inductance_factor']] + * self.params[ACMotor.pl['stator_inductance']] + * self.params[ACMotor.pl['rotor_inductance']] + ) + + def system_function(self, t, y): + # local shorthand for params + p = self.params + pl = ACMotor.pl + sl = ACMotor.sl + + # rotor_resistance = p[pl['rotor_resistance']] + rotor_resistance = assumed_rotor_resistance + mutual_inductance = self.get_mutual_inductance() + + tau_rotor = p[pl['rotor_inductance']] / rotor_resistance # [s] + coupling_factor = mutual_inductance / p[pl['rotor_inductance']] # aka k_r [unitless] + r_sigma = p[pl['stator_resistance']] + coupling_factor**2 * rotor_resistance # [Ohm] + leakage_factor = 1.0 - mutual_inductance**2 / (p[pl['rotor_inductance']] * p[pl['stator_inductance']]) # aka sigma [unitless] + tau_stator_prime = leakage_factor * p[pl['stator_inductance']] / r_sigma # [s] + + # [1] Eq 10a + dstator_current_dt = ( + -1.0j * self.omega_stator * tau_stator_prime * y[sl['stator_current']] + - coupling_factor / (r_sigma * tau_rotor) * (1.0j*self.omega_rotor * tau_rotor - 1.0) * y[sl['rotor_flux']] + + 1.0 / r_sigma * self.stator_voltage + - y[sl['stator_current']] + ) / tau_stator_prime + + # [1] Eq 10b + drotor_flux_dt = ( + -1.0j * (self.omega_stator - self.omega_rotor) * tau_rotor * y[sl['rotor_flux']] + + mutual_inductance * y[sl['stator_current']] + - y[sl['rotor_flux']] + ) / tau_rotor + + return [dstator_current_dt, drotor_flux_dt] + + def run(self, time_series, voltage, omega_stator, omega_rotor): + self.stator_voltage = voltage + self.omega_stator = omega_stator + self.omega_rotor = omega_rotor + + y0 = np.array([x[1] for x in ACMotor.state_definitions], dtype=np.complex) + + result = solve_ivp(self.system_function, (time_series[0], time_series[-1]), y0, t_eval=time_series) + y = result.y + + # compute derived state + rotor_inductance = self.params[ACMotor.pl['rotor_inductance']] + rotor_current = (1/rotor_inductance) * (y[1] - self.get_mutual_inductance() * y[0]) + return np.vstack((y, rotor_current)) + + def print_parameter_info(self): + print() + print('Given parameters:') + print('rotor_resistance = {}ohm'.format(EngNumber(assumed_rotor_resistance))) + + print() + print('Fitted parameters:') + for i, r in enumerate(ACMotor.parameter_definitions): + print('{} = {}{}'.format(r[0], EngNumber(self.params[i]), r[2])) + + print() + print('Derived parameters:') + mutual_inductance = motor.get_mutual_inductance() + coupling_factor = mutual_inductance / self.params[ACMotor.pl['rotor_inductance']] + torque_constant = pole_pairs * coupling_factor * mutual_inductance + motor_constant = torque_constant / (3.0 * self.params[ACMotor.pl['stator_resistance']]) + print('mutual_inductance = {}H'.format(EngNumber(mutual_inductance))) + print('coupling_factor = {}'.format(EngNumber(coupling_factor))) + print('torque_constant = {}Nm/A^2'.format(EngNumber(torque_constant))) + print('motor_constant = {}Nm/W'.format(EngNumber(motor_constant))) + + def print_run_info(self, y): + final_stator_current_d = np.real(y[0,-1]) + final_stator_current_q = np.imag(y[0,-1]) + final_rotor_flux_d = np.real(y[1,-1]) + + mutual_inductance = self.get_mutual_inductance() + coupling_factor = mutual_inductance / self.params[ACMotor.pl['rotor_inductance']] + final_torque_per_q_amp = pole_pairs * coupling_factor * final_rotor_flux_d + + print() + print('Final values:') + print('final_rotor_flux_d = {}Wb'.format(EngNumber(final_rotor_flux_d))) + print('final_stator_current_d = {}A'.format(EngNumber(final_stator_current_d))) + print('final_stator_current_q = {}A'.format(EngNumber(final_stator_current_q))) + print('final_torque_per_q_amp = {}Nm/A'.format(EngNumber(final_torque_per_q_amp))) + +def plot_data(t, y, ref, title): + fig, (ax1, ax2) = plt.subplots(2, sharex=True) + ax1b = ax1.twinx() + ax1.plot(t, ref, label='Measured current') + ax1.plot(t, np.real(y[0]), label='Stator current (d)') + 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]), '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]') + ax2.set_ylabel('Current [A]') + plt.title(title) + fig.legend() + plt.show() + +# load test data +t = np.arange(4096)/8000.0 +voltage_step = 1.0 +with open(filename, 'r') as fp: + test_response = np.array([float(x) for x in fp.readlines()]) + + +inital_parameters = np.zeros(len(ACMotor.parameter_definitions)) +inital_parameters[ACMotor.pl['stator_inductance']] = 7.72181086e-04 +inital_parameters[ACMotor.pl['stator_resistance']] = 3.06884624e-02 +inital_parameters[ACMotor.pl['rotor_inductance']] = assumed_rotor_resistance*6.82013522e-02 +# inital_parameters[ACMotor.pl['rotor_resistance']] = 1.0e-0 +# inital_parameters[ACMotor.pl['mutual_inductance']] = 2.40e-4 +inital_parameters[ACMotor.pl['mutual_inductance_factor']] = 8.68671978e-01 + +# inital_parameters[ACMotor.pl['stator_resistance']] = 1.298 +# inital_parameters[ACMotor.pl['stator_inductance']] = 0.157228647 +# inital_parameters[ACMotor.pl['rotor_resistance']] = 0.975052932 +# inital_parameters[ACMotor.pl['rotor_inductance']] = 0.16674423623999998 +# inital_parameters[ACMotor.pl['mutual_inductance']] = 0.157221177 + +# Plot initial run +if PLOT_INITAL: + print() + print('Initial run:') + motor = ACMotor(inital_parameters) + motor.print_parameter_info() + + y = motor.run( + time_series = t, + voltage = voltage_step, + omega_stator = 0, + omega_rotor= 0) + motor.print_run_info(y) + plot_data(t, y, test_response, 'initial') + + +# Fit to data +def get_residuals(params): + if REPORT_PROGRESS: print(params) + motor = ACMotor(params) + y = motor.run( + time_series = t, + voltage = voltage_step, + omega_stator = 0, + omega_rotor= 0) + + residuals = test_response - np.real(y[0]) + fitness = sum(residuals**2) + if REPORT_PROGRESS: print(fitness) + + if PLOT_PROGRESS: + plot_data(t, y, test_response, 'progress') + + return residuals + +if DO_FITTING: + print() + print('Fitting parameters:') + optiresult = least_squares(get_residuals, inital_parameters, + bounds=list(zip(*[x[1] for x in ACMotor.parameter_definitions])), + x_scale='jac', + diff_step = 1e-2 * np.array([ + 7.58192590e-04, + 3.07166671e-02, + 6.85207075e-02, + # 4.66461518e+00, + 8.67540012e-01]) + ) + print(optiresult.message) + + motor = ACMotor(optiresult.x) + motor.print_parameter_info() + + y = motor.run( + time_series = t, + voltage = voltage_step, + omega_stator = 0, + omega_rotor= 0) + motor.print_run_info(y) + plot_data(t, y, test_response, 'final') + diff --git a/tools/odrive/shell.py b/tools/odrive/shell.py index c25278e3..1f83aa9c 100644 --- a/tools/odrive/shell.py +++ b/tools/odrive/shell.py @@ -5,10 +5,17 @@ import threading import fibre import odrive import odrive.enums -from odrive.utils import start_liveplotter, dump_errors +from odrive.utils import start_liveplotter, dump_errors, oscilloscope_dump #from odrive.enums import * # pylint: disable=W0614 def print_banner(): + print("Website: https://odriverobotics.com/") + print("Docs: https://docs.odriverobotics.com/") + print("Forums: https://discourse.odriverobotics.com/") + print("Discord: https://discord.gg/k3ZZ3mS") + print("Github: https://github.com/madcowswe/ODrive/") + + print() print('Please connect your ODrive.') print('You can also type help() or quit().') @@ -77,7 +84,8 @@ def launch_shell(args, logger, app_shutdown_token): interactive_variables = { 'start_liveplotter': start_liveplotter, - 'dump_errors': dump_errors + 'dump_errors': dump_errors, + 'oscilloscope_dump': oscilloscope_dump } # Expose all enums from odrive.enums diff --git a/tools/odrive/utils.py b/tools/odrive/utils.py index 2b2901a7..c1bb36a8 100755 --- a/tools/odrive/utils.py +++ b/tools/odrive/utils.py @@ -64,6 +64,12 @@ def dump_errors(odrv, clear=False): else: print(prefix + _VT100Colors['green'] + "no error" + _VT100Colors['default']) +def oscilloscope_dump(odrv, num_vals, filename='oscilloscope.csv'): + with open(filename, 'w') as f: + for x in range(num_vals): + f.write(str(odrv.get_oscilloscope_val(x))) + f.write('\n') + data_rate = 10 plot_rate = 10 num_samples = 1000 diff --git a/tools/plot_oscilloscope.py b/tools/plot_oscilloscope.py new file mode 100644 index 00000000..2a1809b8 --- /dev/null +++ b/tools/plot_oscilloscope.py @@ -0,0 +1,9 @@ + +from matplotlib import pyplot as plt +import sys + +with open(sys.argv[1]) as f: + data = list(map(float, f)) + +plt.plot(data) +plt.show() \ No newline at end of file