diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index be15ae3e..172f3dc4 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; }); @@ -306,17 +306,17 @@ 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](){ // 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; @@ -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,16 +356,16 @@ 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 - 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(); @@ -385,16 +385,16 @@ 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 - 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..c15b1cbe 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -13,8 +13,8 @@ Controller::Controller(Config_t& config) : void Controller::reset() { pos_setpoint_ = 0.0f; vel_setpoint_ = 0.0f; - vel_integrator_current_ = 0.0f; - current_setpoint_ = 0.0f; + vel_integrator_torque_ = 0.0f; + torque_setpoint_ = 0.0f; } void Controller::set_error(Error error) { @@ -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; @@ -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_; + 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); - 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,13 @@ 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) 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)]; } float v_err = 0.0f; @@ -304,10 +304,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_torque_; } // Velocity limiting in current mode @@ -316,36 +316,34 @@ 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 + // Torque limiting bool limited = false; - float Ilim = axis_->motor_.effective_current_lim(); - if (Iq > Ilim) { + float Tlim = axis_->motor_.max_available_torque(); + 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) 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; } } - 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..0b692450 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 = 5.0f / 200.0f, // [A/(rad/s)] - float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)] + float vel_gain = 0.2f / 10000.0f; // [Nm/(counts/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 current_ramp_rate = 1.0f; // A / 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] @@ -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 @@ -80,12 +80,12 @@ public: float pos_setpoint_ = 0.0f; float vel_setpoint_ = 0.0f; // float vel_setpoint = 800.0f; - float vel_integrator_current_ = 0.0f; // [A] - float current_setpoint_ = 0.0f; // [A] + 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/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 76cdfa5e..d74fbcec 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -182,7 +182,7 @@ float Motor::effective_current_lim() { float current_lim = config_.current_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); + current_lim = std::min(current_lim, 0.98f*one_by_sqrt3*vbus_voltage); //gimbal motor is voltage control } else { current_lim = std::min(current_lim, axis_->motor_.current_control_.max_allowed_current); } @@ -192,6 +192,21 @@ 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() { + 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, config_.torque_lim); + return max_torque; + } + else { + float max_torque = effective_current_lim() * config_.torque_constant; + max_torque = std::clamp(max_torque, 0.0f, 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 @@ -440,11 +455,19 @@ 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) { - current_setpoint *= config_.direction; +bool Motor::update(float torque_setpoint, float phase, float phase_vel) { + float current_setpoint = 0.0f; 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_current_lim(); float id = std::clamp(current_control_.Id_setpoint, -ilim, ilim); diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 6e449111..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 @@ -45,12 +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 = 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 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] @@ -93,6 +95,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); 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 807e86ca..b738e0df 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -551,10 +551,12 @@ 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 inverter_temp_limit_lower: float32 inverter_temp_limit_upper: float32 requested_current_range: float32 @@ -593,12 +595,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 - current_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 @@ -629,9 +631,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 @@ -866,7 +868,7 @@ valuetypes: # highest level of control, to allow "<" style comparisons. VoltageControl: doc: this one is not normally used - CurrentControl: + TorqueControl: VelocityControl: PositionControl: @@ -884,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: @@ -935,17 +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` + * `CONTROL_MODE_TORQUE_CONTROL` Mirror: brief: Implements "electronic mirroring". doc: | diff --git a/docs/commands.md b/docs/commands.md index 28a184a2..47a31df6 100644 --- a/docs/commands.md +++ b/docs/commands.md @@ -46,7 +46,7 @@ As of version v0.5.0, ODrive now intercepts the incoming commands and can apply * `.controller.input_pos = ` * `.controller.input_vel = ` -* `.controller.input_current = ` +* `.controller.input_torque = ` Modes can be selected by changing `.controller.config.input_mode`. The default input mode is `INPUT_MODE_PASSTHROUGH`. diff --git a/docs/getting-started.md b/docs/getting-started.md index a4a92a04..46b5af18 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 -Set `axis.controller.config.control_mode = CONTROL_MODE_CURRENT_CONTROL`.
-You can now control the current with `axis.controller.input_current = 3` [A]. +### Torque 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/tools/.vscode/launch.json b/tools/.vscode/launch.json index a3b07eff..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.pythonPath}", + "pythonPath": "${command:python.pythonPath}", "program": "${file}", "cwd": "${workspaceRoot}", "env": {}, 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 1012250f..93e72210 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) + 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_current, 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() @@ -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 9a87feaf..31adce5d 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 @@ -209,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. """ @@ -220,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 @@ -229,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 @@ -244,19 +248,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 * torque_constant 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 * torque_constant 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 * torque_constant 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 * 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 @@ -265,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_current = input_current = 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_current = input_current = -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_current = input_current = 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_current = input_current = -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_current = 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_current = input_current = 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) @@ -290,11 +294,93 @@ class TestVelLimitInCurrentControl(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(), - TestVelLimitInCurrentControl() + TestVelLimitInTorqueControl(), + TestTorqueLimit() ]) 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