diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 7d79b3d6..8813aa60 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -11,18 +11,21 @@ Axis::Axis(const AxisHardwareConfig_t& hw_config, Encoder& encoder, SensorlessEstimator& sensorless_estimator, Controller& controller, - Motor& motor) + Motor& motor, + TrapezoidalTrajectory& trap) : hw_config_(hw_config), config_(config), encoder_(encoder), sensorless_estimator_(sensorless_estimator), controller_(controller), - motor_(motor) + motor_(motor), + trap_(trap) { encoder_.axis_ = this; sensorless_estimator_.axis_ = this; controller_.axis_ = this; motor_.axis_ = this; + trap_.axis_ = this; } static void step_cb_wrapper(void* ctx) { diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 111f9e26..a6b3c746 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -65,7 +65,8 @@ public: Encoder& encoder, SensorlessEstimator& sensorless_estimator, Controller& controller, - Motor& motor); + Motor& motor, + TrapezoidalTrajectory& trap); void setup(); void start_thread(); @@ -150,6 +151,7 @@ public: SensorlessEstimator& sensorless_estimator_; Controller& controller_; Motor& motor_; + TrapezoidalTrajectory& trap_; osThreadId thread_id_; volatile bool thread_id_valid_ = false; diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 43bb206b..6e63e27c 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -44,6 +44,19 @@ void Controller::set_current_setpoint(float current_setpoint) { #endif } +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_lim, config_.deccel_lim); + config_.control_mode = CTRL_MODE_PLANNED_MOVE_CONTROL; + TrajectoryStep_t myTraj = axis_->trap_.evalTrapTraj(0.0f); + pos_setpoint_ = myTraj.Y; + vel_setpoint_ = myTraj.Yd; + // current_setpoint_ = myTraj.Ydd; + + planned_move_timer_ = axis_->loop_counter_ * current_meas_period; +} + void Controller::start_anticogging_calibration() { // Ensure the cogging map was correctly allocated earlier and that the motor is capable of calibrating if (anticogging_.cogging_map != NULL && axis_->error_ == Axis::ERROR_NONE) { @@ -82,7 +95,20 @@ bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) bool Controller::update(float pos_estimate, float vel_estimate, float* current_setpoint_output) { // Only runs if anticogging_.calib_anticogging is true; non-blocking anticogging_calibration(pos_estimate, vel_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_) { + config_.control_mode = CTRL_MODE_POSITION_CONTROL; + } else { + TrajectoryStep_t myTraj = axis_->trap_.evalTrapTraj(time_now - planned_move_timer_); + pos_setpoint_ = myTraj.Y; + vel_setpoint_ = myTraj.Yd; + // current_setpoint_ = myTraj.Ydd; + } + } + // 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 f10b6211..89c561b1 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -11,7 +11,8 @@ typedef enum { CTRL_MODE_VOLTAGE_CONTROL = 0, CTRL_MODE_CURRENT_CONTROL = 1, CTRL_MODE_VELOCITY_CONTROL = 2, - CTRL_MODE_POSITION_CONTROL = 3 + CTRL_MODE_POSITION_CONTROL = 3, + CTRL_MODE_PLANNED_MOVE_CONTROL = 4 } Motor_control_mode_t; struct ControllerConfig_t { @@ -21,6 +22,8 @@ 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_lim = 5000.0f; + float deccel_lim = 5000.0f; }; class Controller { @@ -31,6 +34,9 @@ public: 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); + + // Trajectory-Planned control + void move_to_pos(float pos_setpoint); // TODO: make this more similar to other calibration loops void start_anticogging_calibration(); @@ -71,6 +77,9 @@ 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; + // Communication protocol definitions auto make_protocol_definitions() { return make_protocol_member_list( @@ -83,19 +92,20 @@ 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("vel_limit", &config_.vel_limit), + make_protocol_property("accel_lim", &config_.accel_lim), + make_protocol_property("deccel_lim", &config_.deccel_lim)), 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"), - make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration) - ); + "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 18b88433..340767a5 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -162,8 +162,9 @@ 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(); axes[i] = new Axis(hw_configs[i].axis_config, axis_configs[i], - *encoder, *sensorless_estimator, *controller, *motor); + *encoder, *sensorless_estimator, *controller, *motor, *trap); } // Start ADC for temperature measurements and user measurements diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index c55fe333..27be8455 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -109,9 +109,10 @@ inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast(~static_c #include #include #include +#include #include #include -#include + #endif // __cplusplus diff --git a/Firmware/MotorControl/trapTraj.cpp b/Firmware/MotorControl/trapTraj.cpp index bec3cb8e..3ec1fae2 100644 --- a/Firmware/MotorControl/trapTraj.cpp +++ b/Firmware/MotorControl/trapTraj.cpp @@ -10,6 +10,8 @@ int sign(T val) { return (std::signbit(val)) ? -1 : 1; } +TrapezoidalTrajectory::TrapezoidalTrajectory(){}; + float TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi, float Vmax, float Amax, float Dmax) { @@ -75,8 +77,8 @@ float TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, return Ta + Tv + Td; } -TrapezoidalTrajectory::TrajectoryStep_t TrapezoidalTrajectory::evalTrapTraj(float t) { - TrapezoidalTrajectory::TrajectoryStep_t trajStep; +TrajectoryStep_t TrapezoidalTrajectory::evalTrapTraj(float t) { + TrajectoryStep_t trajStep; if (t < 0.0f) { // Initial Conditions trajStep.Y = Xi_; trajStep.Yd = Vi_; diff --git a/Firmware/MotorControl/trapTraj.hpp b/Firmware/MotorControl/trapTraj.hpp index fb5a6c58..570a22aa 100644 --- a/Firmware/MotorControl/trapTraj.hpp +++ b/Firmware/MotorControl/trapTraj.hpp @@ -1,35 +1,39 @@ +#ifndef _TRAP_TRAJ_H +#define _TRAP_TRAJ_H + +struct TrajectoryStep_t { + float Y; + float Yd; + float Ydd; +}; + class TrapezoidalTrajectory { -private: - float yAccel_; - - float Xi_; - float Xf_; - float Vi_; - - float Ar_; - float Dr_; - float Vr_; - - float Ta_; - float Tv_; - float Td_; - float Tav_; - -public: - struct TrajectoryStep_t{ - float Y; - float Yd; - float Ydd; - }; + public: + Axis* axis_ = nullptr; // set by Axis constructor TrapezoidalTrajectory(); - float planTrapezoidal( float Xf, float Xi, - float Vi, float Vmax, - float Amax, float Dmax - ); - + float planTrapezoidal(float Xf, float Xi, + float Vi, float Vmax, + float Amax, float Dmax); + TrajectoryStep_t evalTrapTraj(float t); - ~TrapezoidalTrajectory(); -}; \ No newline at end of file + private: + float yAccel_; + + float Xi_; + float Xf_; + float Vi_; + + float Ar_; + float Dr_; + float Vr_; + + float Ta_; + float Tv_; + float Td_; + float Tav_; +}; + +#endif \ No newline at end of file diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index 78c52c2b..5c573d6a 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -155,8 +155,8 @@ build{ 'MotorControl/encoder.cpp', 'MotorControl/controller.cpp', 'MotorControl/sensorless_estimator.cpp', - 'MotorControl/main.cpp', 'MotorControl/trapTraj.cpp', + 'MotorControl/main.cpp', 'communication/communication.cpp', 'communication/ascii_protocol.cpp', 'communication/interface_uart.cpp',