Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-22 16:14:37 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge branch 'devel' into RazorsEdge

Browse Source
This commit is contained in:
Unknown
2020-01-01 21:45:41 -05:00
parent 62654ed966 50d06576b7
commit 7a756c1761
17 changed files with 435 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CHANGELOG.md
+5 -1
View File
@@ -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
Firmware/MotorControl/controller.cpp
+18
View File
@@ -1,5 +1,6 @@
#include "odrive_main.h"
#include <algorithm>
#include <algorithm>
@@ -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) {
Firmware/MotorControl/controller.hpp
+3
View File
@@ -114,6 +114,7 @@ public:
// float vel_setpoint = 800.0f; <sensorless example>
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", &current_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),
Firmware/MotorControl/encoder.cpp
+8 -4
View File
@@ -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;
Firmware/MotorControl/encoder.hpp
+3 -3
View File
@@ -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<Encoder*>(ctx)->set_idx_subscribe(); }, this),
make_protocol_property("find_idx_on_lockin_only", &config_.find_idx_on_lockin_only,
[](void* ctx) { static_cast<Encoder*>(ctx)->set_idx_subscribe(); }, this),
make_protocol_property("pre_calibrated", &config_.pre_calibrated,
[](void* ctx) { static_cast<Encoder*>(ctx)->check_pre_calibrated(); }, this),
make_protocol_property("abs_spi_cs_gpio_pin", &config_.abs_spi_cs_gpio_pin,
[](void* ctx) { static_cast<Encoder*>(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<Encoder*>(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,
Firmware/MotorControl/low_level.cpp
-5
View File
@@ -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]) {
Firmware/MotorControl/main.cpp
+1 -1
View File
@@ -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,
Firmware/MotorControl/motor.cpp
+72 -4
View File
@@ -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);
Firmware/MotorControl/motor.hpp
+31 -6
View File
@@ -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", &current_control_.Ibus),
make_protocol_property("final_v_alpha", &current_control_.final_v_alpha),
make_protocol_property("final_v_beta", &current_control_.final_v_beta),
make_protocol_property("Iq_setpoint", &current_control_.Iq_setpoint),
make_protocol_property("Id_setpoint", &current_control_.Id_setpoint),
make_protocol_ro_property("Iq_setpoint", &current_control_.Iq_setpoint),
make_protocol_property("Iq_measured", &current_control_.Iq_measured),
make_protocol_property("Id_measured", &current_control_.Id_measured),
make_protocol_property("I_measured_report_filter_k", &current_control_.I_measured_report_filter_k),
make_protocol_ro_property("max_allowed_current", &current_control_.max_allowed_current),
make_protocol_ro_property("overcurrent_trip_level", &current_control_.overcurrent_trip_level)
make_protocol_ro_property("overcurrent_trip_level", &current_control_.overcurrent_trip_level),
make_protocol_property("acim_rotor_flux", &current_control_.acim_rotor_flux),
make_protocol_ro_property("async_phase_vel", &current_control_.async_phase_vel),
make_protocol_property("async_phase_offset", &current_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<Motor*>(ctx)->update_current_controller_gains(); }, this)
[](void* ctx) { static_cast<Motor*>(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)
)
);
}
Firmware/MotorControl/odrive_main.h
+2 -2
View File
@@ -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<ENUMTYPE>(~static_c
// ODrive specific includes
#include <utils.h>
#include <low_level.h>
#include <motor.hpp>
#include <encoder.hpp>
#include <sensorless_estimator.hpp>
#include <controller.hpp>
#include <motor.hpp>
#include <trapTraj.hpp>
#include <endstop.hpp>
#include <axis.hpp>
Firmware/fibre/python/fibre/discovery.py
+17 -5
View File
@@ -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]
Firmware/fibre/python/fibre/usbbulk_transport.py
+1 -1
View File
@@ -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:
ODrive_Workspace.code-workspace
+4
View File
@@ -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",
analysis/motor_analysis/ac_induction_motor.py
+245
View File
@@ -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')
tools/odrive/shell.py
+10 -2
View File
@@ -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
tools/odrive/utils.py
+6
View File
@@ -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
tools/plot_oscilloscope.py
+9
View File
@@ -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()