From c65e7265e7afd1c6e76b33cd940f8a2a53217bcb Mon Sep 17 00:00:00 2001 From: Unknown Date: Mon, 3 Sep 2018 17:13:14 -0400 Subject: [PATCH] Move traj planning config to trap_traj config object --- Firmware/MotorControl/axis.hpp | 3 ++- Firmware/MotorControl/controller.cpp | 8 ++++---- Firmware/MotorControl/controller.hpp | 7 ++----- Firmware/MotorControl/main.cpp | 6 +++++- Firmware/MotorControl/trapTraj.cpp | 20 ++++++++++---------- Firmware/MotorControl/trapTraj.hpp | 23 ++++++++++++++++++++--- 6 files changed, 43 insertions(+), 24 deletions(-) diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index a6b3c746..4eac2c4b 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -190,7 +190,8 @@ public: make_protocol_object("motor", motor_.make_protocol_definitions()), make_protocol_object("controller", controller_.make_protocol_definitions()), make_protocol_object("encoder", encoder_.make_protocol_definitions()), - make_protocol_object("sensorless_estimator", sensorless_estimator_.make_protocol_definitions()) + make_protocol_object("sensorless_estimator", sensorless_estimator_.make_protocol_definitions()), + make_protocol_object("trap_traj", trap_.make_protocol_definitions()) ); } }; diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index c9b65019..89c843f5 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -46,10 +46,10 @@ void Controller::set_current_setpoint(float current_setpoint) { void Controller::move_to_pos(float pos_setpoint) { planned_move_end_time_ = axis_->trap_.planTrapezoidal(pos_setpoint, axis_->encoder_.pos_estimate_, - axis_->encoder_.vel_estimate_, config_.vel_limit, - config_.accel_limit, config_.decel_limit); + axis_->encoder_.vel_estimate_, axis_->trap_.config_.vel_limit, + axis_->trap_.config_.accel_limit, axis_->trap_.config_.decel_limit); config_.control_mode = CTRL_MODE_PLANNED_MOVE_CONTROL; - TrajectoryStep_t myTraj = axis_->trap_.evalTrapTraj(0.0f); + TrapTrajStep_t myTraj = axis_->trap_.evalTrapTraj(0.0f); pos_setpoint_ = myTraj.Y; vel_setpoint_ = myTraj.Yd; // current_setpoint_ = myTraj.Ydd; @@ -105,7 +105,7 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s vel_setpoint_ = 0.0f; current_setpoint_ = 0.0f; } else { - TrajectoryStep_t myTraj = axis_->trap_.evalTrapTraj(time_now - planned_move_timer_); + TrapTrajStep_t myTraj = axis_->trap_.evalTrapTraj(time_now - planned_move_timer_); pos_setpoint_ = myTraj.Y; vel_setpoint_ = myTraj.Yd; // current_setpoint_ = myTraj.Ydd; diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 0736fbfa..b4e904b6 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -22,8 +22,6 @@ struct ControllerConfig_t { // 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] - float accel_limit = 5000.0f; - float decel_limit = 5000.0f; }; class Controller { @@ -92,9 +90,8 @@ public: make_protocol_property("pos_gain", &config_.pos_gain), make_protocol_property("vel_gain", &config_.vel_gain), make_protocol_property("vel_integrator_gain", &config_.vel_integrator_gain), - 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("vel_limit", &config_.vel_limit) + ), make_protocol_function("set_pos_setpoint", *this, &Controller::set_pos_setpoint, "pos_setpoint", "vel_feed_forward", diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 340767a5..815f406e 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -14,6 +14,7 @@ SensorlessEstimator::Config_t sensorless_configs[AXIS_COUNT]; ControllerConfig_t controller_configs[AXIS_COUNT]; MotorConfig_t motor_configs[AXIS_COUNT]; AxisConfig_t axis_configs[AXIS_COUNT]; +TrapTrajConfig_t trap_configs[AXIS_COUNT]; bool user_config_loaded_; SystemStats_t system_stats_ = { 0 }; @@ -26,6 +27,7 @@ typedef Config< SensorlessEstimator::Config_t[AXIS_COUNT], ControllerConfig_t[AXIS_COUNT], MotorConfig_t[AXIS_COUNT], + TrapTrajConfig_t[AXIS_COUNT], AxisConfig_t[AXIS_COUNT]> ConfigFormat; void save_configuration(void) { @@ -35,6 +37,7 @@ void save_configuration(void) { &sensorless_configs, &controller_configs, &motor_configs, + &trap_configs, &axis_configs)) { //printf("saving configuration failed\r\n"); osDelay(5); } else { @@ -51,6 +54,7 @@ void load_configuration(void) { &sensorless_configs, &controller_configs, &motor_configs, + &trap_configs, &axis_configs)) { //If loading failed, restore defaults board_config = BoardConfig_t(); @@ -162,7 +166,7 @@ int odrive_main(void) { Motor *motor = new Motor(hw_configs[i].motor_config, hw_configs[i].gate_driver_config, motor_configs[i]); - TrapezoidalTrajectory *trap = new TrapezoidalTrajectory(); + TrapezoidalTrajectory *trap = new TrapezoidalTrajectory(trap_configs[i]); axes[i] = new Axis(hw_configs[i].axis_config, axis_configs[i], *encoder, *sensorless_estimator, *controller, *motor, *trap); } diff --git a/Firmware/MotorControl/trapTraj.cpp b/Firmware/MotorControl/trapTraj.cpp index 419cb30e..0c5f3f35 100644 --- a/Firmware/MotorControl/trapTraj.cpp +++ b/Firmware/MotorControl/trapTraj.cpp @@ -1,5 +1,5 @@ -#include "odrive_main.h" #include +#include "odrive_main.h" // Standard sign function, implemented to match the Python impelmentation template @@ -10,7 +10,7 @@ int sign(T val) { return (std::signbit(val)) ? -1 : 1; } -TrapezoidalTrajectory::TrapezoidalTrajectory(){}; +TrapezoidalTrajectory::TrapezoidalTrajectory(TrapTrajConfig_t &config) : config_(config) {} float TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi, float Vmax, @@ -40,15 +40,15 @@ float TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, Ar = -1.0f * s * Amax; } - Ta = (Vr - Vi) / Ar; // Acceleration time - Td = (-Vr) / Dr; // Deceleration time + Ta = (Vr - Vi) / Ar; // Acceleration time + Td = (-Vr) / Dr; // Deceleration time // Peak Velocity handling - float dXmin = Ta*(Vr + Vi)/2.0f + Td*Vr/2.0f; + float dXmin = Ta * (Vr + Vi) / 2.0f + Td * Vr / 2.0f; // Short move handling if (fabs(dX) < fabs(dXmin)) { - Vr = s*sqrt((-((Vi*Vi)/Ar)-2.0f*dX)/(1.0f/Dr-1.0f/Ar)); + Vr = s * sqrt((-((Vi * Vi) / Ar) - 2.0f * dX) / (1.0f / Dr - 1.0f / Ar)); Ta = std::max(0.0f, (Vr - Vi) / Ar); Tv = 0; Td = std::max(0.0f, -Vr / Dr); @@ -58,7 +58,7 @@ float TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, } // Populate object's values - + Xf_ = Xf; Xi_ = Xi; Vi_ = Vi; @@ -77,14 +77,14 @@ float TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, return Ta + Tv + Td; } -TrajectoryStep_t TrapezoidalTrajectory::evalTrapTraj(float t) { - TrajectoryStep_t trajStep; +TrapTrajStep_t TrapezoidalTrajectory::evalTrapTraj(float t) { + TrapTrajStep_t trajStep; if (t < 0.0f) { // Initial Conditions trajStep.Y = Xi_; trajStep.Yd = Vi_; trajStep.Ydd = Ar_; } else if (t < Ta_) { // Accelerating - trajStep.Y = (Ar_ * (t * t)/ 2.0f) + (Vi_ * t) + Xi_; + trajStep.Y = (Ar_ * (t * t) / 2.0f) + (Vi_ * t) + Xi_; trajStep.Yd = (Ar_ * t) + Vi_; trajStep.Ydd = Ar_; } else if (t < Ta_ + Tv_) { // Coasting diff --git a/Firmware/MotorControl/trapTraj.hpp b/Firmware/MotorControl/trapTraj.hpp index 570a22aa..bd2ae236 100644 --- a/Firmware/MotorControl/trapTraj.hpp +++ b/Firmware/MotorControl/trapTraj.hpp @@ -1,7 +1,14 @@ #ifndef _TRAP_TRAJ_H #define _TRAP_TRAJ_H -struct TrajectoryStep_t { + +struct TrapTrajConfig_t { + float vel_limit = 20000.0f; + float accel_limit = 5000.0f; + float decel_limit = 5000.0f; +}; + +struct TrapTrajStep_t { float Y; float Yd; float Ydd; @@ -10,16 +17,26 @@ struct TrajectoryStep_t { class TrapezoidalTrajectory { public: Axis* axis_ = nullptr; // set by Axis constructor + TrapTrajConfig_t &config_; - TrapezoidalTrajectory(); + TrapezoidalTrajectory(TrapTrajConfig_t &config); float planTrapezoidal(float Xf, float Xi, float Vi, float Vmax, float Amax, float Dmax); - TrajectoryStep_t evalTrapTraj(float t); + TrapTrajStep_t evalTrapTraj(float t); + + auto make_protocol_definitions(){ + return make_protocol_member_list( + make_protocol_property("vel_limit", &config_.vel_limit), + make_protocol_property("accel_limit", &config_.accel_limit), + make_protocol_property("decel_limit", &config_.decel_limit) + ); + } private: + float yAccel_; float Xi_;