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

rewrite cpp side of trapezoidal trajectory

Browse Source
This commit is contained in:
Oskar Weigl
2018-09-24 22:45:04 -07:00
parent 4b6943e401
commit 327ac51abf
8 changed files with 73 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CHANGELOG.md
+1 -1
View File
@@ -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
Firmware/MotorControl/axis.cpp
+1 -1
View File
@@ -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
Firmware/MotorControl/controller.cpp
+19 -19
View File
@@ -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
}
Firmware/MotorControl/controller.hpp
+25 -29
View File
@@ -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)] <sensorless example>
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)] <sensorless example>
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)
);
Firmware/MotorControl/main.cpp
+6 -6
View File
@@ -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 {
Firmware/MotorControl/odrive_main.h
-1
View File
@@ -113,7 +113,6 @@ inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast<ENUMTYPE>(~static_c
#include <axis.hpp>
#include <communication/communication.h>
#endif // __cplusplus
Firmware/MotorControl/trapTraj.cpp
+4 -4
View File
@@ -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;
Firmware/MotorControl/trapTraj.hpp
+17 -18
View File
@@ -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_;