From e2527ec468e87cb5ca106667509bb52e30ae98b9 Mon Sep 17 00:00:00 2001 From: pjohnson Date: Tue, 16 Jun 2020 15:55:18 -0400 Subject: [PATCH 01/13] added torque_constant to motor config struct and interface file changed all instances of current_setpoint to torque_setpoint modified limitVel() to use torque instead of current units removed effective_current_lim, added effective_torque_lim() added torque_lim to motor config struct and interface file --- Firmware/MotorControl/axis.cpp | 24 +++++++------- Firmware/MotorControl/controller.cpp | 49 ++++++++++++++-------------- Firmware/MotorControl/controller.hpp | 5 +-- Firmware/MotorControl/motor.cpp | 19 ++++++----- Firmware/MotorControl/motor.hpp | 8 ++++- Firmware/odrive-interface.yaml | 7 +++- 6 files changed, 63 insertions(+), 49 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index be15ae3e..2882ca2a 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -286,10 +286,10 @@ bool Axis::run_sensorless_control_loop() { run_control_loop([this](){ // Note that all estimators are updated in the loop prefix in run_control_loop - float current_setpoint; - if (!controller_.update(¤t_setpoint)) + float torque_setpoint; + if (!controller_.update(&torque_setpoint)) return error_ |= ERROR_CONTROLLER_FAILED, false; - if (!motor_.update(current_setpoint, sensorless_estimator_.phase_, sensorless_estimator_.vel_estimate_)) + if (!motor_.update(torque_setpoint, sensorless_estimator_.phase_, sensorless_estimator_.vel_estimate_)) return false; // set_error should update axis.error_ return true; }); @@ -311,12 +311,12 @@ bool Axis::run_closed_loop_control_loop() { set_step_dir_active(config_.enable_step_dir); run_control_loop([this](){ // Note that all estimators are updated in the loop prefix in run_control_loop - float current_setpoint; - if (!controller_.update(¤t_setpoint)) + float torque_setpoint; + if (!controller_.update(&torque_setpoint)) return error_ |= ERROR_CONTROLLER_FAILED, false; float phase_vel = 2 * M_PI * encoder_.vel_estimate_ / (float)encoder_.config_.cpr * motor_.config_.pole_pairs; - if (!motor_.update(current_setpoint, encoder_.phase_, phase_vel)) + if (!motor_.update(torque_setpoint, encoder_.phase_, phase_vel)) return false; // set_error should update axis.error_ return true; @@ -360,12 +360,12 @@ bool Axis::run_homing() { run_control_loop([this](){ // Note that all estimators are updated in the loop prefix in run_control_loop - float current_setpoint; - if (!controller_.update(¤t_setpoint)) + float torque_setpoint; + if (!controller_.update(&torque_setpoint)) return error_ |= ERROR_CONTROLLER_FAILED, false; float phase_vel = 2 * M_PI * encoder_.vel_estimate_ / (float)encoder_.config_.cpr * motor_.config_.pole_pairs; - if (!motor_.update(current_setpoint, encoder_.phase_, phase_vel)) + if (!motor_.update(torque_setpoint, encoder_.phase_, phase_vel)) return false; // set_error should update axis.error_ return !min_endstop_.get_state(); @@ -389,12 +389,12 @@ bool Axis::run_homing() { run_control_loop([this](){ // Note that all estimators are updated in the loop prefix in run_control_loop - float current_setpoint; - if (!controller_.update(¤t_setpoint)) + float torque_setpoint; + if (!controller_.update(&torque_setpoint)) return error_ |= ERROR_CONTROLLER_FAILED, false; float phase_vel = 2 * M_PI * encoder_.vel_estimate_ / (float)encoder_.config_.cpr * motor_.config_.pole_pairs; - if (!motor_.update(current_setpoint, encoder_.phase_, phase_vel)) + if (!motor_.update(torque_setpoint, encoder_.phase_, phase_vel)) return false; // set_error should update axis.error_ return !controller_.trajectory_done_; diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 6f918c8b..2138af3c 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -14,7 +14,7 @@ void Controller::reset() { pos_setpoint_ = 0.0f; vel_setpoint_ = 0.0f; vel_integrator_current_ = 0.0f; - current_setpoint_ = 0.0f; + torque_setpoint_ = 0.0f; } void Controller::set_error(Error error) { @@ -115,13 +115,13 @@ void Controller::update_filter_gains() { input_filter_kp_ = 0.25f * (input_filter_ki_ * input_filter_ki_); // Critically damped } -static float limitVel(const float vel_limit, const float vel_estimate, const float vel_gain, const float Iq) { - float Imax = (vel_limit - vel_estimate) * vel_gain; - float Imin = (-vel_limit - vel_estimate) * vel_gain; - return std::clamp(Iq, Imin, Imax); +static float limitVel(const float vel_limit, const float vel_estimate, const float vel_gain, const float torque) { + float Tmax = (vel_limit - vel_estimate) * vel_gain; + float Tmin = (-vel_limit - vel_estimate) * vel_gain; + return std::clamp(torque, Tmin, Tmax); } -bool Controller::update(float* current_setpoint_output) { +bool Controller::update(float* torque_setpoint_output) { float* pos_estimate_src = (pos_estimate_valid_src_ && *pos_estimate_valid_src_) ? pos_estimate_src_ : nullptr; float* vel_estimate_src = (vel_estimate_valid_src_ && *vel_estimate_valid_src_) @@ -153,7 +153,7 @@ bool Controller::update(float* current_setpoint_output) { case INPUT_MODE_PASSTHROUGH: { pos_setpoint_ = input_pos_; vel_setpoint_ = input_vel_; - current_setpoint_ = input_current_; + torque_setpoint_ = input_torque_; // } break; case INPUT_MODE_VEL_RAMP: { float max_step_size = std::abs(current_meas_period * config_.vel_ramp_rate); @@ -161,21 +161,21 @@ bool Controller::update(float* current_setpoint_output) { float step = std::clamp(full_step, -max_step_size, max_step_size); vel_setpoint_ += step; - current_setpoint_ = (step / current_meas_period) * config_.inertia; + torque_setpoint_ = (step / current_meas_period) * config_.inertia; } break; case INPUT_MODE_CURRENT_RAMP: { float max_step_size = std::abs(current_meas_period * config_.current_ramp_rate); - float full_step = input_current_ - current_setpoint_; + float full_step = input_torque_ - torque_setpoint_; float step = std::clamp(full_step, -max_step_size, max_step_size); - current_setpoint_ += step; + torque_setpoint_ += step; } break; case INPUT_MODE_POS_FILTER: { // 2nd order pos tracking filter float delta_pos = input_pos_ - pos_setpoint_; // Pos error float delta_vel = input_vel_ - vel_setpoint_; // Vel error float accel = input_filter_kp_*delta_pos + input_filter_ki_*delta_vel; // Feedback - current_setpoint_ = accel * config_.inertia; // Accel + torque_setpoint_ = accel * config_.inertia; // Accel vel_setpoint_ += current_meas_period * accel; // delta vel pos_setpoint_ += current_meas_period * vel_setpoint_; // Delta pos } break; @@ -205,13 +205,13 @@ bool Controller::update(float* current_setpoint_output) { config_.control_mode = CONTROL_MODE_POSITION_CONTROL; pos_setpoint_ = input_pos_; vel_setpoint_ = 0.0f; - current_setpoint_ = 0.0f; + torque_setpoint_ = 0.0f; trajectory_done_ = true; } else { TrapezoidalTrajectory::Step_t traj_step = axis_->trap_traj_.eval(axis_->trap_traj_.t_); pos_setpoint_ = traj_step.Y; vel_setpoint_ = traj_step.Yd; - current_setpoint_ = traj_step.Ydd * config_.inertia; + torque_setpoint_ = traj_step.Ydd * config_.inertia; axis_->trap_traj_.t_ += current_meas_period; } anticogging_pos = pos_setpoint_; // FF the position setpoint instead of the pos_estimate @@ -287,13 +287,14 @@ bool Controller::update(float* current_setpoint_output) { } // Velocity control - float Iq = current_setpoint_; + float torque = torque_setpoint_; // Anti-cogging is enabled after calibration // We get the current position and apply a current feed-forward // ensuring that we handle negative encoder positions properly (-1 == motor->encoder.encoder_cpr - 1) + // anticogging currently in units of [A], multiply by Kt to get back to torque. if (anticogging_valid_ && config_.anticogging.anticogging_enabled) { - Iq += config_.anticogging.cogging_map[std::clamp(mod((int)anticogging_pos, 3600), 0, 3600)]; + torque += config_.anticogging.cogging_map[std::clamp(mod((int)anticogging_pos, 3600), 0, 3600)] * axis_->motor_.config_.torque_constant; } float v_err = 0.0f; @@ -304,10 +305,10 @@ bool Controller::update(float* current_setpoint_output) { } v_err = vel_des - *vel_estimate_src; - Iq += (vel_gain * gain_scheduling_multiplier) * v_err; + torque += (vel_gain * gain_scheduling_multiplier) * v_err; // Velocity integral action before limiting - Iq += vel_integrator_current_; + torque += vel_integrator_current_; } // Velocity limiting in current mode @@ -316,21 +317,21 @@ bool Controller::update(float* current_setpoint_output) { set_error(ERROR_INVALID_ESTIMATE); return false; } - Iq = limitVel(config_.vel_limit, *vel_estimate_src, vel_gain, Iq); + torque = limitVel(config_.vel_limit, *vel_estimate_src, vel_gain, torque); } // 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) { + float Tlim = axis_->motor_.effective_torque_lim(); + if (torque > Tlim) { limited = true; - Iq = Ilim; + torque = Tlim; } - if (Iq < -Ilim) { + if (torque < -Tlim) { limited = true; - Iq = -Ilim; + torque = -Tlim; } // Velocity integrator (behaviour dependent on limiting) @@ -346,6 +347,6 @@ bool Controller::update(float* current_setpoint_output) { } } - if (current_setpoint_output) *current_setpoint_output = Iq; + if (torque_setpoint_output) *torque_setpoint_output = torque; return true; } diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index be84d159..9529f822 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -64,7 +64,7 @@ public: bool anticogging_calibration(float pos_estimate, float vel_estimate); void update_filter_gains(); - bool update(float* current_setpoint); + bool update(float* torque_setpoint); Config_t& config_; Axis* axis_ = nullptr; // set by Axis constructor @@ -81,11 +81,12 @@ public: float vel_setpoint_ = 0.0f; // float vel_setpoint = 800.0f; float vel_integrator_current_ = 0.0f; // [A] - float current_setpoint_ = 0.0f; // [A] + float torque_setpoint_ = 0.0f; // [Nm] float input_pos_ = 0.0f; float input_vel_ = 0.0f; float input_current_ = 0.0f; + float input_torque_ = 0.0f; float input_filter_kp_ = 0.0f; float input_filter_ki_ = 0.0f; diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 76cdfa5e..568f9373 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -177,19 +177,19 @@ bool Motor::do_checks() { return true; } -float Motor::effective_current_lim() { +float Motor::effective_torque_lim() { // Configured limit - float current_lim = config_.current_lim; + float torque_lim = config_.torque_lim; // Hardware limit if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL) { - current_lim = std::min(current_lim, 0.98f*one_by_sqrt3*vbus_voltage); + torque_lim = std::min(torque_lim, 0.98f*one_by_sqrt3*vbus_voltage); //gimbal motor is voltage control, not Nm or A } else { - current_lim = std::min(current_lim, axis_->motor_.current_control_.max_allowed_current); + torque_lim = std::min(torque_lim, axis_->motor_.current_control_.max_allowed_torque); } // Thermal limit - current_lim = std::min(current_lim, thermal_current_lim_); + torque_lim = std::min(torque_lim, thermal_torque_lim_); - return current_lim; + return torque_lim; } void Motor::log_timing(TimingLog_t log_idx) { @@ -359,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 = effective_current_lim() + config_.current_lim_margin; + float I_trip = (effective_torque_lim() + config_.torque_lim_margin) / config_.torque_constant; if (SQ(Id) + SQ(Iq) > SQ(I_trip)) { set_error(ERROR_CURRENT_LIMIT_VIOLATION); return false; @@ -440,13 +440,14 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_pha } -bool Motor::update(float current_setpoint, float phase, float phase_vel) { +bool Motor::update(float torque_setpoint, float phase, float phase_vel) { + float current_setpoint = torque_setpoint / config_.torque_constant; current_setpoint *= config_.direction; phase *= config_.direction; phase_vel *= config_.direction; // TODO: 2-norm vs independent clamping (current could be sqrt(2) bigger) - float ilim = effective_current_lim(); + float ilim = effective_torque_lim() / config_.torque_constant; float id = std::clamp(current_control_.Id_setpoint, -ilim, ilim); float iq = std::clamp(current_setpoint, -ilim, ilim); diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 6e449111..6aaaaf18 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -29,6 +29,7 @@ public: float Id_measured; // [A] float I_measured_report_filter_k; float max_allowed_current; // [A] + float max_allowed_torque; // [Nm] float overcurrent_trip_level; // [A] float acim_rotor_flux; // [A] float async_phase_vel; // [rad/s electrical] @@ -45,12 +46,15 @@ public: float resistance_calib_max_voltage = 2.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 + float torque_constant = 1.0f; // to be set by user int32_t direction = 0; // 1 or -1 (0 = unspecified) MotorType 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] float current_lim_margin = 8.0f; // Maximum violation of current_lim + float torque_lim = 10.0f; //[Nm] + float torque_lim_margin = 8.0f; // Value used to compute shunt amplifier gains float requested_current_range = 60.0f; // [A] float current_control_bandwidth = 1000.0f; // [rad/s] @@ -92,7 +96,7 @@ public: bool do_checks(); float get_inverter_temp(); bool update_thermal_limits(float fet_temp); - float effective_current_lim(); + float effective_torque_lim(); void log_timing(TimingLog_t log_idx); float phase_current_from_adcval(uint32_t ADCValue); bool measure_phase_resistance(float test_current, float max_voltage); @@ -149,6 +153,7 @@ public: .Id_measured = 0.0f, .I_measured_report_filter_k = 1.0f, .max_allowed_current = 0.0f, + .max_allowed_torque = 0.0f, .overcurrent_trip_level = 0.0f, .acim_rotor_flux = 0.0f, .async_phase_vel = 0.0f, @@ -159,6 +164,7 @@ public: } gate_driver_exported_; DRV_SPI_8301_Vars_t gate_driver_regs_; //Local view of DRV registers (initialized by DRV8301_setup) float thermal_current_lim_ = 10.0f; //[A] + float thermal_torque_lim_ = 10.0f; //[Nm] float inverter_temp_ = NAN; // [°C] NaN while the ODrive is initializing. }; diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index f17aca63..ea118264 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -376,6 +376,7 @@ interfaces: Id_measured: float32 I_measured_report_filter_k: float32 max_allowed_current: readonly float32 + max_allowed_torque: readonly float32 overcurrent_trip_level: readonly float32 acim_rotor_flux: float32 async_phase_vel: readonly float32 @@ -427,10 +428,13 @@ interfaces: resistance_calib_max_voltage: float32 phase_inductance: {type: float32, c_setter: set_phase_inductance} phase_resistance: {type: float32, c_setter: set_phase_resistance} + torque_constant: float32 direction: int32 motor_type: MotorType current_lim: float32 current_lim_margin: float32 + torque_lim: float32 + torque_lim_margin: float32 inverter_temp_limit_lower: float32 inverter_temp_limit_upper: float32 requested_current_range: float32 @@ -458,9 +462,10 @@ interfaces: input_pos: {type: float32, c_setter: set_input_pos} input_vel: float32 input_current: float32 + input_torque: float32 pos_setpoint: readonly float32 vel_setpoint: readonly float32 - current_setpoint: readonly float32 + torque_setpoint: readonly float32 trajectory_done: readonly bool vel_integrator_current: float32 anticogging_valid: bool From d78119e29fc567fae9991624b6f89987775a4d64 Mon Sep 17 00:00:00 2001 From: pjohnson Date: Tue, 16 Jun 2020 17:10:36 -0400 Subject: [PATCH 02/13] Modified docs and communication interfaces to reflect change from A to Nm for motor control input. Renamed vel_integrator_current_ to vel_integrator_torque_ Removed input_current_ from controller, added input_torque_ --- Firmware/MotorControl/axis.cpp | 8 +++--- Firmware/MotorControl/controller.cpp | 19 +++++++------- Firmware/MotorControl/controller.hpp | 5 ++-- Firmware/communication/ascii_protocol.cpp | 30 +++++++++++------------ Firmware/communication/can_simple.cpp | 12 ++++----- Firmware/communication/can_simple.hpp | 4 +-- Firmware/odrive-interface.yaml | 5 ++-- docs/commands.md | 2 +- docs/getting-started.md | 4 +-- docs/input_modes.md | 6 ++--- tools/.vscode/launch.json | 2 +- tools/odrive/tests/can_test.py | 14 +++++------ tools/odrive/tests/closed_loop_test.py | 24 +++++++++--------- tools/odrive/tests/old_tests.py | 2 +- tools/odrive/tests/uart_ascii_test.py | 12 ++++----- 15 files changed, 73 insertions(+), 76 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 2882ca2a..172f3dc4 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -306,7 +306,7 @@ bool Axis::run_closed_loop_control_loop() { controller_.input_pos_ = *controller_.pos_estimate_src_; // Avoid integrator windup issues - controller_.vel_integrator_current_ = 0.0f; + controller_.vel_integrator_torque_ = 0.0f; set_step_dir_active(config_.enable_step_dir); run_control_loop([this](){ @@ -344,7 +344,7 @@ bool Axis::run_homing() { controller_.input_pos_ = 0.0f; controller_.input_pos_updated(); controller_.input_vel_ = -controller_.config_.homing_speed; - controller_.input_current_ = 0.0f; + controller_.input_torque_ = 0.0f; homing_.is_homed = false; @@ -356,7 +356,7 @@ bool Axis::run_homing() { controller_.pos_setpoint_ = *controller_.pos_estimate_src_; // Avoid integrator windup issues - controller_.vel_integrator_current_ = 0.0f; + controller_.vel_integrator_torque_ = 0.0f; run_control_loop([this](){ // Note that all estimators are updated in the loop prefix in run_control_loop @@ -385,7 +385,7 @@ bool Axis::run_homing() { controller_.input_pos_ = 0.0f; controller_.input_pos_updated(); controller_.input_vel_ = 0.0f; - controller_.input_current_ = 0.0f; + controller_.input_torque_ = 0.0f; run_control_loop([this](){ // Note that all estimators are updated in the loop prefix in run_control_loop diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 2138af3c..89eb9a5e 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -13,7 +13,7 @@ Controller::Controller(Config_t& config) : void Controller::reset() { pos_setpoint_ = 0.0f; vel_setpoint_ = 0.0f; - vel_integrator_current_ = 0.0f; + vel_integrator_torque_ = 0.0f; torque_setpoint_ = 0.0f; } @@ -87,13 +87,13 @@ bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) float pos_err = input_pos_ - pos_estimate; if (std::abs(pos_err) <= config_.anticogging.calib_pos_threshold && std::abs(vel_estimate) < config_.anticogging.calib_vel_threshold) { - config_.anticogging.cogging_map[std::clamp(config_.anticogging.index++, 0, 3600)] = vel_integrator_current_; + config_.anticogging.cogging_map[std::clamp(config_.anticogging.index++, 0, 3600)] = vel_integrator_torque_; } if (config_.anticogging.index < 3600) { config_.control_mode = CONTROL_MODE_POSITION_CONTROL; input_pos_ = config_.anticogging.index * axis_->encoder_.getCoggingRatio(); input_vel_ = 0.0f; - input_current_ = 0.0f; + input_torque_ = 0.0f; input_pos_updated(); return false; } else { @@ -101,7 +101,7 @@ bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) config_.control_mode = CONTROL_MODE_POSITION_CONTROL; input_pos_ = 0.0f; // Send the motor home input_vel_ = 0.0f; - input_current_ = 0.0f; + input_torque_ = 0.0f; input_pos_updated(); anticogging_valid_ = true; config_.anticogging.calib_anticogging = false; @@ -292,9 +292,8 @@ bool Controller::update(float* torque_setpoint_output) { // Anti-cogging is enabled after calibration // We get the current position and apply a current feed-forward // ensuring that we handle negative encoder positions properly (-1 == motor->encoder.encoder_cpr - 1) - // anticogging currently in units of [A], multiply by Kt to get back to torque. if (anticogging_valid_ && config_.anticogging.anticogging_enabled) { - torque += config_.anticogging.cogging_map[std::clamp(mod((int)anticogging_pos, 3600), 0, 3600)] * axis_->motor_.config_.torque_constant; + torque += config_.anticogging.cogging_map[std::clamp(mod((int)anticogging_pos, 3600), 0, 3600)]; } float v_err = 0.0f; @@ -308,7 +307,7 @@ bool Controller::update(float* torque_setpoint_output) { torque += (vel_gain * gain_scheduling_multiplier) * v_err; // Velocity integral action before limiting - torque += vel_integrator_current_; + torque += vel_integrator_torque_; } // Velocity limiting in current mode @@ -337,13 +336,13 @@ bool Controller::update(float* torque_setpoint_output) { // Velocity integrator (behaviour dependent on limiting) if (config_.control_mode < CONTROL_MODE_VELOCITY_CONTROL) { // reset integral if not in use - vel_integrator_current_ = 0.0f; + vel_integrator_torque_ = 0.0f; } else { if (limited) { // TODO make decayfactor configurable - vel_integrator_current_ *= 0.99f; + vel_integrator_torque_ *= 0.99f; } else { - vel_integrator_current_ += ((vel_integrator_gain * gain_scheduling_multiplier) * current_meas_period) * v_err; + vel_integrator_torque_ += ((vel_integrator_gain * gain_scheduling_multiplier) * current_meas_period) * v_err; } } diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 9529f822..69ae5162 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -80,12 +80,11 @@ public: float pos_setpoint_ = 0.0f; float vel_setpoint_ = 0.0f; // float vel_setpoint = 800.0f; - float vel_integrator_current_ = 0.0f; // [A] - float torque_setpoint_ = 0.0f; // [Nm] + float vel_integrator_torque_ = 0.0f; // [Nm] + float torque_setpoint_ = 0.0f; // [Nm] float input_pos_ = 0.0f; float input_vel_ = 0.0f; - float input_current_ = 0.0f; float input_torque_ = 0.0f; float input_filter_kp_ = 0.0f; float input_filter_ki_ = 0.0f; diff --git a/Firmware/communication/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp index 39f4d891..6046214f 100644 --- a/Firmware/communication/ascii_protocol.cpp +++ b/Firmware/communication/ascii_protocol.cpp @@ -95,8 +95,8 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& // check incoming packet type if (cmd[0] == 'p') { // position control unsigned motor_number; - float pos_setpoint, vel_feed_forward, current_feed_forward; - int numscan = sscanf(cmd, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, ¤t_feed_forward); + float pos_setpoint, vel_feed_forward, torque_feed_forward; + int numscan = sscanf(cmd, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, &torque_feed_forward); if (numscan < 2) { respond(response_channel, use_checksum, "invalid command format"); } else if (motor_number >= AXIS_COUNT) { @@ -108,15 +108,15 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& if (numscan >= 3) axis->controller_.input_vel_ = vel_feed_forward; if (numscan >= 4) - axis->controller_.input_current_ = current_feed_forward; + axis->controller_.input_torque_ = torque_feed_forward; axis->controller_.input_pos_updated(); axis->watchdog_feed(); } } else if (cmd[0] == 'q') { // position control with limits unsigned motor_number; - float pos_setpoint, vel_limit, current_lim; - int numscan = sscanf(cmd, "q %u %f %f %f", &motor_number, &pos_setpoint, &vel_limit, ¤t_lim); + float pos_setpoint, vel_limit, torque_lim; + int numscan = sscanf(cmd, "q %u %f %f %f", &motor_number, &pos_setpoint, &vel_limit, &torque_lim); if (numscan < 2) { respond(response_channel, use_checksum, "invalid command format"); } else if (motor_number >= AXIS_COUNT) { @@ -128,15 +128,15 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& if (numscan >= 3) axis->controller_.config_.vel_limit = vel_limit; if (numscan >= 4) - axis->motor_.config_.current_lim = current_lim; + axis->motor_.config_.torque_lim = torque_lim; axis->controller_.input_pos_updated(); axis->watchdog_feed(); } } else if (cmd[0] == 'v') { // velocity control unsigned motor_number; - float vel_setpoint, current_feed_forward; - int numscan = sscanf(cmd, "v %u %f %f", &motor_number, &vel_setpoint, ¤t_feed_forward); + float vel_setpoint, torque_feed_forward; + int numscan = sscanf(cmd, "v %u %f %f", &motor_number, &vel_setpoint, &torque_feed_forward); if (numscan < 2) { respond(response_channel, use_checksum, "invalid command format"); } else if (motor_number >= AXIS_COUNT) { @@ -146,22 +146,22 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& axis->controller_.config_.control_mode = Controller::CONTROL_MODE_VELOCITY_CONTROL; axis->controller_.input_vel_ = vel_setpoint; if (numscan >= 3) - axis->controller_.input_current_ = current_feed_forward; + axis->controller_.input_torque_ = torque_feed_forward; axis->watchdog_feed(); } - } else if (cmd[0] == 'c') { // current control + } else if (cmd[0] == 'c') { // torque control unsigned motor_number; - float current_setpoint; - int numscan = sscanf(cmd, "c %u %f", &motor_number, ¤t_setpoint); + float torque_setpoint; + int numscan = sscanf(cmd, "c %u %f", &motor_number, &torque_setpoint); if (numscan < 2) { respond(response_channel, use_checksum, "invalid command format"); } else if (motor_number >= AXIS_COUNT) { respond(response_channel, use_checksum, "invalid motor %u", motor_number); } else { Axis* axis = axes[motor_number]; - axis->controller_.config_.control_mode = Controller::CONTROL_MODE_CURRENT_CONTROL; - axis->controller_.input_current_ = current_setpoint; + axis->controller_.config_.control_mode = Controller::CONTROL_MODE_TORQUE_CONTROL; + axis->controller_.input_torque_ = torque_setpoint; axis->watchdog_feed(); } @@ -200,7 +200,7 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& respond(response_channel, use_checksum, "Position: q axis pos vel-lim I-lim"); respond(response_channel, use_checksum, "Position: p axis pos vel-ff I-ff"); respond(response_channel, use_checksum, "Velocity: v axis vel I-ff"); - respond(response_channel, use_checksum, "Current: c axis I"); + respond(response_channel, use_checksum, "Torque: c axis T"); respond(response_channel, use_checksum, ""); respond(response_channel, use_checksum, "Properties start at odrive root, such as axis0.requested_state"); respond(response_channel, use_checksum, "Read: r property"); diff --git a/Firmware/communication/can_simple.cpp b/Firmware/communication/can_simple.cpp index a6953a41..18d39838 100644 --- a/Firmware/communication/can_simple.cpp +++ b/Firmware/communication/can_simple.cpp @@ -82,8 +82,8 @@ void CANSimple::handle_can_message(can_Message_t& msg) { case MSG_SET_INPUT_VEL: set_input_vel_callback(axis, msg); break; - case MSG_SET_INPUT_CURRENT: - set_input_current_callback(axis, msg); + case MSG_SET_INPUT_TORQUE: + set_input_torque_callback(axis, msg); break; case MSG_SET_CONTROLLER_MODES: set_controller_modes_callback(axis, msg); @@ -281,17 +281,17 @@ void CANSimple::get_encoder_count_callback(Axis* axis, can_Message_t& msg) { void CANSimple::set_input_pos_callback(Axis* axis, can_Message_t& msg) { axis->controller_.input_pos_ = can_getSignal(msg, 0, 32, true); axis->controller_.input_vel_ = can_getSignal(msg, 32, 16, true, 0.1f, 0); - axis->controller_.input_current_ = can_getSignal(msg, 48, 16, true, 0.01f, 0); + axis->controller_.input_torque_ = can_getSignal(msg, 48, 16, true, 0.01f, 0); axis->controller_.input_pos_updated(); } void CANSimple::set_input_vel_callback(Axis* axis, can_Message_t& msg) { axis->controller_.input_vel_ = can_getSignal(msg, 0, 32, true, 0.01f, 0.0f); - axis->controller_.input_current_ = can_getSignal(msg, 32, 16, true, 0.01f, 0.0f); + axis->controller_.input_torque_ = can_getSignal(msg, 32, 16, true, 0.01f, 0.0f); } -void CANSimple::set_input_current_callback(Axis* axis, can_Message_t& msg) { - axis->controller_.input_current_ = can_getSignal(msg, 0, 32, true, 0.01f, 0); +void CANSimple::set_input_torque_callback(Axis* axis, can_Message_t& msg) { + axis->controller_.input_torque_ = can_getSignal(msg, 0, 32, true, 0.01f, 0); } void CANSimple::set_controller_modes_callback(Axis* axis, can_Message_t& msg) { diff --git a/Firmware/communication/can_simple.hpp b/Firmware/communication/can_simple.hpp index c4b6d6ed..a4ccae4d 100644 --- a/Firmware/communication/can_simple.hpp +++ b/Firmware/communication/can_simple.hpp @@ -20,7 +20,7 @@ class CANSimple { MSG_SET_CONTROLLER_MODES, MSG_SET_INPUT_POS, MSG_SET_INPUT_VEL, - MSG_SET_INPUT_CURRENT, + MSG_SET_INPUT_TORQUE, MSG_SET_VEL_LIMIT, MSG_START_ANTICOGGING, MSG_SET_TRAJ_VEL_LIMIT, @@ -51,7 +51,7 @@ class CANSimple { static void get_encoder_count_callback(Axis* axis, can_Message_t& msg); static void set_input_pos_callback(Axis* axis, can_Message_t& msg); static void set_input_vel_callback(Axis* axis, can_Message_t& msg); - static void set_input_current_callback(Axis* axis, can_Message_t& msg); + static void set_input_torque_callback(Axis* axis, can_Message_t& msg); static void set_controller_modes_callback(Axis* axis, can_Message_t& msg); static void set_vel_limit_callback(Axis* axis, can_Message_t& msg); static void start_anticogging_callback(Axis* axis, can_Message_t& msg); diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index ea118264..5fbfc10d 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -461,13 +461,12 @@ interfaces: InvalidEstimate: input_pos: {type: float32, c_setter: set_input_pos} input_vel: float32 - input_current: float32 input_torque: float32 pos_setpoint: readonly float32 vel_setpoint: readonly float32 torque_setpoint: readonly float32 trajectory_done: readonly bool - vel_integrator_current: float32 + vel_integrator_torque: float32 anticogging_valid: bool config: c_is_class: False @@ -681,7 +680,7 @@ valuetypes: # Note: these should be sorted from lowest level of control to # highest level of control, to allow "<" style comparisons. VoltageControl: - CurrentControl: + TorqueControl: VelocityControl: PositionControl: diff --git a/docs/commands.md b/docs/commands.md index 8849c9dc..ba915564 100644 --- a/docs/commands.md +++ b/docs/commands.md @@ -85,7 +85,7 @@ For more information, see [input_modes](input_modes.md). # Control Commands * `.controller.input_pos = ` * `.controller.input_vel = ` -* `.controller.input_current = ` +* `.controller.input_torque = ` ### Input Mode To modify the way the control command affects the motor, you can use the input mode. The default input mode is pass through. diff --git a/docs/getting-started.md b/docs/getting-started.md index a4a92a04..968cf34b 100644 --- a/docs/getting-started.md +++ b/docs/getting-started.md @@ -346,9 +346,9 @@ Set the velocity ramp rate (acceleration): `axis.controller.config.vel_ramp_rate Activate the ramped velocity mode: `axis.controller.config.input_mode = INPUT_MODE_VEL_RAMP`.
You can now control the velocity with `axis.controller.input_vel = 5000` [count/s]. -### Current control +### Torque control Set `axis.controller.config.control_mode = CONTROL_MODE_CURRENT_CONTROL`.
-You can now control the current with `axis.controller.input_current = 3` [A]. +You can now control the torque with `axis.controller.input_torque = 3` [Nm]. Note: If you exceed `vel_limit` in current control mode, the current is reduced. To disable this, set `axis.controller.enable_current_mode_vel_limit = False`. diff --git a/docs/input_modes.md b/docs/input_modes.md index be449e24..4b0657d9 100644 --- a/docs/input_modes.md +++ b/docs/input_modes.md @@ -4,7 +4,7 @@ As of version ###, ODrive now intercepts the incoming commands and can apply fil * `.controller.config.input_mode` * `.controller.input_pos` * `.controller.input_vel` -* `.controller.input_current` +* `.controller.input_torque` The Input Modes currently valid are: * `INPUT_MODE_INACTIVE` @@ -27,7 +27,7 @@ Pass `input_xxx` through to `xxx_setpoint` directly. ### Valid Inputs: * `input_pos` * `input_vel` -* `input_current` +* `input_torque` ### Valid Control modes: * `CONTROL_MODE_VOLTAGE_CONTROL` @@ -92,7 +92,7 @@ Ramp a current command from the current value to the target value. * `.controller.config.current_ramp_rate` ### Valid Inputs: -* `input_current` +* `input_torque` ### Valid Control Modes: * `CONTROL_MODE_CURRENT_CONTROL` diff --git a/tools/.vscode/launch.json b/tools/.vscode/launch.json index a3b07eff..69a77ee7 100644 --- a/tools/.vscode/launch.json +++ b/tools/.vscode/launch.json @@ -9,7 +9,7 @@ "type": "python", "request": "launch", "stopOnEntry": true, - "pythonPath": "${config:python.pythonPath}", + "pythonPath": "${config:python.interpreterPath}", "program": "${file}", "cwd": "${workspaceRoot}", "env": {}, diff --git a/tools/odrive/tests/can_test.py b/tools/odrive/tests/can_test.py index 1012250f..8e8e9c4b 100644 --- a/tools/odrive/tests/can_test.py +++ b/tools/odrive/tests/can_test.py @@ -26,7 +26,7 @@ command_set = { 'set_controller_modes': (0x00b, [('control_mode', 'i', 1), ('input_mode', 'i', 1)]), # tested 'set_input_pos': (0x00c, [('input_pos', 'i', 1), ('vel_ff', 'h', 0.1), ('cur_ff', 'h', 0.01)]), # tested 'set_input_vel': (0x00d, [('input_vel', 'i', 0.01), ('cur_ff', 'h', 0.01)]), # tested - 'set_input_current': (0x00e, [('input_current', 'i', 0.01)]), # tested + 'set_input_torque': (0x00e, [('input_torque', 'i', 0.01)]), # tested 'set_velocity_limit': (0x00f, [('velocity_limit', 'f', 1)]), # tested 'start_anticogging': (0x010, []), # untested 'set_traj_vel_limit': (0x011, [('traj_vel_limit', 'f', 1)]), # tested @@ -174,23 +174,23 @@ class TestSimpleCAN(): axis.controller.input_pos = 1234 axis.controller.input_vel = 1234 - axis.controller.input_current = 1234 + axis.controller.input_torque = 1234 my_cmd('set_input_pos', input_pos=1, vel_ff=2, cur_ff=3) fence() test_assert_eq(axis.controller.input_pos, 1.0, range=0.1) test_assert_eq(axis.controller.input_vel, 2.0, range=0.01) - test_assert_eq(axis.controller.input_current, 3.0, range=0.001) + test_assert_eq(axis.controller.input_torque, 3.0, range=0.001) axis.controller.config.control_mode = CONTROL_MODE_VELOCITY_CONTROL my_cmd('set_input_vel', input_vel=-10.0, cur_ff=30.1234) fence() test_assert_eq(axis.controller.input_vel, -10.0, range=0.01) - test_assert_eq(axis.controller.input_current, 30.1234, range=0.01) + test_assert_eq(axis.controller.input_torque, 30.1234, range=0.01) axis.controller.config.control_mode = CONTROL_MODE_CURRENT_CONTROL - my_cmd('set_input_current', input_current=3.1415) + my_cmd('set_input_torque', input_torque=3.1415) fence() - test_assert_eq(axis.controller.input_current, 3.1415, range=0.01) + test_assert_eq(axis.controller.input_torque, 3.1415, range=0.01) my_cmd('set_velocity_limit', velocity_limit=23456.78) fence() @@ -210,7 +210,7 @@ class TestSimpleCAN(): test_assert_eq(axis.controller.config.inertia, 55.086, range=0.0001) # any CAN cmd will feed the watchdog - test_watchdog(axis, lambda: my_cmd('set_input_current', input_current=0.0), logger) + test_watchdog(axis, lambda: my_cmd('set_input_torque', input_torque=0.0), logger) logger.debug('testing heartbeat...') # note that this will include the heartbeats that were received during the diff --git a/tools/odrive/tests/closed_loop_test.py b/tools/odrive/tests/closed_loop_test.py index 6d5bcecb..b92ada8c 100644 --- a/tools/odrive/tests/closed_loop_test.py +++ b/tools/odrive/tests/closed_loop_test.py @@ -246,19 +246,19 @@ class TestVelLimitInCurrentControl(TestClosedLoopControlBase): # Abort immediately if the absolute limits are exceeded test_assert_within(current_setpoint, -max_current, max_current) test_assert_within(velocity, -absolute_max_vel, absolute_max_vel) - return input_current, velocity, current_setpoint, get_expected_setpoint(input_current, velocity) + return input_torque, velocity, current_setpoint, get_expected_setpoint(input_torque, velocity) - axis_ctx.handle.controller.input_current = input_current = 0.0 + axis_ctx.handle.controller.input_torque = input_torque = 0.0 request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) # Move the system around its operating envelope - axis_ctx.handle.controller.input_current = input_current = 2.0 + axis_ctx.handle.controller.input_torque = input_torque = 2.0 dataA = record_log(data_getter, duration=1.0) - axis_ctx.handle.controller.input_current = input_current = -2.0 + axis_ctx.handle.controller.input_torque = input_torque = -2.0 dataA = np.concatenate([dataA, record_log(data_getter, duration=1.0)]) - axis_ctx.handle.controller.input_current = input_current = 4.0 + axis_ctx.handle.controller.input_torque = input_torque = 4.0 dataA = np.concatenate([dataA, record_log(data_getter, duration=1.0)]) - axis_ctx.handle.controller.input_current = input_current = -4.0 + axis_ctx.handle.controller.input_torque = input_torque = -4.0 dataA = np.concatenate([dataA, record_log(data_getter, duration=1.0)]) # Shrink the operating envelope while motor is moving faster than the envelope allows @@ -267,22 +267,22 @@ class TestVelLimitInCurrentControl(TestClosedLoopControlBase): axis_ctx.handle.controller.config.vel_limit = max_vel # Move the system around its operating envelope - axis_ctx.handle.controller.input_current = input_current = 2.0 + axis_ctx.handle.controller.input_torque = input_torque = 2.0 dataB = record_log(data_getter, duration=1.0) - axis_ctx.handle.controller.input_current = input_current = -2.0 + axis_ctx.handle.controller.input_torque = input_torque = -2.0 dataB = np.concatenate([dataB, record_log(data_getter, duration=1.0)]) - axis_ctx.handle.controller.input_current = input_current = 4.0 + axis_ctx.handle.controller.input_torque = input_torque = 4.0 dataB = np.concatenate([dataB, record_log(data_getter, duration=1.0)]) - axis_ctx.handle.controller.input_current = input_current = -4.0 + axis_ctx.handle.controller.input_torque = input_torque = -4.0 dataB = np.concatenate([dataB, record_log(data_getter, duration=1.0)]) # Try the shrink maneuver again at positive velocity axis_ctx.handle.controller.config.vel_limit = 20.0 * float(enc_ctx.yaml['cpr']) - axis_ctx.handle.controller.input_current = 4.0 + axis_ctx.handle.controller.input_torque = 4.0 time.sleep(0.5) axis_ctx.handle.controller.config.vel_limit = max_vel - axis_ctx.handle.controller.input_current = input_current = 2.0 + axis_ctx.handle.controller.input_torque = input_torque = 2.0 dataB = np.concatenate([dataB, record_log(data_getter, duration=1.0)]) test_assert_no_error(axis_ctx) diff --git a/tools/odrive/tests/old_tests.py b/tools/odrive/tests/old_tests.py index cf22ed10..fac90f1d 100644 --- a/tools/odrive/tests/old_tests.py +++ b/tools/odrive/tests/old_tests.py @@ -581,7 +581,7 @@ class TestVelCtrlVsPosCtrl(DualAxisTest): # Set up viscous fluid load logger.debug("activating load on {}...".format(load_ctx.name)) load_ctx.handle.controller.config.vel_integrator_gain = 0 - load_ctx.handle.controller.vel_integrator_current = 0 + load_ctx.handle.controller.vel_integrator_torque = 0 set_limits(load_ctx, logger, vel_limit=100000, current_limit=50) load_ctx.handle.controller.set_vel_setpoint(0, 0) request_state(load_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) diff --git a/tools/odrive/tests/uart_ascii_test.py b/tools/odrive/tests/uart_ascii_test.py index ee9a3fa4..6c650e28 100644 --- a/tools/odrive/tests/uart_ascii_test.py +++ b/tools/odrive/tests/uart_ascii_test.py @@ -101,28 +101,28 @@ class TestUartAscii(): # Test 'c', 'v', 'p', 'q' and 'f' commands - odrive.handle.axis0.controller.input_current = 0 + odrive.handle.axis0.controller.input_torque = 0 ser.write(b'c 0 12.5\n') test_assert_eq(ser.readline(), b'') - test_assert_eq(odrive.handle.axis0.controller.input_current, 12.5, accuracy=0.001) + test_assert_eq(odrive.handle.axis0.controller.input_torque, 12.5, accuracy=0.001) test_assert_eq(odrive.handle.axis0.controller.config.control_mode, CONTROL_MODE_CURRENT_CONTROL) odrive.handle.axis0.controller.input_vel = 0 - odrive.handle.axis0.controller.input_current = 0 + odrive.handle.axis0.controller.input_torque = 0 ser.write(b'v 0 567.8 12.5\n') test_assert_eq(ser.readline(), b'') test_assert_eq(odrive.handle.axis0.controller.input_vel, 567.8, accuracy=0.001) - test_assert_eq(odrive.handle.axis0.controller.input_current, 12.5, accuracy=0.001) + test_assert_eq(odrive.handle.axis0.controller.input_torque, 12.5, accuracy=0.001) test_assert_eq(odrive.handle.axis0.controller.config.control_mode, CONTROL_MODE_VELOCITY_CONTROL) odrive.handle.axis0.controller.input_pos = 0 odrive.handle.axis0.controller.input_vel = 0 - odrive.handle.axis0.controller.input_current = 0 + odrive.handle.axis0.controller.input_torque = 0 ser.write(b'p 0 123.4 567.8 12.5\n') test_assert_eq(ser.readline(), b'') test_assert_eq(odrive.handle.axis0.controller.input_pos, 123.4, accuracy=0.001) test_assert_eq(odrive.handle.axis0.controller.input_vel, 567.8, accuracy=0.001) - test_assert_eq(odrive.handle.axis0.controller.input_current, 12.5, accuracy=0.001) + test_assert_eq(odrive.handle.axis0.controller.input_torque, 12.5, accuracy=0.001) test_assert_eq(odrive.handle.axis0.controller.config.control_mode, CONTROL_MODE_POSITION_CONTROL) odrive.handle.axis0.controller.input_pos = 0 From ceabd24582f805fa7cd16e9267eb3e0e6003fd76 Mon Sep 17 00:00:00 2001 From: pjohnson Date: Wed, 17 Jun 2020 13:15:36 -0400 Subject: [PATCH 03/13] Made changes reflecting PR comments. Added torque_ramp_rate to controller config. Changed INPUT_MODE_CURRENT_RAMP to INPUT_MODE_TORQUE_RAMP for controller input mode enum. Torque limits and current limits are now observed seperately. Torque limit is in the controller, current limit is in the motor object. Fixed torque -> current calculation in motor_update to handle ACIM motors. --- Firmware/MotorControl/controller.cpp | 11 +++++------ Firmware/MotorControl/controller.hpp | 2 +- Firmware/MotorControl/motor.cpp | 27 +++++++++++++++++---------- Firmware/MotorControl/motor.hpp | 4 +--- Firmware/odrive-interface.yaml | 7 +++---- docs/commands.md | 4 ++-- docs/getting-started.md | 2 +- docs/input_modes.md | 8 ++++---- tools/.vscode/launch.json | 2 +- 9 files changed, 35 insertions(+), 32 deletions(-) diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 89eb9a5e..6dbb6878 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -163,8 +163,8 @@ bool Controller::update(float* torque_setpoint_output) { vel_setpoint_ += step; torque_setpoint_ = (step / current_meas_period) * config_.inertia; } break; - case INPUT_MODE_CURRENT_RAMP: { - float max_step_size = std::abs(current_meas_period * config_.current_ramp_rate); + case INPUT_MODE_TORQUE_RAMP: { + float max_step_size = std::abs(current_meas_period * config_.torque_ramp_rate); float full_step = input_torque_ - torque_setpoint_; float step = std::clamp(full_step, -max_step_size, max_step_size); @@ -319,11 +319,10 @@ bool Controller::update(float* torque_setpoint_output) { torque = limitVel(config_.vel_limit, *vel_estimate_src, vel_gain, torque); } - // Current limiting - // TODO: Change to controller working in torque units - // and get the torque limits from a function of the motor + // Limit max torque to a user defined torque limit. This functions as an acceleration limit. + // The motor object handles current limiting bool limited = false; - float Tlim = axis_->motor_.effective_torque_lim(); + float Tlim = axis_->motor_.config_.torque_lim; if (torque > Tlim) { limited = true; torque = Tlim; diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 69ae5162..851f265c 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -28,7 +28,7 @@ public: float vel_limit = 20000.0f; // [counts/s] Infinity to disable. float vel_limit_tolerance = 1.2f; // ratio to vel_lim. Infinity to disable. float vel_ramp_rate = 10000.0f; // [(counts/s) / s] - float current_ramp_rate = 1.0f; // A / sec + float torque_ramp_rate = 0.1f; // Nm / sec bool setpoints_in_cpr = false; float inertia = 0.0f; // [A/(count/s^2)] float input_filter_bandwidth = 2.0f; // [1/s] diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 568f9373..2c7ab5f9 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -177,19 +177,19 @@ bool Motor::do_checks() { return true; } -float Motor::effective_torque_lim() { +float Motor::effective_current_lim() { // Configured limit - float torque_lim = config_.torque_lim; + float current_lim = config_.current_lim; // Hardware limit if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL) { - torque_lim = std::min(torque_lim, 0.98f*one_by_sqrt3*vbus_voltage); //gimbal motor is voltage control, not Nm or A + current_lim = std::min(current_lim, 0.98f*one_by_sqrt3*vbus_voltage); //gimbal motor is voltage control } else { - torque_lim = std::min(torque_lim, axis_->motor_.current_control_.max_allowed_torque); + current_lim = std::min(current_lim, axis_->motor_.current_control_.max_allowed_current); } // Thermal limit - torque_lim = std::min(torque_lim, thermal_torque_lim_); + current_lim = std::min(current_lim, thermal_current_lim_); - return torque_lim; + return current_lim; } void Motor::log_timing(TimingLog_t log_idx) { @@ -359,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 = (effective_torque_lim() + config_.torque_lim_margin) / config_.torque_constant; + 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; @@ -441,13 +441,20 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_pha bool Motor::update(float torque_setpoint, float phase, float phase_vel) { - float current_setpoint = torque_setpoint / config_.torque_constant; - current_setpoint *= config_.direction; + float current_setpoint; phase *= config_.direction; phase_vel *= config_.direction; + if (config_.motor_type == MOTOR_TYPE_ACIM) { + current_setpoint = torque_setpoint / (config_.torque_constant * fmax(current_control_.acim_rotor_flux, config_.acim_gain_min_flux)); + } + else { + current_setpoint = torque_setpoint / config_.torque_constant; + } + current_setpoint *= config_.direction; + // TODO: 2-norm vs independent clamping (current could be sqrt(2) bigger) - float ilim = effective_torque_lim() / config_.torque_constant; + float ilim = effective_current_lim(); float id = std::clamp(current_control_.Id_setpoint, -ilim, ilim); float iq = std::clamp(current_setpoint, -ilim, ilim); diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 6aaaaf18..178a99db 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -29,7 +29,6 @@ public: float Id_measured; // [A] float I_measured_report_filter_k; float max_allowed_current; // [A] - float max_allowed_torque; // [Nm] float overcurrent_trip_level; // [A] float acim_rotor_flux; // [A] float async_phase_vel; // [rad/s electrical] @@ -96,7 +95,7 @@ public: bool do_checks(); float get_inverter_temp(); bool update_thermal_limits(float fet_temp); - float effective_torque_lim(); + float effective_current_lim(); void log_timing(TimingLog_t log_idx); float phase_current_from_adcval(uint32_t ADCValue); bool measure_phase_resistance(float test_current, float max_voltage); @@ -153,7 +152,6 @@ public: .Id_measured = 0.0f, .I_measured_report_filter_k = 1.0f, .max_allowed_current = 0.0f, - .max_allowed_torque = 0.0f, .overcurrent_trip_level = 0.0f, .acim_rotor_flux = 0.0f, .async_phase_vel = 0.0f, diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index 5fbfc10d..38e8a936 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -376,7 +376,6 @@ interfaces: Id_measured: float32 I_measured_report_filter_k: float32 max_allowed_current: readonly float32 - max_allowed_torque: readonly float32 overcurrent_trip_level: readonly float32 acim_rotor_flux: float32 async_phase_vel: readonly float32 @@ -497,9 +496,9 @@ interfaces: type: float32 doc: Ratio to `vel_limit`. Infinity to disable. vel_ramp_rate: float32 - current_ramp_rate: + torque_ramp_rate: type: float32 - unit: A / sec + unit: Nm / sec homing_speed: type: float32 unit: counts/s @@ -692,7 +691,7 @@ valuetypes: PosFilter: MixChannels: TrapTraj: - CurrentRamp: + TorqueRamp: Mirror: diff --git a/docs/commands.md b/docs/commands.md index ba915564..618ddaba 100644 --- a/docs/commands.md +++ b/docs/commands.md @@ -77,7 +77,7 @@ Possible values are: * `INPUT_MODE_POS_FILTER` * `INPUT_MODE_MIX_CHANNELS` * `INPUT_MODE_TRAP_TRAJ` -* `INPUT_MODE_CURRENT_RAMP` +* `INPUT_MODE_TORQUE_RAMP` * `INPUT_MODE_MIRROR` For more information, see [input_modes](input_modes.md). @@ -97,7 +97,7 @@ Possible values are: * `INPUT_MODE_POS_FILTER` * `INPUT_MODE_MIX_CHANNELS` * `INPUT_MODE_TRAP_TRAJ` -* `INPUT_MODE_CURRENT_RAMP` +* `INPUT_MODE_TORQUE_RAMP` * `INPUT_MODE_MIRROR` ## System monitoring commands diff --git a/docs/getting-started.md b/docs/getting-started.md index 968cf34b..9426f395 100644 --- a/docs/getting-started.md +++ b/docs/getting-started.md @@ -348,7 +348,7 @@ You can now control the velocity with `axis.controller.input_vel = 5000` [count/ ### Torque control Set `axis.controller.config.control_mode = CONTROL_MODE_CURRENT_CONTROL`.
-You can now control the torque with `axis.controller.input_torque = 3` [Nm]. +You can now control the torque with `axis.controller.input_torque = 0.1` [Nm]. Note: If you exceed `vel_limit` in current control mode, the current is reduced. To disable this, set `axis.controller.enable_current_mode_vel_limit = False`. diff --git a/docs/input_modes.md b/docs/input_modes.md index 4b0657d9..53463ad0 100644 --- a/docs/input_modes.md +++ b/docs/input_modes.md @@ -13,7 +13,7 @@ The Input Modes currently valid are: * `INPUT_MODE_POS_FILTER` * `INPUT_MODE_MIX_CHANNELS` * `INPUT_MODE_TRAP_TRAJ` -* `INPUT_MODE_CURRENT_RAMP` +* `INPUT_MODE_TORQUE_RAMP` * `INPUT_MODE_MIRROR` --- @@ -85,11 +85,11 @@ Implementes an online trapezoidal trajectory planner. ### Valid Control Modes: * `CONTROL_MODE_POSITION_CONTROL` -## INPUT_MODE_CURRENT_RAMP -Ramp a current command from the current value to the target value. +## INPUT_MODE_TORQUE_RAMP +Ramp a torque command from the current value to the target value. ### Configuration Values: -* `.controller.config.current_ramp_rate` +* `.controller.config.torque_ramp_rate` ### Valid Inputs: * `input_torque` diff --git a/tools/.vscode/launch.json b/tools/.vscode/launch.json index 69a77ee7..9a36a076 100644 --- a/tools/.vscode/launch.json +++ b/tools/.vscode/launch.json @@ -9,7 +9,7 @@ "type": "python", "request": "launch", "stopOnEntry": true, - "pythonPath": "${config:python.interpreterPath}", + "pythonPath": "${command:python.pythonPath}", "program": "${file}", "cwd": "${workspaceRoot}", "env": {}, From a9b1841092474460f78efe32974234933693d06d Mon Sep 17 00:00:00 2001 From: pjohnson Date: Thu, 18 Jun 2020 13:11:41 -0400 Subject: [PATCH 04/13] Added Motor::max_available_torque(). Use is to correctly determine torque limit for velocity anti-windup. Set default value (0.0) for current_setpoint Units documentation Removed unused variable thermal_torque_lim_ --- Firmware/MotorControl/controller.cpp | 5 ++--- Firmware/MotorControl/motor.cpp | 17 ++++++++++++++++- Firmware/MotorControl/motor.hpp | 4 ++-- 3 files changed, 20 insertions(+), 6 deletions(-) diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 6dbb6878..c15b1cbe 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -319,10 +319,9 @@ bool Controller::update(float* torque_setpoint_output) { torque = limitVel(config_.vel_limit, *vel_estimate_src, vel_gain, torque); } - // Limit max torque to a user defined torque limit. This functions as an acceleration limit. - // The motor object handles current limiting + // Torque limiting bool limited = false; - float Tlim = axis_->motor_.config_.torque_lim; + float Tlim = axis_->motor_.max_available_torque(); if (torque > Tlim) { limited = true; torque = Tlim; diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 2c7ab5f9..cbe32c5d 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -192,6 +192,21 @@ float Motor::effective_current_lim() { return current_lim; } +float Motor::max_available_torque() { + //return the maximum available torque for the motor. + //Note - for ACIM motors, available torque is allowed to be 0. + if (config_.motor_type == Motor::MOTOR_TYPE_ACIM) { + float max_torque = effective_current_lim() * config_.torque_constant * current_control_.acim_rotor_flux; + max_torque = fmin(max_torque, config_.torque_lim); + return max_torque; + } + else { + float max_torque = effective_current_lim() * config_.torque_constant; + max_torque = fmin(max_torque, config_.torque_lim); + return max_torque; + } +} + void Motor::log_timing(TimingLog_t log_idx) { static const uint16_t clocks_per_cnt = (uint16_t)((float)TIM_1_8_CLOCK_HZ / (float)TIM_APB1_CLOCK_HZ); uint16_t timing = clocks_per_cnt * htim13.Instance->CNT; // TODO: Use a hw_config @@ -441,7 +456,7 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_pha bool Motor::update(float torque_setpoint, float phase, float phase_vel) { - float current_setpoint; + float current_setpoint = 0.0f; phase *= config_.direction; phase_vel *= config_.direction; diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 178a99db..e491ef3b 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -45,7 +45,7 @@ public: float resistance_calib_max_voltage = 2.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 - float torque_constant = 1.0f; // to be set by user + float torque_constant = 1.0f; // [Nm/A] for PM motors, [Nm/A^2] for induction motors. To be set by user int32_t direction = 0; // 1 or -1 (0 = unspecified) MotorType motor_type = MOTOR_TYPE_HIGH_CURRENT; // Read out max_allowed_current to see max supported value for current_lim. @@ -96,6 +96,7 @@ public: float get_inverter_temp(); bool update_thermal_limits(float fet_temp); float effective_current_lim(); + float max_available_torque(); void log_timing(TimingLog_t log_idx); float phase_current_from_adcval(uint32_t ADCValue); bool measure_phase_resistance(float test_current, float max_voltage); @@ -162,7 +163,6 @@ public: } gate_driver_exported_; DRV_SPI_8301_Vars_t gate_driver_regs_; //Local view of DRV registers (initialized by DRV8301_setup) float thermal_current_lim_ = 10.0f; //[A] - float thermal_torque_lim_ = 10.0f; //[Nm] float inverter_temp_ = NAN; // [°C] NaN while the ODrive is initializing. }; From 63f7c09bc2d6cf41e3396e2e9459f1d38d9aa644 Mon Sep 17 00:00:00 2001 From: pjohnson Date: Thu, 18 Jun 2020 15:13:58 -0400 Subject: [PATCH 05/13] Clamped max_torque to 0 for ACIM motors in case max_torque happened to be negative. --- Firmware/MotorControl/motor.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index cbe32c5d..326d41e7 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -192,17 +192,18 @@ float Motor::effective_current_lim() { return current_lim; } +//return the maximum available torque for the motor. +//Note - for ACIM motors, available torque is allowed to be 0. float Motor::max_available_torque() { - //return the maximum available torque for the motor. - //Note - for ACIM motors, available torque is allowed to be 0. if (config_.motor_type == Motor::MOTOR_TYPE_ACIM) { float max_torque = effective_current_lim() * config_.torque_constant * current_control_.acim_rotor_flux; - max_torque = fmin(max_torque, config_.torque_lim); + max_torque = std::clamp(max_torque, 0.0f, max_torque); + max_torque = std::min(max_torque, config_.torque_lim); return max_torque; } else { float max_torque = effective_current_lim() * config_.torque_constant; - max_torque = fmin(max_torque, config_.torque_lim); + max_torque = std::min(max_torque, config_.torque_lim); return max_torque; } } From 758989e2b4d85578a28357523cf923b45cafaf7a Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Thu, 18 Jun 2020 12:58:10 -0700 Subject: [PATCH 06/13] Cleaner clamping --- Firmware/MotorControl/motor.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 326d41e7..d74fbcec 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -197,13 +197,12 @@ float Motor::effective_current_lim() { float Motor::max_available_torque() { if (config_.motor_type == Motor::MOTOR_TYPE_ACIM) { float max_torque = effective_current_lim() * config_.torque_constant * current_control_.acim_rotor_flux; - max_torque = std::clamp(max_torque, 0.0f, max_torque); - max_torque = std::min(max_torque, config_.torque_lim); + max_torque = std::clamp(max_torque, 0.0f, config_.torque_lim); return max_torque; } else { float max_torque = effective_current_lim() * config_.torque_constant; - max_torque = std::min(max_torque, config_.torque_lim); + max_torque = std::clamp(max_torque, 0.0f, config_.torque_lim); return max_torque; } } From f19f57779bdcb2267a3ec1925c9d70230ee92b6e Mon Sep 17 00:00:00 2001 From: PAJohnson Date: Thu, 18 Jun 2020 18:27:19 -0400 Subject: [PATCH 07/13] Changed vel_gain, vel_integrator_gain and torque_constant to work out of the box with ODrive branded motors. Removed unused variable torque_lim_margin Changed default value of torque_lim to +inf --- Firmware/MotorControl/controller.hpp | 6 +++--- Firmware/MotorControl/motor.hpp | 9 ++++----- Firmware/odrive-interface.yaml | 1 - 3 files changed, 7 insertions(+), 9 deletions(-) diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 851f265c..4fcc8a52 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -22,13 +22,13 @@ public: ControlMode control_mode = CONTROL_MODE_POSITION_CONTROL; //see: ControlMode_t InputMode input_mode = INPUT_MODE_PASSTHROUGH; //see: InputMode_t float pos_gain = 20.0f; // [(counts/s) / counts] - float vel_gain = 5.0f / 10000.0f; // [A/(counts/s)] + float vel_gain = 0.2f / 10000.0f; // [Nm/(counts/s)] // float vel_gain = 5.0f / 200.0f, // [A/(rad/s)] - float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)] + float vel_integrator_gain = 0.4f / 10000.0f; // [Nm/(counts/s * s)] float vel_limit = 20000.0f; // [counts/s] Infinity to disable. float vel_limit_tolerance = 1.2f; // ratio to vel_lim. Infinity to disable. float vel_ramp_rate = 10000.0f; // [(counts/s) / s] - float torque_ramp_rate = 0.1f; // Nm / sec + float torque_ramp_rate = 0.01f; // Nm / sec bool setpoints_in_cpr = false; float inertia = 0.0f; // [A/(count/s^2)] float input_filter_bandwidth = 2.0f; // [1/s] diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index e491ef3b..8583e0ef 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -45,15 +45,14 @@ public: float resistance_calib_max_voltage = 2.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 - float torque_constant = 1.0f; // [Nm/A] for PM motors, [Nm/A^2] for induction motors. To be set by user + float torque_constant = 0.04f; // [Nm/A] for PM motors, [Nm/A^2] for induction motors. Equal to 8.27/Kv of the motor int32_t direction = 0; // 1 or -1 (0 = unspecified) MotorType 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] - float current_lim_margin = 8.0f; // Maximum violation of current_lim - float torque_lim = 10.0f; //[Nm] - float torque_lim_margin = 8.0f; + float current_lim = 10.0f; //[A] + float current_lim_margin = 8.0f; // Maximum violation of current_lim + float torque_lim = std::numeric_limits::infinity(); //[Nm]. // Value used to compute shunt amplifier gains float requested_current_range = 60.0f; // [A] float current_control_bandwidth = 1000.0f; // [rad/s] diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index 38e8a936..204e51ad 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -433,7 +433,6 @@ interfaces: current_lim: float32 current_lim_margin: float32 torque_lim: float32 - torque_lim_margin: float32 inverter_temp_limit_lower: float32 inverter_temp_limit_upper: float32 requested_current_range: float32 From e5ace9ecc831240757deb2e870e47793684ab92b Mon Sep 17 00:00:00 2001 From: PAJohnson Date: Thu, 18 Jun 2020 18:32:24 -0400 Subject: [PATCH 08/13] Changed default values of vel_gain, vel_integrator_gain and torque_constant to work with ODrive branded motors. Changed units in comments from A to Nm where appropriate. --- Firmware/MotorControl/controller.hpp | 6 +++--- Firmware/MotorControl/motor.hpp | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 4fcc8a52..0b692450 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -23,12 +23,12 @@ public: InputMode input_mode = INPUT_MODE_PASSTHROUGH; //see: InputMode_t float pos_gain = 20.0f; // [(counts/s) / counts] float vel_gain = 0.2f / 10000.0f; // [Nm/(counts/s)] - // float vel_gain = 5.0f / 200.0f, // [A/(rad/s)] - float vel_integrator_gain = 0.4f / 10000.0f; // [Nm/(counts/s * s)] + // float vel_gain = 0.2f / 200.0f, // [Nm/(rad/s)] + float vel_integrator_gain = 0.4f / 10000.0f; // [Nm/(counts/s * s)] float vel_limit = 20000.0f; // [counts/s] Infinity to disable. float vel_limit_tolerance = 1.2f; // ratio to vel_lim. Infinity to disable. float vel_ramp_rate = 10000.0f; // [(counts/s) / s] - float torque_ramp_rate = 0.01f; // Nm / sec + float torque_ramp_rate = 0.01f; // Nm / sec bool setpoints_in_cpr = false; float inertia = 0.0f; // [A/(count/s^2)] float input_filter_bandwidth = 2.0f; // [1/s] diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 8583e0ef..fbf14af9 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -35,8 +35,8 @@ public: float async_phase_offset; // [rad electrical] }; - // NOTE: for gimbal motors, all units of A are instead V. - // example: vel_gain is [V/(count/s)] instead of [A/(count/s)] + // NOTE: for gimbal motors, all units of Nm are instead V. + // example: vel_gain is [V/(count/s)] instead of [Nm/(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 From cc87e4e6a8d11b96a9e758371c0e4f8c57027628 Mon Sep 17 00:00:00 2001 From: PAJohnson Date: Tue, 23 Jun 2020 00:34:33 +0100 Subject: [PATCH 09/13] Raised current limit from 10A to 15A for regen test. Previously, a motor overcurrent error was triggered ~50% of the time during the braking test. --- tools/odrive/tests/closed_loop_test.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/tools/odrive/tests/closed_loop_test.py b/tools/odrive/tests/closed_loop_test.py index 631e3b17..dcc9b962 100644 --- a/tools/odrive/tests/closed_loop_test.py +++ b/tools/odrive/tests/closed_loop_test.py @@ -172,15 +172,17 @@ class TestRegenProtection(TestClosedLoopControlBase): def run_test(self, axis_ctx: ODriveAxisComponent, motor_ctx: MotorComponent, enc_ctx: EncoderComponent, logger: Logger): with self.prepare(axis_ctx, motor_ctx, enc_ctx, logger): - nominal_rps = 6.0 + nominal_rps = 10.0 nominal_vel = float(enc_ctx.yaml['cpr']) * nominal_rps + max_current = 15.0 # Accept a bit of noise on Ibus - axis_ctx.parent.handle.config.dc_max_negative_current = -0.1 + axis_ctx.parent.handle.config.dc_max_negative_current = -0.2 logger.debug(f'Brake control test from {nominal_rps} rounds/s...') - axis_ctx.handle.controller.config.vel_limit = float(enc_ctx.yaml['cpr']) * 10.0 # max 10 rps + axis_ctx.handle.controller.config.vel_limit = float(enc_ctx.yaml['cpr']) * 15.0 # max 15 rps + axis_ctx.handle.motor.config.current_lim = max_current axis_ctx.handle.controller.config.control_mode = CONTROL_MODE_VELOCITY_CONTROL axis_ctx.handle.controller.config.input_mode = INPUT_MODE_PASSTHROUGH From 7c33fa5d27274d3ec48b86506768da1800298b7a Mon Sep 17 00:00:00 2001 From: PAJohnson Date: Tue, 23 Jun 2020 01:23:11 +0100 Subject: [PATCH 10/13] Changed docs to reflect removal of current control mode and addition of torque control mode Regenerated enums changed can_test.py to reflect control mode change modified closed_loop_test.py to pass with A->Nm change. --- docs/commands.md | 2 +- docs/getting-started.md | 2 +- docs/input_modes.md | 4 +-- tools/odrive/enums.py | 4 +-- tools/odrive/tests/can_test.py | 6 ++--- tools/odrive/tests/closed_loop_test.py | 34 ++++++++++++++------------ 6 files changed, 27 insertions(+), 25 deletions(-) diff --git a/docs/commands.md b/docs/commands.md index 618ddaba..c647adfe 100644 --- a/docs/commands.md +++ b/docs/commands.md @@ -64,7 +64,7 @@ If you want a different mode, you can change `.controller.config.control_m Possible values are: * `CONTROL_MODE_POSITION_CONTROL` * `CONTROL_MODE_VELOCITY_CONTROL` -* `CONTROL_MODE_CURRENT_CONTROL` +* `CONTROL_MODE_TORQUE_CONTROL` * `CONTROL_MODE_VOLTAGE_CONTROL` - this one is not normally used. ### Input Mode diff --git a/docs/getting-started.md b/docs/getting-started.md index 9426f395..46b5af18 100644 --- a/docs/getting-started.md +++ b/docs/getting-started.md @@ -347,7 +347,7 @@ Activate the ramped velocity mode: `axis.controller.config.input_mode = INPUT_MO You can now control the velocity with `axis.controller.input_vel = 5000` [count/s]. ### Torque control -Set `axis.controller.config.control_mode = CONTROL_MODE_CURRENT_CONTROL`.
+Set `axis.controller.config.control_mode = CONTROL_MODE_TORQUE_CONTROL`.
You can now control the torque with `axis.controller.input_torque = 0.1` [Nm]. Note: If you exceed `vel_limit` in current control mode, the current is reduced. To disable this, set `axis.controller.enable_current_mode_vel_limit = False`. diff --git a/docs/input_modes.md b/docs/input_modes.md index 53463ad0..9a87bcee 100644 --- a/docs/input_modes.md +++ b/docs/input_modes.md @@ -31,7 +31,7 @@ Pass `input_xxx` through to `xxx_setpoint` directly. ### Valid Control modes: * `CONTROL_MODE_VOLTAGE_CONTROL` -* `CONTROL_MODE_CURRENT_CONTROL` +* `CONTROL_MODE_TORQUE_CONTROL` * `CONTROL_MODE_VELOCITY_CONTROL` * `CONTROL_MODE_POSITION_CONTROL` @@ -95,7 +95,7 @@ Ramp a torque command from the current value to the target value. * `input_torque` ### Valid Control Modes: -* `CONTROL_MODE_CURRENT_CONTROL` +* `CONTROL_MODE_TORQUE_CONTROL` ## INPUT_MODE_MIRROR Implements "electronic mirroring". This is like electronic camming, but you can only mirror exactly the movements of the other motor, according to a fixed ratio diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index 2fe32b24..8fccc74d 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -30,7 +30,7 @@ ENCODER_MODE_SPI_ABS_AEAT = 258 # ODrive.Controller.ControlMode CONTROL_MODE_VOLTAGE_CONTROL = 0 -CONTROL_MODE_CURRENT_CONTROL = 1 +CONTROL_MODE_TORQUE_CONTROL = 1 CONTROL_MODE_VELOCITY_CONTROL = 2 CONTROL_MODE_POSITION_CONTROL = 3 @@ -41,7 +41,7 @@ INPUT_MODE_VEL_RAMP = 2 INPUT_MODE_POS_FILTER = 3 INPUT_MODE_MIX_CHANNELS = 4 INPUT_MODE_TRAP_TRAJ = 5 -INPUT_MODE_CURRENT_RAMP = 6 +INPUT_MODE_TORQUE_RAMP = 6 INPUT_MODE_MIRROR = 7 # ODrive.Motor.MotorType diff --git a/tools/odrive/tests/can_test.py b/tools/odrive/tests/can_test.py index 8e8e9c4b..93e72210 100644 --- a/tools/odrive/tests/can_test.py +++ b/tools/odrive/tests/can_test.py @@ -187,10 +187,10 @@ class TestSimpleCAN(): test_assert_eq(axis.controller.input_vel, -10.0, range=0.01) test_assert_eq(axis.controller.input_torque, 30.1234, range=0.01) - axis.controller.config.control_mode = CONTROL_MODE_CURRENT_CONTROL - my_cmd('set_input_torque', input_torque=3.1415) + axis.controller.config.control_mode = CONTROL_MODE_TORQUE_CONTROL + my_cmd('set_input_torque', input_torque=0.1) fence() - test_assert_eq(axis.controller.input_torque, 3.1415, range=0.01) + test_assert_eq(axis.controller.input_torque, 0.1, range=0.01) my_cmd('set_velocity_limit', velocity_limit=23456.78) fence() diff --git a/tools/odrive/tests/closed_loop_test.py b/tools/odrive/tests/closed_loop_test.py index dcc9b962..d900e6a4 100644 --- a/tools/odrive/tests/closed_loop_test.py +++ b/tools/odrive/tests/closed_loop_test.py @@ -211,9 +211,9 @@ class TestRegenProtection(TestClosedLoopControlBase): test_assert_eq(axis_ctx.handle.motor.error, MOTOR_ERROR_DC_BUS_OVER_REGEN_CURRENT) -class TestVelLimitInCurrentControl(TestClosedLoopControlBase): +class TestVelLimitInTorqueControl(TestClosedLoopControlBase): """ - Ensures that the current setpoint in current control is always within the + Ensures that the current setpoint in torque control is always within the parallelogram that arises from -Ilim, +Ilim, vel_limit and vel_gain. """ @@ -222,7 +222,8 @@ class TestVelLimitInCurrentControl(TestClosedLoopControlBase): max_rps = 20.0 max_vel = float(enc_ctx.yaml['cpr']) * max_rps absolute_max_vel = max_vel * 1.2 - max_current = 10.0 + max_current = 15.0 + torque_constant = 0.0305 #correct for 5065 motor axis_ctx.handle.controller.config.vel_gain /= 10 # reduce the slope to make it easier to see what's going on vel_gain = axis_ctx.handle.controller.config.vel_gain @@ -231,11 +232,12 @@ class TestVelLimitInCurrentControl(TestClosedLoopControlBase): axis_ctx.handle.controller.config.vel_limit = max_vel axis_ctx.handle.controller.config.vel_limit_tolerance = inf # disable hard limit on velocity axis_ctx.handle.motor.config.current_lim = max_current - axis_ctx.handle.controller.config.control_mode = CONTROL_MODE_CURRENT_CONTROL + axis_ctx.handle.motor.config.torque_constant = torque_constant + axis_ctx.handle.controller.config.control_mode = CONTROL_MODE_TORQUE_CONTROL # Returns the expected limited setpoint for a given velocity and current def get_expected_setpoint(input_setpoint, velocity): - return clamp(clamp(input_setpoint, (velocity + max_vel) * -vel_gain, (velocity - max_vel) * -vel_gain), -max_current, max_current) + return clamp(clamp(input_setpoint / torque_constant, (velocity + max_vel) * -vel_gain / torque_constant, (velocity - max_vel) * -vel_gain / torque_constant), -max_current, max_current) def data_getter(): # sample velocity twice to avoid systematic bias @@ -252,13 +254,13 @@ class TestVelLimitInCurrentControl(TestClosedLoopControlBase): request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) # Move the system around its operating envelope - axis_ctx.handle.controller.input_torque = input_torque = 2.0 + axis_ctx.handle.controller.input_torque = input_torque = 2.0 * torque_constant dataA = record_log(data_getter, duration=1.0) - axis_ctx.handle.controller.input_torque = input_torque = -2.0 + axis_ctx.handle.controller.input_torque = input_torque = -2.0 * torque_constant dataA = np.concatenate([dataA, record_log(data_getter, duration=1.0)]) - axis_ctx.handle.controller.input_torque = input_torque = 4.0 + axis_ctx.handle.controller.input_torque = input_torque = 4.0 * torque_constant dataA = np.concatenate([dataA, record_log(data_getter, duration=1.0)]) - axis_ctx.handle.controller.input_torque = input_torque = -4.0 + axis_ctx.handle.controller.input_torque = input_torque = -4.0 * torque_constant dataA = np.concatenate([dataA, record_log(data_getter, duration=1.0)]) # Shrink the operating envelope while motor is moving faster than the envelope allows @@ -267,22 +269,22 @@ class TestVelLimitInCurrentControl(TestClosedLoopControlBase): axis_ctx.handle.controller.config.vel_limit = max_vel # Move the system around its operating envelope - axis_ctx.handle.controller.input_torque = input_torque = 2.0 + axis_ctx.handle.controller.input_torque = input_torque = 2.0 * torque_constant dataB = record_log(data_getter, duration=1.0) - axis_ctx.handle.controller.input_torque = input_torque = -2.0 + axis_ctx.handle.controller.input_torque = input_torque = -2.0 * torque_constant dataB = np.concatenate([dataB, record_log(data_getter, duration=1.0)]) - axis_ctx.handle.controller.input_torque = input_torque = 4.0 + axis_ctx.handle.controller.input_torque = input_torque = 4.0 * torque_constant dataB = np.concatenate([dataB, record_log(data_getter, duration=1.0)]) - axis_ctx.handle.controller.input_torque = input_torque = -4.0 + axis_ctx.handle.controller.input_torque = input_torque = -4.0 * torque_constant dataB = np.concatenate([dataB, record_log(data_getter, duration=1.0)]) # Try the shrink maneuver again at positive velocity axis_ctx.handle.controller.config.vel_limit = 20.0 * float(enc_ctx.yaml['cpr']) - axis_ctx.handle.controller.input_torque = 4.0 + axis_ctx.handle.controller.input_torque = 4.0 * torque_constant time.sleep(0.5) axis_ctx.handle.controller.config.vel_limit = max_vel - axis_ctx.handle.controller.input_torque = input_torque = 2.0 + axis_ctx.handle.controller.input_torque = input_torque = 2.0 * torque_constant dataB = np.concatenate([dataB, record_log(data_getter, duration=1.0)]) test_assert_no_error(axis_ctx) @@ -298,5 +300,5 @@ if __name__ == '__main__': test_runner.run([ TestClosedLoopControl(), TestRegenProtection(), - TestVelLimitInCurrentControl() + TestVelLimitInTorqueControl() ]) From fa1cf2553fa5611fcbbd2ede5d58538303ab20fc Mon Sep 17 00:00:00 2001 From: PAJohnson Date: Tue, 23 Jun 2020 03:46:45 +0100 Subject: [PATCH 11/13] Added test for torque limit - TestTorqueLimit() --- tools/odrive/tests/closed_loop_test.py | 84 +++++++++++++++++++++++++- 1 file changed, 83 insertions(+), 1 deletion(-) diff --git a/tools/odrive/tests/closed_loop_test.py b/tools/odrive/tests/closed_loop_test.py index d900e6a4..31adce5d 100644 --- a/tools/odrive/tests/closed_loop_test.py +++ b/tools/odrive/tests/closed_loop_test.py @@ -294,11 +294,93 @@ class TestVelLimitInTorqueControl(TestClosedLoopControlBase): test_curve_fit(dataA[:,(0,3)], dataA[:,4], max_mean_err=0.02, inlier_range=0.05, max_outliers=len(dataA[:,0]*0.01)) test_curve_fit(dataB[:,(0,3)], dataB[:,4], max_mean_err=0.1, inlier_range=0.2, max_outliers=len(dataB[:,0])*0.01) +class TestTorqueLimit(TestClosedLoopControlBase): + """ + Checks that the torque limit is respected in position, velocity, and torque control modes + """ + def run_test(self, axis_ctx: ODriveAxisComponent, motor_ctx: MotorComponent, enc_ctx: EncoderComponent, logger: Logger): + with self.prepare(axis_ctx, motor_ctx, enc_ctx, logger): + max_rps = 15.0 + max_vel = max_rps * float(enc_ctx.yaml['cpr']) + max_current = 30.0 + max_torque = 0.1 # must be less than max_current * torque_constant. + torque_constant = axis_ctx.handle.motor.config.torque_constant + test_pos = 5 * float(enc_ctx.yaml['cpr']) + test_vel = 10 * float(enc_ctx.yaml['cpr']) + test_torque = 0.5 + + axis_ctx.handle.controller.config.vel_limit = max_vel + axis_ctx.handle.motor.config.current_lim = max_current + axis_ctx.handle.motor.config.torque_lim = inf #disable torque limit + axis_ctx.handle.controller.config.control_mode = CONTROL_MODE_VELOCITY_CONTROL + + def data_getter(): + current_setpoint = axis_ctx.handle.motor.current_control.Iq_setpoint + torque_setpoint = current_setpoint * torque_constant + torque_limit = axis_ctx.handle.motor.config.torque_lim + # Abort immediately if the absolute limits are exceeded + test_assert_within(current_setpoint, -max_current, max_current) + test_assert_within(torque_setpoint, -torque_limit, torque_limit) + return max_current, current_setpoint, torque_limit, torque_setpoint + + # begin test + axis_ctx.handle.motor.config.torque_lim = max_torque + request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) + + # step input positions + logger.debug('input_pos step test') + axis_ctx.handle.controller.config.control_mode = CONTROL_MODE_POSITION_CONTROL + axis_ctx.handle.controller.input_pos = test_pos + dataPos = record_log(data_getter, duration=1.0) + axis_ctx.handle.controller.input_pos = -test_pos + dataPos = np.concatenate([dataPos, record_log(data_getter, duration=1.0)]) + axis_ctx.handle.controller.input_pos = test_pos + dataPos = np.concatenate([dataPos, record_log(data_getter, duration=1.0)]) + axis_ctx.handle.controller.input_pos = -test_pos + dataPos = np.concatenate([dataPos, record_log(data_getter, duration=1.0)]) + time.sleep(0.5) + + test_assert_no_error(axis_ctx) + + # step input velocities + logger.debug('input_vel step test') + axis_ctx.handle.controller.config.control_mode = CONTROL_MODE_VELOCITY_CONTROL + axis_ctx.handle.controller.input_vel = test_vel + dataVel = record_log(data_getter, duration=1.0) + axis_ctx.handle.controller.input_vel = -test_vel + dataVel = np.concatenate([dataVel, record_log(data_getter, duration=1.0)]) + axis_ctx.handle.controller.input_vel = test_vel + dataVel = np.concatenate([dataVel, record_log(data_getter, duration=1.0)]) + axis_ctx.handle.controller.input_vel = -test_vel + dataVel = np.concatenate([dataVel, record_log(data_getter, duration=1.0)]) + axis_ctx.handle.controller.input_vel = 0 + time.sleep(0.5) + + # step input torques + logger.debug('input_torque step test') + axis_ctx.handle.controller.config.control_mode = CONTROL_MODE_TORQUE_CONTROL + axis_ctx.handle.controller.input_torque = test_torque + dataTq = record_log(data_getter, duration=1.0) + axis_ctx.handle.controller.input_torque = -test_torque + dataTq = np.concatenate([dataTq, record_log(data_getter, duration=1.0)]) + axis_ctx.handle.controller.input_torque = test_torque + dataTq = np.concatenate([dataTq, record_log(data_getter, duration=1.0)]) + axis_ctx.handle.controller.input_torque = -test_torque + dataTq = np.concatenate([dataTq, record_log(data_getter, duration=1.0)]) + axis_ctx.handle.controller.input_torque = 0 + time.sleep(0.5) + + # did we pass? + + test_assert_no_error(axis_ctx) + + axis_ctx.handle.requested_state=1 if __name__ == '__main__': test_runner.run([ TestClosedLoopControl(), TestRegenProtection(), - TestVelLimitInTorqueControl() + TestVelLimitInTorqueControl(), + TestTorqueLimit() ]) From c63e51f8d7bd4f16e33f8befbe75991e3cda3094 Mon Sep 17 00:00:00 2001 From: PAJohnson Date: Tue, 23 Jun 2020 16:04:00 -0400 Subject: [PATCH 12/13] Changed docs to reflect change from CONTROL_MODE_CURRENT_CONTROL to CONTROL_MODE_TORQUE_CONTROL --- Firmware/odrive-interface.yaml | 18 +++++---------- docs/commands.md | 40 ---------------------------------- 2 files changed, 5 insertions(+), 53 deletions(-) diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index eab23463..7113b71c 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -867,12 +867,8 @@ valuetypes: # Note: these should be sorted from lowest level of control to # highest level of control, to allow "<" style comparisons. VoltageControl: -<<<<<<< HEAD - TorqueControl: -======= doc: this one is not normally used CurrentControl: ->>>>>>> 99507d2ece83127ca556f8fd34994075d517111e VelocityControl: PositionControl: @@ -890,7 +886,7 @@ valuetypes: ### Valid Control modes: * `CONTROL_MODE_VOLTAGE_CONTROL` - * `CONTROL_MODE_CURRENT_CONTROL` + * `CONTROL_MODE_TORQUE_CONTROL` * `CONTROL_MODE_VELOCITY_CONTROL` * `CONTROL_MODE_POSITION_CONTROL` VelRamp: @@ -926,9 +922,6 @@ valuetypes: MixChannels: brief: Not Implemented. TrapTraj: -<<<<<<< HEAD - TorqueRamp: -======= brief: Implementes an online trapezoidal trajectory planner. doc: | ![Trapezoidal Planner Response](../TrapTrajPosVel.PNG) @@ -944,18 +937,17 @@ valuetypes: ### Valid Control Modes: * `CONTROL_MODE_POSITION_CONTROL` - CurrentRamp: - brief: Ramp a current command from the current value to the target value. + TorqueRamp: + brief: Ramp a torque command from the current value to the target value. doc: | ### Configuration Values: - * `config.current_ramp_rate` + * `config.torque_ramp_rate` ### Valid Inputs: * `input_current` ### Valid Control Modes: - * `CONTROL_MODE_CURRENT_CONTROL` ->>>>>>> 99507d2ece83127ca556f8fd34994075d517111e + * `CONTROL_MODE_TORQUE_CONTROL` Mirror: brief: Implements "electronic mirroring". doc: | diff --git a/docs/commands.md b/docs/commands.md index 15bf9cb3..47a31df6 100644 --- a/docs/commands.md +++ b/docs/commands.md @@ -38,59 +38,19 @@ See [here](api/odrive.axis.axisstate) for a description of each state. ### Control Mode The default control mode is position control. If you want a different mode, you can change `.controller.config.control_mode`. -<<<<<<< HEAD -Possible values are: -* `CONTROL_MODE_POSITION_CONTROL` -* `CONTROL_MODE_VELOCITY_CONTROL` -* `CONTROL_MODE_TORQUE_CONTROL` -* `CONTROL_MODE_VOLTAGE_CONTROL` - this one is not normally used. - -### Input Mode -The default input mode is `INPUT_MODE_PASSTHROUGH`. -Modes can be selected by changing `.controller.config.input_mode`. -Possible values are: -* `INPUT_MODE_INACTIVE` -* `INPUT_MODE_PASSTHROUGH` -* `INPUT_MODE_VEL_RAMP` -* `INPUT_MODE_POS_FILTER` -* `INPUT_MODE_MIX_CHANNELS` -* `INPUT_MODE_TRAP_TRAJ` -* `INPUT_MODE_TORQUE_RAMP` -* `INPUT_MODE_MIRROR` - -For more information, see [input_modes](input_modes.md). - -# Control Commands -======= Possible values are listed [here](api/odrive.axis.controller.controlmode). ### Input Mode As of version v0.5.0, ODrive now intercepts the incoming commands and can apply filters to them. The old protocol values `pos_setpoint`, `vel_setpoint`, and `current_setpoint` are still used internally by the closed-loop cascade control, but the user cannot write to them directly. This allows us to condense the number of ways the ODrive accepts motion commands. The new commands are: ->>>>>>> 99507d2ece83127ca556f8fd34994075d517111e * `.controller.input_pos = ` * `.controller.input_vel = ` * `.controller.input_torque = ` -<<<<<<< HEAD -### Input Mode -To modify the way the control command affects the motor, you can use the input mode. The default input mode is pass through. -If you want a different mode, you can change `.controller.config.input_mode`. -Possible values are: -* `INPUT_MODE_INACTIVE` -* `INPUT_MODE_PASSTHROUGH` -* `INPUT_MODE_VEL_RAMP` -* `INPUT_MODE_POS_FILTER` -* `INPUT_MODE_MIX_CHANNELS` -* `INPUT_MODE_TRAP_TRAJ` -* `INPUT_MODE_TORQUE_RAMP` -* `INPUT_MODE_MIRROR` -======= Modes can be selected by changing `.controller.config.input_mode`. The default input mode is `INPUT_MODE_PASSTHROUGH`. Possible values are listed [here](api/odrive.axis.controller.inputmode). ->>>>>>> 99507d2ece83127ca556f8fd34994075d517111e ## System monitoring commands From eb494f409afd8e0feed28f59db55bb6e5b3ac6c1 Mon Sep 17 00:00:00 2001 From: PAJohnson Date: Tue, 23 Jun 2020 16:11:11 -0400 Subject: [PATCH 13/13] Change from CurrentControl in interface yaml to TorqueControl --- Firmware/odrive-interface.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index 7113b71c..b738e0df 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -868,7 +868,7 @@ valuetypes: # highest level of control, to allow "<" style comparisons. VoltageControl: doc: this one is not normally used - CurrentControl: + TorqueControl: VelocityControl: PositionControl: