diff --git a/CHANGELOG.md b/CHANGELOG.md index ff881b35..8b4d531a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -4,7 +4,7 @@ Please add a note of your changes below this heading if you make a Pull Request. # Unreleased ## Added -* Trapezoidal Trajectory Planner +* **Trapezoidal Trajectory Planner** # Releases diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 8813aa60..52d4c125 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -182,7 +182,7 @@ bool Axis::run_sensorless_spin_up() { bool Axis::run_sensorless_control_loop() { set_step_dir_enabled(config_.enable_step_dir); run_control_loop([this](){ - if (controller_.config_.control_mode >= CTRL_MODE_POSITION_CONTROL) + if (controller_.config_.control_mode >= Controller::CTRL_MODE_POSITION_CONTROL) return error_ |= ERROR_POS_CTRL_DURING_SENSORLESS, false; // Note that all estimators are updated in the loop prefix in run_control_loop diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index d7740a5e..bf50a15a 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -2,7 +2,7 @@ #include "odrive_main.h" -Controller::Controller(ControllerConfig_t& config) : +Controller::Controller(Config_t& config) : config_(config) {} @@ -45,16 +45,12 @@ void Controller::set_current_setpoint(float current_setpoint) { } void Controller::move_to_pos(float goal_point) { - planned_move_end_time_ = axis_->trap_.planTrapezoidal(goal_point, pos_setpoint_, - vel_setpoint_, axis_->trap_.config_.vel_limit, - axis_->trap_.config_.accel_limit, axis_->trap_.config_.decel_limit); - config_.control_mode = CTRL_MODE_PLANNED_MOVE_CONTROL; - TrapTrajStep_t myTraj = axis_->trap_.evalTrapTraj(0.0f); - pos_setpoint_ = myTraj.Y; - vel_setpoint_ = myTraj.Yd; - current_setpoint_ = myTraj.Ydd * axis_->trap_.config_.cpss_to_A; - - planned_move_timer_ = axis_->loop_counter_ * current_meas_period; + axis_->trap_.planTrapezoidal(goal_point, pos_setpoint_, vel_setpoint_, + axis_->trap_.config_.vel_limit, + axis_->trap_.config_.accel_limit, + axis_->trap_.config_.decel_limit); + traj_start_loop_count_ = axis_->loop_counter_; + config_.control_mode = CTRL_MODE_TRAJECTORY_CONTROL; } void Controller::start_anticogging_calibration() { @@ -97,18 +93,22 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s anticogging_calibration(pos_estimate, vel_estimate); float anticogging_pos = pos_estimate; - // Controlled Move - if (config_.control_mode >= CTRL_MODE_PLANNED_MOVE_CONTROL) { - float time_now = axis_->loop_counter_ * current_meas_period; - if ((time_now - planned_move_timer_) > planned_move_end_time_) { + // 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 already set by trajectory vel_setpoint_ = 0.0f; current_setpoint_ = 0.0f; } else { - TrapTrajStep_t myTraj = axis_->trap_.evalTrapTraj(time_now - planned_move_timer_); - pos_setpoint_ = myTraj.Y; - vel_setpoint_ = myTraj.Yd; - current_setpoint_ = myTraj.Ydd * axis_->trap_.config_.cpss_to_A; + TrapezoidalTrajectory::Step_t traj_step = axis_->trap_.eval(t); + pos_setpoint_ = traj_step.Y; + vel_setpoint_ = traj_step.Yd; + current_setpoint_ = traj_step.Ydd * axis_->trap_.config_.A_per_css; } anticogging_pos = pos_setpoint_; // FF the position setpoint instead of the pos_estimate } diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index dd3f76a5..b5e45d03 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -5,28 +5,28 @@ #error "This file should not be included directly. Include odrive_main.h instead." #endif -// Note: these should be sorted from lowest level of control to -// highest level of control, to allow "<" style comparisons. -typedef enum { - CTRL_MODE_VOLTAGE_CONTROL = 0, - CTRL_MODE_CURRENT_CONTROL = 1, - CTRL_MODE_VELOCITY_CONTROL = 2, - CTRL_MODE_POSITION_CONTROL = 3, - CTRL_MODE_PLANNED_MOVE_CONTROL = 4 -} Motor_control_mode_t; - -struct ControllerConfig_t { - Motor_control_mode_t control_mode = CTRL_MODE_POSITION_CONTROL; //see: Motor_control_mode_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_limit = 20000.0f; // [counts/s] -}; - class Controller { public: - Controller(ControllerConfig_t& config); + // Note: these should be sorted from lowest level of control to + // highest level of control, to allow "<" style comparisons. + enum ControlMode_t{ + 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 + }; + + struct Config_t { + ControlMode_t control_mode = CTRL_MODE_POSITION_CONTROL; //see: Motor_control_mode_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_limit = 20000.0f; // [counts/s] + }; + + Controller(Config_t& config); void reset(); void set_pos_setpoint(float pos_setpoint, float vel_feed_forward, float current_feed_forward); @@ -42,7 +42,7 @@ public: bool update(float pos_estimate, float vel_estimate, float* current_setpoint); - ControllerConfig_t& config_; + Config_t& config_; Axis* axis_ = nullptr; // set by Axis constructor // TODO: anticogging overhaul: @@ -75,8 +75,7 @@ public: float vel_integrator_current_ = 0.0f; // [A] float current_setpoint_ = 0.0f; // [A] - float planned_move_timer_ = 0.0f; - float planned_move_end_time_ = 0.0f; + uint32_t traj_start_loop_count_ = 0; // Communication protocol definitions auto make_protocol_definitions() { @@ -93,14 +92,11 @@ public: make_protocol_property("vel_limit", &config_.vel_limit) ), make_protocol_function("set_pos_setpoint", *this, &Controller::set_pos_setpoint, - "pos_setpoint", - "vel_feed_forward", - "current_feed_forward"), + "pos_setpoint", "vel_feed_forward", "current_feed_forward"), make_protocol_function("set_vel_setpoint", *this, &Controller::set_vel_setpoint, - "vel_setpoint", - "current_feed_forward"), + "vel_setpoint", "current_feed_forward"), make_protocol_function("set_current_setpoint", *this, &Controller::set_current_setpoint, - "current_setpoint"), + "current_setpoint"), make_protocol_function("move_to_pos", *this, &Controller::move_to_pos, "pos_setpoint"), make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration) ); diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 1f9b38cb..d9f14ebc 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -11,10 +11,10 @@ BoardConfig_t board_config; Encoder::Config_t encoder_configs[AXIS_COUNT]; SensorlessEstimator::Config_t sensorless_configs[AXIS_COUNT]; -ControllerConfig_t controller_configs[AXIS_COUNT]; +Controller::Config_t controller_configs[AXIS_COUNT]; MotorConfig_t motor_configs[AXIS_COUNT]; AxisConfig_t axis_configs[AXIS_COUNT]; -TrapTrajConfig_t trap_configs[AXIS_COUNT]; +TrapezoidalTrajectory::Config_t trap_configs[AXIS_COUNT]; bool user_config_loaded_; SystemStats_t system_stats_ = { 0 }; @@ -25,9 +25,9 @@ typedef Config< BoardConfig_t, Encoder::Config_t[AXIS_COUNT], SensorlessEstimator::Config_t[AXIS_COUNT], - ControllerConfig_t[AXIS_COUNT], + Controller::Config_t[AXIS_COUNT], MotorConfig_t[AXIS_COUNT], - TrapTrajConfig_t[AXIS_COUNT], + TrapezoidalTrajectory::Config_t[AXIS_COUNT], AxisConfig_t[AXIS_COUNT]> ConfigFormat; void save_configuration(void) { @@ -61,9 +61,9 @@ void load_configuration(void) { for (size_t i = 0; i < AXIS_COUNT; ++i) { encoder_configs[i] = Encoder::Config_t(); sensorless_configs[i] = SensorlessEstimator::Config_t(); - controller_configs[i] = ControllerConfig_t(); + controller_configs[i] = Controller::Config_t(); motor_configs[i] = MotorConfig_t(); - trap_configs[i] = TrapTrajConfig_t(); + trap_configs[i] = TrapezoidalTrajectory::Config_t(); axis_configs[i] = AxisConfig_t(); } } else { diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index 27be8455..0165c673 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -113,7 +113,6 @@ inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast(~static_c #include #include - #endif // __cplusplus diff --git a/Firmware/MotorControl/trapTraj.cpp b/Firmware/MotorControl/trapTraj.cpp index ab4a1854..f1e41aa5 100644 --- a/Firmware/MotorControl/trapTraj.cpp +++ b/Firmware/MotorControl/trapTraj.cpp @@ -15,7 +15,7 @@ float sign_hard(float val) { // Vmax, Amax, Dmax and jmax Kinematic bounds // Ar, Dr and Vr Reached values of acceleration and velocity -TrapezoidalTrajectory::TrapezoidalTrajectory(TrapTrajConfig_t& config) : config_(config) {} +TrapezoidalTrajectory::TrapezoidalTrajectory(Config_t& config) : config_(config) {} bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi, float Vmax, float Amax, float Dmax) { @@ -63,8 +63,8 @@ bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi, return true; } -TrapTrajStep_t TrapezoidalTrajectory::evalTrapTraj(float t) { - TrapTrajStep_t trajStep; +TrapezoidalTrajectory::Step_t TrapezoidalTrajectory::eval(float t) { + Step_t trajStep; if (t < 0.0f) { // Initial Condition trajStep.Y = Xi_; trajStep.Yd = Vi_; @@ -82,7 +82,7 @@ TrapTrajStep_t TrapezoidalTrajectory::evalTrapTraj(float t) { trajStep.Y = Xf_ + 0.5f*Dr_*SQ(td); trajStep.Yd = Dr_*td; trajStep.Ydd = Dr_; - } else if (t >= Tf_) { // Final Condition + } else if (t >= Tf_) { // Final Condition trajStep.Y = Xf_; trajStep.Yd = 0.0f; trajStep.Ydd = 0.0f; diff --git a/Firmware/MotorControl/trapTraj.hpp b/Firmware/MotorControl/trapTraj.hpp index dec5254d..42dac0ef 100644 --- a/Firmware/MotorControl/trapTraj.hpp +++ b/Firmware/MotorControl/trapTraj.hpp @@ -1,25 +1,24 @@ #ifndef _TRAP_TRAJ_H #define _TRAP_TRAJ_H -struct TrapTrajConfig_t { - float vel_limit = 20000.0f; // [count/s] - float accel_limit = 5000.0f; // [count/s^2] - float decel_limit = 5000.0f; // [count/s^2] - float cpss_to_A = 0.0f; // [A/(count/s^2)] -}; - -struct TrapTrajStep_t { - float Y; - float Yd; - float Ydd; -}; - class TrapezoidalTrajectory { - public: - TrapezoidalTrajectory(TrapTrajConfig_t& config); +public: + struct Config_t { + float vel_limit = 20000.0f; // [count/s] + float accel_limit = 5000.0f; // [count/s^2] + float decel_limit = 5000.0f; // [count/s^2] + float A_per_css = 0.0f; // [A/(count/s^2)] + }; + struct Step_t { + float Y; + float Yd; + float Ydd; + }; + + TrapezoidalTrajectory(Config_t& config); bool planTrapezoidal(float Xf, float Xi, float Vi, float Vmax, float Amax, float Dmax); - TrapTrajStep_t evalTrapTraj(float t); + Step_t eval(float t); auto make_protocol_definitions() { return make_protocol_member_list( @@ -27,13 +26,13 @@ class TrapezoidalTrajectory { make_protocol_property("vel_limit", &config_.vel_limit), make_protocol_property("accel_limit", &config_.accel_limit), make_protocol_property("decel_limit", &config_.decel_limit), - make_protocol_property("cpss_to_A", &config_.cpss_to_A) + make_protocol_property("A_per_css", &config_.A_per_css) ) ); } Axis* axis_ = nullptr; // set by Axis constructor - TrapTrajConfig_t& config_; + Config_t& config_; float Xi_; float Xf_;