36 #include <mathlib/mathlib.h> 43 for (
int i = 0; i < 2; i++) {
44 _trajectory[i].reset(accel(i), vel(i), pos(i));
52 _position_lock_active =
false;
53 _position_setpoint_locked(0) = NAN;
54 _position_setpoint_locked(1) = NAN;
60 updateTrajectories(dt);
64 checkPositionLock(velocity_target);
67 updateTrajDurations(velocity_target);
72 for (
int i = 0; i < 2; ++i) {
73 _trajectory[i].updateTraj(dt);
75 _state.j(i) = _trajectory[i].getCurrentJerk();
76 _state.a(i) = _trajectory[i].getCurrentAcceleration();
77 _state.v(i) = _trajectory[i].getCurrentVelocity();
78 _state.x(i) = _trajectory[i].getCurrentPosition();
84 for (
int i = 0; i < 2; ++i) {
85 _trajectory[i].updateDurations(velocity_target(i));
101 if (_state.v.length() < 0.1f &&
102 _state.a.length() < .2f &&
105 _position_lock_active =
true;
106 _position_setpoint_locked = _state.x;
110 if (_position_lock_active) {
112 _trajectory[0].setCurrentVelocity(_velocity_setpoint_feedback(0));
113 _trajectory[1].setCurrentVelocity(_velocity_setpoint_feedback(1));
114 _state.v = _velocity_setpoint_feedback;
118 _trajectory[0].setCurrentPosition(_position_estimate(0));
119 _trajectory[1].setCurrentPosition(_position_estimate(1));
void checkPositionLock(const Vector2f &velocity_target)
void update(float dt, const Vector2f &velocity_target)
void updateTrajDurations(const Vector2f &velocity_target)
static void timeSynchronization(VelocitySmoothing *traj, int n_traj)
Synchronize several trajectories to have the same total time.
void reset(const Vector2f &accel, const Vector2f &vel, const Vector2f &pos)
void updateTrajectories(float dt)