Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-02-06 15:11:52 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

finish implementing basic ACIM control

Browse Source
This commit is contained in:
Oskar Weigl
2019-09-22 23:27:40 -07:00
parent 66cb273540
commit c8c6f6237c
6 changed files with 83 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
Firmware/MotorControl/controller.cpp
View File

@@ -1,6 +1,6 @@
#include "odrive_main.h"
#include <algorithm>
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;
}
}

41
Firmware/MotorControl/motor.cpp
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))
@@ -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);

25
Firmware/MotorControl/motor.hpp
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.
@@ -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", &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_)
@@ -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<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)
)
);
}

2
Firmware/fibre/python/fibre/usbbulk_transport.py
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:

4
ODrive_Workspace.code-workspace
View File

@@ -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"
}
}
}

4
analysis/motor_analysis/ac_induction_motor.py
View File

@@ -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]')