From 499d33d7886945b5c740e2856e46790a3d0850e9 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Tue, 19 Mar 2019 19:47:52 -0700 Subject: [PATCH] change to explicit pos-writing callback for trajectory updates --- Firmware/MotorControl/controller.cpp | 76 +++++++---------------- Firmware/MotorControl/controller.hpp | 24 +++---- Firmware/communication/ascii_protocol.cpp | 28 +++++---- tools/odrive/enums.py | 2 +- 4 files changed, 50 insertions(+), 80 deletions(-) diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 35181049..741b3fd0 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -24,31 +24,10 @@ void Controller::set_error(Error_t error) { // Command Handling //-------------------------------- -void Controller::set_pos_setpoint(float pos_setpoint, float vel_feed_forward, float current_feed_forward) { - pos_setpoint_ = pos_setpoint; - vel_setpoint_ = vel_feed_forward; - current_setpoint_ = current_feed_forward; - config_.control_mode = CTRL_MODE_POSITION_CONTROL; -#ifdef DEBUG_PRINT - printf("POSITION_CONTROL %6.0f %3.3f %3.3f\n", pos_setpoint, vel_setpoint_, current_setpoint_); -#endif -} - -void Controller::set_vel_setpoint(float vel_setpoint, float current_feed_forward) { - vel_setpoint_ = vel_setpoint; - current_setpoint_ = current_feed_forward; - config_.control_mode = CTRL_MODE_VELOCITY_CONTROL; -#ifdef DEBUG_PRINT - printf("VELOCITY_CONTROL %3.3f %3.3f\n", vel_setpoint_, motor->current_setpoint_); -#endif -} - -void Controller::set_current_setpoint(float current_setpoint) { - current_setpoint_ = current_setpoint; - config_.control_mode = CTRL_MODE_CURRENT_CONTROL; -#ifdef DEBUG_PRINT - printf("CURRENT_CONTROL %3.3f\n", current_setpoint_); -#endif +void Controller::input_pos_updated() { + if (config_.input_mode == INPUT_MODE_TRAP_TRAJ) { + move_to_pos(input_pos_); + } } void Controller::move_to_pos(float goal_point) { @@ -57,7 +36,7 @@ void Controller::move_to_pos(float goal_point) { axis_->trap_.config_.accel_limit, axis_->trap_.config_.decel_limit); traj_start_loop_count_ = axis_->loop_counter_; - config_.control_mode = CTRL_MODE_TRAJECTORY_CONTROL; + trajectory_done_ = false; goal_point_ = goal_point; } @@ -91,11 +70,10 @@ bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) anticogging_.cogging_map[anticogging_.index++] = vel_integrator_current_; } if (anticogging_.index < axis_->encoder_.config_.cpr) { // TODO: remove the dependency on encoder CPR - set_pos_setpoint(anticogging_.index, 0.0f, 0.0f); + pos_setpoint_ = anticogging_.index; return false; } else { anticogging_.index = 0; - set_pos_setpoint(0.0f, 0.0f, 0.0f); // Send the motor home anticogging_.use_anticogging = true; // We're good to go, enable anti-cogging anticogging_.calib_anticogging = false; return true; @@ -149,11 +127,25 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s // // NOT YET IMPLEMENTED // } break; case INPUT_MODE_TRAP_TRAJ: { - static auto last_pos = input_pos_; - if(last_pos != input_pos_){ - last_pos = input_pos_; - move_to_pos(input_pos_); // We should really move the *setpoint* handling here, but this will work for now + // Avoid updating uninitialized trajectory + if (trajectory_done_) + break; + // Note: uint32_t loop count delta is OK across overflow + // Beware of negative deltas, as they will not be well behaved due to uint! + float t = (axis_->loop_counter_ - traj_start_loop_count_) * current_meas_period; + if (t > axis_->trap_.Tf_) { + // Drop into position control mode when done to avoid problems on loop counter delta overflow + config_.control_mode = CTRL_MODE_POSITION_CONTROL; + pos_setpoint_ = input_pos_; + vel_setpoint_ = 0.0f; + current_setpoint_ = 0.0f; + } else { + TrapezoidalTrajectory::Step_t traj_step = axis_->trap_.eval(t); + pos_setpoint_ = traj_step.Y; + vel_setpoint_ = traj_step.Yd; + current_setpoint_ = traj_step.Ydd * config_.inertia; } + anticogging_pos = pos_setpoint_; // FF the position setpoint instead of the pos_estimate } break; default: { set_error(ERROR_INVALID_INPUT_MODE); @@ -161,26 +153,6 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s } } - // Trajectory control - if (config_.control_mode == CTRL_MODE_TRAJECTORY_CONTROL) { - // Note: uint32_t loop count delta is OK across overflow - // Beware of negative deltas, as they will not be well behaved due to uint! - float t = (axis_->loop_counter_ - traj_start_loop_count_) * current_meas_period; - if (t > axis_->trap_.Tf_) { - // Drop into position control mode when done to avoid problems on loop counter delta overflow - config_.control_mode = CTRL_MODE_POSITION_CONTROL; - pos_setpoint_ = input_pos_; - vel_setpoint_ = 0.0f; - current_setpoint_ = 0.0f; - } else { - TrapezoidalTrajectory::Step_t traj_step = axis_->trap_.eval(t); - pos_setpoint_ = traj_step.Y; - vel_setpoint_ = traj_step.Yd; - current_setpoint_ = traj_step.Ydd * config_.inertia; - } - anticogging_pos = pos_setpoint_; // FF the position setpoint instead of the pos_estimate - } - // Position control // TODO Decide if we want to use encoder or pll position here float vel_des = vel_setpoint_; diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 74021c0b..4bc3bae5 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -20,8 +20,7 @@ public: CTRL_MODE_VOLTAGE_CONTROL = 0, CTRL_MODE_CURRENT_CONTROL = 1, CTRL_MODE_VELOCITY_CONTROL = 2, - CTRL_MODE_POSITION_CONTROL = 3, - CTRL_MODE_TRAJECTORY_CONTROL = 4 + CTRL_MODE_POSITION_CONTROL = 3 }; enum InputMode_t{ @@ -35,7 +34,7 @@ public: struct Config_t { ControlMode_t control_mode = CTRL_MODE_POSITION_CONTROL; //see: ControlMode_t - InputMode_t input_mode = INPUT_MODE_INACTIVE; //see: InputMode_t + InputMode_t 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)] @@ -52,9 +51,7 @@ public: void reset(); void set_error(Error_t error); - void set_pos_setpoint(float pos_setpoint, float vel_feed_forward, float current_feed_forward); - void set_vel_setpoint(float vel_setpoint, float current_feed_forward); - void set_current_setpoint(float current_setpoint); + void input_pos_updated(); // Trajectory-Planned control void move_to_pos(float goal_point); @@ -94,7 +91,7 @@ public: }; Error_t error_ = ERROR_NONE; - // variables exposed on protocol + float pos_setpoint_ = 0.0f; float vel_setpoint_ = 0.0f; // float vel_setpoint = 800.0f; @@ -108,19 +105,21 @@ public: float input_filter_ki_ = 0.0f; uint32_t traj_start_loop_count_ = 0; - float goal_point_ = 0.0f; + bool trajectory_done_ = true; // Communication protocol definitions auto make_protocol_definitions() { return make_protocol_member_list( make_protocol_property("error", &error_), - make_protocol_property("input_pos", &input_pos_), + make_protocol_property("input_pos", &input_pos_, + [](void* ctx) { static_cast(ctx)->input_pos_updated(); }, this), make_protocol_property("input_vel", &input_vel_), make_protocol_property("input_current", &input_current_), make_protocol_ro_property("pos_setpoint", &pos_setpoint_), make_protocol_ro_property("vel_setpoint", &vel_setpoint_), make_protocol_ro_property("current_setpoint", ¤t_setpoint_), + make_protocol_ro_property("trajectory_done", &trajectory_done_), make_protocol_property("vel_integrator_current", &vel_integrator_current_), make_protocol_object("config", make_protocol_property("control_mode", &config_.control_mode), @@ -136,13 +135,6 @@ public: make_protocol_property("input_filter_bandwidth", &config_.input_filter_bandwidth, [](void* ctx) { static_cast(ctx)->update_filter_gains(); }, this) ), - make_protocol_function("set_pos_setpoint", *this, &Controller::set_pos_setpoint, - "pos_setpoint", "vel_feed_forward", "current_feed_forward"), - make_protocol_function("set_vel_setpoint", *this, &Controller::set_vel_setpoint, - "vel_setpoint", "current_feed_forward"), - make_protocol_function("set_current_setpoint", *this, &Controller::set_current_setpoint, - "current_setpoint"), - make_protocol_function("move_to_pos", *this, &Controller::move_to_pos, "pos_setpoint"), make_protocol_function("move_incremental", *this, &Controller::move_incremental, "displacement", "from_goal_point"), make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration) ); diff --git a/Firmware/communication/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp index 1e1c9ba4..f16d5dc2 100644 --- a/Firmware/communication/ascii_protocol.cpp +++ b/Firmware/communication/ascii_protocol.cpp @@ -95,12 +95,14 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& } else if (motor_number >= AXIS_COUNT) { respond(response_channel, use_checksum, "invalid motor %u", motor_number); } else { - if (numscan < 3) - vel_feed_forward = 0.0f; - if (numscan < 4) - current_feed_forward = 0.0f; Axis* axis = axes[motor_number]; - axis->controller_.set_pos_setpoint(pos_setpoint, vel_feed_forward, current_feed_forward); + axis->controller_.config_.control_mode = Controller::CTRL_MODE_POSITION_CONTROL; + axis->controller_.input_pos_ = pos_setpoint; + if (numscan >= 3) + axis->controller_.input_vel_ = vel_feed_forward; + if (numscan >= 4) + axis->controller_.input_current_ = current_feed_forward; + axis->controller_.input_pos_updated(); axis->watchdog_feed(); } @@ -114,12 +116,13 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& respond(response_channel, use_checksum, "invalid motor %u", motor_number); } else { Axis* axis = axes[motor_number]; - axis->controller_.pos_setpoint_ = pos_setpoint; + axis->controller_.config_.control_mode = Controller::CTRL_MODE_POSITION_CONTROL; + axis->controller_.input_pos_ = pos_setpoint; if (numscan >= 3) axis->controller_.config_.vel_limit = vel_limit; if (numscan >= 4) axis->motor_.config_.current_lim = current_lim; - + axis->controller_.input_pos_updated(); axis->watchdog_feed(); } @@ -132,10 +135,11 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& } else if (motor_number >= AXIS_COUNT) { respond(response_channel, use_checksum, "invalid motor %u", motor_number); } else { - if (numscan < 3) - current_feed_forward = 0.0f; Axis* axis = axes[motor_number]; - axis->controller_.set_vel_setpoint(vel_setpoint, current_feed_forward); + axis->controller_.config_.control_mode = Controller::CTRL_MODE_VELOCITY_CONTROL; + axis->controller_.input_vel_ = vel_setpoint; + if (numscan >= 3) + axis->controller_.input_current_ = current_feed_forward; axis->watchdog_feed(); } @@ -149,7 +153,8 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& respond(response_channel, use_checksum, "invalid motor %u", motor_number); } else { Axis* axis = axes[motor_number]; - axis->controller_.set_current_setpoint(current_setpoint); + axis->controller_.config_.control_mode = Controller::CTRL_MODE_CURRENT_CONTROL; + axis->controller_.input_current_ = current_setpoint; axis->watchdog_feed(); } @@ -163,6 +168,7 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& respond(response_channel, use_checksum, "invalid motor %u", motor_number); } else { Axis* axis = axes[motor_number]; + axis->controller_.config_.input_mode = Controller::INPUT_MODE_TRAP_TRAJ; axis->controller_.move_to_pos(goal_point); axis->watchdog_feed(); } diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index 329d1396..6b7113d6 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -64,13 +64,13 @@ CTRL_MODE_VOLTAGE_CONTROL = 0 CTRL_MODE_CURRENT_CONTROL = 1 CTRL_MODE_VELOCITY_CONTROL = 2 CTRL_MODE_POSITION_CONTROL = 3 -CTRL_MODE_TRAJECTORY_CONTROL = 4 INPUT_MODE_INACTIVE = 0 INPUT_MODE_PASSTHROUGH = 1 INPUT_MODE_VEL_RAMP = 2 INPUT_MODE_POS_FILTER = 3 INPUT_MODE_MIX_CHANNELS = 4 +INPUT_MODE_TRAP_TRAJ = 5 ENCODER_MODE_INCREMENTAL = 0 ENCODER_MODE_HALL = 1