Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-08-20 22:14:34 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Add TrapTraj to Axis and Controller. Add move_to_pos funciton

Browse Source
This commit is contained in:
Unknown
2018-09-01 23:39:31 -04:00
parent 4a38ed07a2
commit 03bf297bd8
9 changed files with 98 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/axis.cpp
+5 -2
View File
@@ -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) {
Firmware/MotorControl/axis.hpp
+3 -1
View File
@@ -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;
Firmware/MotorControl/controller.cpp
+27 -1
View File
@@ -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_;
Firmware/MotorControl/controller.hpp
+21 -11
View File
@@ -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)] <sensorless example>
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), );
}
};
Firmware/MotorControl/main.cpp
+2 -1
View File
@@ -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
Firmware/MotorControl/odrive_main.h
+2 -1
View File
@@ -109,9 +109,10 @@ inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast<ENUMTYPE>(~static_c
#include <sensorless_estimator.hpp>
#include <controller.hpp>
#include <motor.hpp>
#include <trapTraj.hpp>
#include <axis.hpp>
#include <communication/communication.h>
#include <trapTraj.hpp>
#endif // __cplusplus
Firmware/MotorControl/trapTraj.cpp
+4 -2
View File
@@ -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_;
Firmware/MotorControl/trapTraj.hpp
+33 -29
View File
@@ -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();
};
private:
float yAccel_;
float Xi_;
float Xf_;
float Vi_;
float Ar_;
float Dr_;
float Vr_;
float Ta_;
float Tv_;
float Td_;
float Tav_;
};
#endif
Firmware/Tupfile.lua
+1 -1
View File
@@ -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',