Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-24 01:23:52 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge pull request #426 from PAJohnson/A_to_Nm

Browse Source 
Change motor control input from Amps to Nm.
This commit is contained in:
PAJohnson
2020-06-23 16:23:48 -04:00
committed by GitHub
parent 99507d2ece eb494f409a
commit 8ab640f387
17 changed files with 256 additions and 144 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/axis.cpp
+16 -16
View File
@@ -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(&current_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(&current_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(&current_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(&current_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_;
Firmware/MotorControl/controller.cpp
+34 -36
View File
@@ -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<uint32_t>(config_.anticogging.index++, 0, 3600)] = vel_integrator_current_;
config_.anticogging.cogging_map[std::clamp<uint32_t>(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;
}
Firmware/MotorControl/controller.hpp
+8 -8
View File
@@ -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)] <sensorless example>
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)] <sensorless example>
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; <sensorless example>
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;
Firmware/MotorControl/motor.cpp
+26 -3
View File
@@ -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);
Firmware/MotorControl/motor.hpp
+7 -4
View File
@@ -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<float>::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);
Firmware/communication/ascii_protocol.cpp
+15 -15
View File
@@ -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, &current_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, &current_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, &current_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, &current_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");
Firmware/communication/can_simple.cpp
+6 -6
View File
@@ -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<int32_t>(msg, 0, 32, true);
axis->controller_.input_vel_ = can_getSignal<int16_t>(msg, 32, 16, true, 0.1f, 0);
axis->controller_.input_current_ = can_getSignal<int16_t>(msg, 48, 16, true, 0.01f, 0);
axis->controller_.input_torque_ = can_getSignal<int16_t>(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<int32_t>(msg, 0, 32, true, 0.01f, 0.0f);
axis->controller_.input_current_ = can_getSignal<int16_t>(msg, 32, 16, true, 0.01f, 0.0f);
axis->controller_.input_torque_ = can_getSignal<int16_t>(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<int32_t>(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<int32_t>(msg, 0, 32, true, 0.01f, 0);
}
void CANSimple::set_controller_modes_callback(Axis* axis, can_Message_t& msg) {
Firmware/communication/can_simple.hpp
+2 -2
View File
@@ -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);
Firmware/odrive-interface.yaml
+13 -11
View File
@@ -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: |
docs/commands.md
+1 -1
View File
@@ -46,7 +46,7 @@ As of version v0.5.0, ODrive now intercepts the incoming commands and can apply
* `<axis>.controller.input_pos = <encoder_counts>`
* `<axis>.controller.input_vel = <encoder_counts/s>`
* `<axis>.controller.input_current = <current_in_A>`
* `<axis>.controller.input_torque = <torque in Nm>`
Modes can be selected by changing `<axis>.controller.config.input_mode`.
The default input mode is `INPUT_MODE_PASSTHROUGH`.
docs/getting-started.md
+3 -3
View File
@@ -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`.<br>
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`.<br>
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`.<br>
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`.
tools/.vscode/launch.json
Vendored
+1 -1
View File
@@ -9,7 +9,7 @@
"type": "python",
"request": "launch",
"stopOnEntry": true,
"pythonPath": "${config:python.pythonPath}",
"pythonPath": "${command:python.pythonPath}",
"program": "${file}",
"cwd": "${workspaceRoot}",
"env": {},
tools/odrive/enums.py
+2 -2
View File
@@ -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
tools/odrive/tests/can_test.py
+8 -8
View File
@@ -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
tools/odrive/tests/closed_loop_test.py
+107 -21
View File
@@ -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()
])
tools/odrive/tests/old_tests.py
+1 -1
View File
@@ -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)
tools/odrive/tests/uart_ascii_test.py
+6 -6
View File
@@ -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