Support decoupling acceleration and deceleration in TrapezoidProfile
For the catapult, we want to be able to accelerate more gently than we
stop it. Rather than try to change the trajectory in the middle to stop
nicely, teach the profile how to solve for the two separately.
Change-Id: I798d937a1f6b41da872b8023a9039136fdc8e04b
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/aos/util/trapezoid_profile.cc b/aos/util/trapezoid_profile.cc
index bba7696..05e79ca 100644
--- a/aos/util/trapezoid_profile.cc
+++ b/aos/util/trapezoid_profile.cc
@@ -1,54 +1,70 @@
#include "aos/util/trapezoid_profile.h"
-#include "aos/logging/logging.h"
-
-using ::Eigen::Matrix;
+#include "glog/logging.h"
namespace aos::util {
-TrapezoidProfile::TrapezoidProfile(::std::chrono::nanoseconds delta_time)
- : maximum_acceleration_(0), maximum_velocity_(0), timestep_(delta_time) {
+AsymmetricTrapezoidProfile::AsymmetricTrapezoidProfile(
+ ::std::chrono::nanoseconds delta_time)
+ : timestep_(delta_time) {
output_.setZero();
}
-void TrapezoidProfile::UpdateVals(double acceleration, double delta_time) {
+void AsymmetricTrapezoidProfile::UpdateVals(double acceleration,
+ double delta_time) {
output_(0) +=
output_(1) * delta_time + 0.5 * acceleration * delta_time * delta_time;
output_(1) += acceleration * delta_time;
}
-const Matrix<double, 2, 1> &TrapezoidProfile::Update(double goal_position,
- double goal_velocity) {
- CalculateTimes(goal_position - output_(0), goal_velocity);
+const Eigen::Matrix<double, 2, 1> &AsymmetricTrapezoidProfile::Update(
+ double goal_position, double goal_velocity) {
+ CalculateTimes(goal_position - output_(0), goal_velocity, output_);
double next_timestep = ::aos::time::DurationInSeconds(timestep_);
+ if (deceleration_reversal_time_ > next_timestep) {
+ UpdateVals(deceleration_reversal_, next_timestep);
+ return output_;
+ }
+
+ UpdateVals(deceleration_reversal_, deceleration_reversal_time_);
+ next_timestep -= deceleration_reversal_time_;
+
if (acceleration_time_ > next_timestep) {
UpdateVals(acceleration_, next_timestep);
- } else {
- UpdateVals(acceleration_, acceleration_time_);
+ return output_;
+ }
- next_timestep -= acceleration_time_;
- if (constant_time_ > next_timestep) {
- UpdateVals(0, next_timestep);
- } else {
- UpdateVals(0, constant_time_);
- next_timestep -= constant_time_;
- if (deceleration_time_ > next_timestep) {
- UpdateVals(deceleration_, next_timestep);
- } else {
- UpdateVals(deceleration_, deceleration_time_);
- next_timestep -= deceleration_time_;
- UpdateVals(0, next_timestep);
- }
+ UpdateVals(acceleration_, acceleration_time_);
+ next_timestep -= acceleration_time_;
+
+ if (constant_time_ > next_timestep) {
+ UpdateVals(0, next_timestep);
+ return output_;
+ }
+
+ UpdateVals(0, constant_time_);
+ next_timestep -= constant_time_;
+ if (deceleration_time_ > next_timestep) {
+ UpdateVals(deceleration_, next_timestep);
+ } else {
+ UpdateVals(deceleration_, deceleration_time_);
+ next_timestep -= deceleration_time_;
+ UpdateVals(0, next_timestep);
+
+ if (next_timestep >= 0 && goal_velocity == 0) {
+ output_(0) = goal_position;
+ output_(1) = goal_velocity;
}
}
return output_;
}
-void TrapezoidProfile::CalculateTimes(double distance_to_target,
- double goal_velocity) {
+void AsymmetricTrapezoidProfile::CalculateTimes(
+ double distance_to_target, double goal_velocity,
+ Eigen::Matrix<double, 2, 1> current) {
if (distance_to_target == 0) {
// We're there. Stop everything.
// TODO(aschuh): Deal with velocity not right.
@@ -57,39 +73,69 @@
constant_time_ = 0;
deceleration_time_ = 0;
deceleration_ = 0;
+ deceleration_reversal_time_ = 0;
+ deceleration_reversal_ = 0;
return;
} else if (distance_to_target < 0) {
// Recurse with everything inverted.
- output_(1) *= -1;
- CalculateTimes(-distance_to_target, -goal_velocity);
- output_(1) *= -1;
+ current(1) *= -1;
+ CalculateTimes(-distance_to_target, -goal_velocity, current);
acceleration_ *= -1;
deceleration_ *= -1;
+ deceleration_reversal_ *= -1;
return;
}
constant_time_ = 0;
acceleration_ = maximum_acceleration_;
+
+ // Calculate the fastest speed we could get going to by the distance to
+ // target. We will have normalized everything out to be a positive distance
+ // by now so we never have to deal with going "backwards".
double maximum_acceleration_velocity =
distance_to_target * 2 * std::abs(acceleration_) +
- output_(1) * output_(1);
- if (maximum_acceleration_velocity > 0) {
- maximum_acceleration_velocity = sqrt(maximum_acceleration_velocity);
- } else {
- maximum_acceleration_velocity = -sqrt(-maximum_acceleration_velocity);
- }
+ current(1) * current(1);
+ CHECK_GE(maximum_acceleration_velocity, 0);
+ maximum_acceleration_velocity = sqrt(maximum_acceleration_velocity);
- // Since we know what we'd have to do if we kept after it to decelerate, we
- // know the sign of the acceleration.
+ // If we could get going faster than the target, we will need to decelerate
+ // after accelerating.
if (maximum_acceleration_velocity > goal_velocity) {
- deceleration_ = -maximum_acceleration_;
+ deceleration_ = -maximum_deceleration_;
} else {
+ // We couldn't get up to speed by the destination. Set our decel to
+ // accelerate to keep accelerating to get up to speed.
+ //
+ // Note: goal_velocity != 0 isn't well tested, use at your own risk.
+ LOG(FATAL) << "Untested";
deceleration_ = maximum_acceleration_;
}
+ // If we are going away from the goal, we will need to change directions.
+ if (current(1) < 0) {
+ deceleration_reversal_time_ = current(1) / deceleration_;
+ deceleration_reversal_ = -deceleration_;
+ } else if ((goal_velocity - current(1)) * (goal_velocity + current(1)) <
+ 2.0 * deceleration_ * distance_to_target) {
+ // Then, can we stop in time if we get after it? If so, we don't need to
+ // decel first before running the profile.
+ deceleration_reversal_time_ = -current(1) / deceleration_;
+ deceleration_reversal_ = deceleration_;
+ } else {
+ // Otherwise, we are all good and don't need to handle reversing.
+ deceleration_reversal_time_ = 0.0;
+ deceleration_reversal_ = 0.0;
+ }
+
+ current(0) += current(1) * deceleration_reversal_time_ +
+ 0.5 * deceleration_reversal_ * deceleration_reversal_time_ *
+ deceleration_reversal_time_;
+ current(1) += deceleration_reversal_ * deceleration_reversal_time_;
+ // OK, now we've compensated for slowing down.
+
// We now know the top velocity we can get to.
- double top_velocity = sqrt(
- (distance_to_target + (output_(1) * output_(1)) / (2.0 * acceleration_) +
+ const double top_velocity = sqrt(
+ (distance_to_target + (current(1) * current(1)) / (2.0 * acceleration_) +
(goal_velocity * goal_velocity) / (2.0 * deceleration_)) /
(-1.0 / (2.0 * deceleration_) + 1.0 / (2.0 * acceleration_)));
@@ -97,22 +143,22 @@
// how long to go at constant velocity.
if (top_velocity > maximum_velocity_) {
acceleration_time_ =
- (maximum_velocity_ - output_(1)) / maximum_acceleration_;
+ (maximum_velocity_ - current(1)) / maximum_acceleration_;
constant_time_ = ((-0.5 * maximum_acceleration_ * acceleration_time_ *
acceleration_time_ -
- output_(1) * acceleration_time_) +
+ current(1) * acceleration_time_) +
distance_to_target +
(goal_velocity * goal_velocity -
maximum_velocity_ * maximum_velocity_) /
- (2.0 * maximum_acceleration_)) /
+ (2.0 * maximum_deceleration_)) /
maximum_velocity_;
} else {
- acceleration_time_ = (top_velocity - output_(1)) / acceleration_;
+ acceleration_time_ = (top_velocity - current(1)) / acceleration_;
}
- AOS_CHECK_GT(top_velocity, -maximum_velocity_);
+ CHECK_GT(top_velocity, -maximum_velocity_);
- if (output_(1) > maximum_velocity_) {
+ if (current(1) > maximum_velocity_) {
constant_time_ = 0;
acceleration_time_ = 0;
}
@@ -120,9 +166,9 @@
deceleration_time_ =
(goal_velocity - ::std::min(top_velocity, maximum_velocity_)) /
deceleration_;
+ if (acceleration_time_ <= 0) acceleration_time_ = 0;
if (constant_time_ <= 0) constant_time_ = 0;
if (deceleration_time_ <= 0) deceleration_time_ = 0;
- if (acceleration_time_ <= 0) acceleration_time_ = 0;
}
} // namespace aos::util
diff --git a/aos/util/trapezoid_profile.h b/aos/util/trapezoid_profile.h
index 6669777..621ff30 100644
--- a/aos/util/trapezoid_profile.h
+++ b/aos/util/trapezoid_profile.h
@@ -9,57 +9,116 @@
namespace aos::util {
// Calculates a trapezoidal motion profile (like for a control loop's goals).
-// Supports having the end speed and position changed in the middle.
+// Supports having the destination position, acceleration, and velocity caps
+// changed in the middle, and for having different accelerations and
+// decelerations.
//
// The only units assumption that this class makes is that the unit of time is
// seconds.
-class TrapezoidProfile {
+class AsymmetricTrapezoidProfile {
public:
- // delta_time is how long between each call to Update.
- TrapezoidProfile(::std::chrono::nanoseconds delta_time);
+ // Constructs a profile. delta_time is the timestep to assume when solving.
+ AsymmetricTrapezoidProfile(::std::chrono::nanoseconds delta_time);
- // Updates the state.
+ // Updates the state to provide the next position and velocity to go to to
+ // follow the profile.
const Eigen::Matrix<double, 2, 1> &Update(double goal_position,
double goal_velocity);
- // Useful for preventing windup etc.
+
+ // Updates the internal position. Useful for handling windup when a loop
+ // saturates or gets disabled.
void MoveCurrentState(const Eigen::Matrix<double, 2, 1> ¤t) {
output_ = current;
}
- // Useful for preventing windup etc.
+ // Adjusts the internal position by the provided position delta.
void MoveGoal(double dx) { output_(0, 0) += dx; }
+ // Sets the internal position to the provided position.
void SetGoal(double x) { output_(0, 0) = x; }
void set_maximum_acceleration(double maximum_acceleration) {
maximum_acceleration_ = maximum_acceleration;
}
+ void set_maximum_deceleration(double maximum_deceleration) {
+ maximum_deceleration_ = maximum_deceleration;
+ }
+
void set_maximum_velocity(double maximum_velocity) {
maximum_velocity_ = maximum_velocity;
}
private:
- // Basic kinematics to update output_, given that we are going to accelerate
- // by acceleration over delta_time.
+ // Updates output_ to match the basic kinematics, given that we are going to
+ // accelerate by acceleration over delta_time.
void UpdateVals(double acceleration, double delta_time);
// Calculates how long to go for each segment.
- void CalculateTimes(double distance_to_target, double goal_velocity);
+ void CalculateTimes(double distance_to_target, double goal_velocity,
+ Eigen::Matrix<double, 2, 1> current);
// output_ is where it should go at time_.
Eigen::Matrix<double, 2, 1> output_;
+ // Time and acceleration to slow down if we need to reverse directions.
+ double deceleration_reversal_time_;
+ double deceleration_reversal_;
+
+ // Time and acceleration to speed up with.
double acceleration_time_;
double acceleration_;
+ // Time to go at max speed at.
double constant_time_;
+ // Time and acceleration to slow down with.
double deceleration_time_;
double deceleration_;
- double maximum_acceleration_;
- double maximum_velocity_;
+ double maximum_acceleration_ = 0;
+ double maximum_deceleration_ = 0;
+ double maximum_velocity_ = 0;
// How long between calls to Update.
::std::chrono::nanoseconds timestep_;
- DISALLOW_COPY_AND_ASSIGN(TrapezoidProfile);
+ DISALLOW_COPY_AND_ASSIGN(AsymmetricTrapezoidProfile);
+};
+
+// Class to implement a AsymmetricTrapezoidProfile where both acceleration and
+// deceleration match.
+class TrapezoidProfile {
+ public:
+ TrapezoidProfile(::std::chrono::nanoseconds delta_time)
+ : asymmetric_trapezoid_profile_(delta_time) {}
+
+ // Updates the state to provide the next position and velocity to go to to
+ // follow the profile.
+ const Eigen::Matrix<double, 2, 1> &Update(double goal_position,
+ double goal_velocity) {
+ return asymmetric_trapezoid_profile_.Update(goal_position, goal_velocity);
+ }
+
+ // Updates the internal position. Useful for handling windup when a loop
+ // saturates or gets disabled.
+ void MoveCurrentState(const Eigen::Matrix<double, 2, 1> ¤t) {
+ asymmetric_trapezoid_profile_.MoveCurrentState(current);
+ }
+
+ // Adjusts the internal position by the provided position delta.
+ void MoveGoal(double dx) { asymmetric_trapezoid_profile_.MoveGoal(dx); }
+
+ // Sets the internal position to the provided position.
+ void SetGoal(double x) { asymmetric_trapezoid_profile_.SetGoal(x); }
+
+ void set_maximum_acceleration(double maximum_acceleration) {
+ asymmetric_trapezoid_profile_.set_maximum_acceleration(
+ maximum_acceleration);
+ asymmetric_trapezoid_profile_.set_maximum_deceleration(
+ maximum_acceleration);
+ }
+ void set_maximum_velocity(double maximum_velocity) {
+ asymmetric_trapezoid_profile_.set_maximum_velocity(maximum_velocity);
+ }
+
+ private:
+ AsymmetricTrapezoidProfile asymmetric_trapezoid_profile_;
};
} // namespace aos::util
diff --git a/aos/util/trapezoid_profile_test.cc b/aos/util/trapezoid_profile_test.cc
index 34dc35c..54ffeb5 100644
--- a/aos/util/trapezoid_profile_test.cc
+++ b/aos/util/trapezoid_profile_test.cc
@@ -1,6 +1,7 @@
#include "aos/util/trapezoid_profile.h"
#include "Eigen/Dense"
+#include "glog/logging.h"
#include "gtest/gtest.h"
namespace aos::util::testing {
@@ -13,6 +14,7 @@
TrapezoidProfileTest() : profile_(kDeltaTime) {
position_.setZero();
profile_.set_maximum_acceleration(0.75);
+ profile_.set_maximum_deceleration(0.75);
profile_.set_maximum_velocity(1.75);
}
@@ -33,7 +35,7 @@
const Eigen::Matrix<double, 2, 1> &position() { return position_; }
- TrapezoidProfile profile_;
+ AsymmetricTrapezoidProfile profile_;
::testing::AssertionResult At(double position, double velocity) {
static const double kDoubleNear = 0.00001;
@@ -59,9 +61,7 @@
constexpr ::std::chrono::nanoseconds TrapezoidProfileTest::kDeltaTime;
TEST_F(TrapezoidProfileTest, ReachesGoal) {
- for (int i = 0; i < 450; ++i) {
- RunIteration(3, 0);
- }
+ RunFor(3, 0, std::chrono::milliseconds(4500));
EXPECT_TRUE(At(3, 0));
}
@@ -69,14 +69,14 @@
// moving faster than the new max is handled correctly.
TEST_F(TrapezoidProfileTest, ContinousUnderVelChange) {
profile_.set_maximum_velocity(1.75);
- RunIteration(12.0, 0);
+ RunFor(12.0, 0, std::chrono::milliseconds(10));
double last_pos = position()(0);
double last_vel = 1.75;
for (int i = 0; i < 1600; ++i) {
if (i == 400) {
profile_.set_maximum_velocity(0.75);
}
- RunIteration(12.0, 0);
+ RunFor(12.0, 0, std::chrono::milliseconds(10));
if (i >= 400) {
EXPECT_TRUE(::std::abs(last_pos - position()(0)) <= 1.75 * 0.01);
EXPECT_NEAR(last_vel, ::std::abs(last_pos - position()(0)), 0.0001);
@@ -89,48 +89,38 @@
// There is some somewhat tricky code for dealing with going backwards.
TEST_F(TrapezoidProfileTest, Backwards) {
- for (int i = 0; i < 400; ++i) {
- RunIteration(-2, 0);
- }
+ RunFor(-2, 0, std::chrono::milliseconds(4000));
EXPECT_TRUE(At(-2, 0));
}
TEST_F(TrapezoidProfileTest, SwitchGoalInMiddle) {
- for (int i = 0; i < 200; ++i) {
- RunIteration(-2, 0);
- }
+ RunFor(-2, 0, std::chrono::milliseconds(2000));
EXPECT_FALSE(At(-2, 0));
- for (int i = 0; i < 550; ++i) {
- RunIteration(0, 0);
- }
+ RunFor(0, 0, std::chrono::milliseconds(5500));
EXPECT_TRUE(At(0, 0));
}
// Checks to make sure that it hits top speed.
TEST_F(TrapezoidProfileTest, TopSpeed) {
- for (int i = 0; i < 200; ++i) {
- RunIteration(4, 0);
- }
+ RunFor(4, 0, std::chrono::milliseconds(2000));
EXPECT_NEAR(1.5, position()(1), 10e-5);
- for (int i = 0; i < 2000; ++i) {
- RunIteration(4, 0);
- }
+ RunFor(4, 0, std::chrono::milliseconds(20000));
EXPECT_TRUE(At(4, 0));
}
// Tests that the position and velocity exactly match at the end. Some code we
// have assumes this to be true as a simplification.
TEST_F(TrapezoidProfileTest, ExactlyReachesGoal) {
- for (int i = 0; i < 450; ++i) {
- RunIteration(1, 0);
- }
+ RunFor(1, 0, std::chrono::milliseconds(4500));
EXPECT_EQ(position()(1), 0.0);
EXPECT_EQ(position()(0), 1.0);
}
-// Tests that we can move a goal without the trajectory teleporting.
+// Tests that we can move a goal without the trajectory teleporting. The goal
+// needs to move to something we haven't already passed, but will blow by.
TEST_F(TrapezoidProfileTest, MoveGoal) {
profile_.set_maximum_acceleration(2.0);
+ profile_.set_maximum_deceleration(2.0);
profile_.set_maximum_velocity(2.0);
RunFor(5.0, 0, std::chrono::seconds(1));
@@ -143,4 +133,168 @@
EXPECT_TRUE(At(3.5, 0.0));
}
+// Tests that we can move a goal back before where we currently are without
+// teleporting.
+TEST_F(TrapezoidProfileTest, MoveGoalFar) {
+ profile_.set_maximum_acceleration(2.0);
+ profile_.set_maximum_deceleration(2.0);
+ profile_.set_maximum_velocity(2.0);
+
+ RunFor(5.0, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(1.0, 2.0));
+ RunFor(5.0, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(3.0, 2.0));
+ RunFor(2.5, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(4.0, 0.0));
+ RunFor(2.5, 0, std::chrono::seconds(2));
+ EXPECT_TRUE(At(2.5, 0.0));
+}
+
+// Tests that we can move a goal without the trajectory teleporting. The goal
+// needs to move to something we haven't already passed, but will blow by. Do
+// this one in the negative direction.
+TEST_F(TrapezoidProfileTest, MoveGoalNegative) {
+ profile_.set_maximum_acceleration(2.0);
+ profile_.set_maximum_deceleration(2.0);
+ profile_.set_maximum_velocity(2.0);
+
+ RunFor(-5.0, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(-1.0, -2.0));
+ RunFor(-5.0, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(-3.0, -2.0));
+ RunFor(-3.5, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(-4.0, 0.0));
+ RunFor(-3.5, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(-3.5, 0.0));
+}
+
+// Tests that we can move a goal back before where we currently are without
+// teleporting. Do this one in the negative direction.
+TEST_F(TrapezoidProfileTest, MoveGoalNegativeFar) {
+ profile_.set_maximum_acceleration(2.0);
+ profile_.set_maximum_deceleration(2.0);
+ profile_.set_maximum_velocity(2.0);
+
+ RunFor(-5.0, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(-1.0, -2.0));
+ RunFor(-5.0, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(-3.0, -2.0));
+ RunFor(-2.5, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(-4.0, 0.0));
+ RunFor(-2.5, 0, std::chrono::seconds(2));
+ EXPECT_TRUE(At(-2.5, 0.0));
+}
+
+// Tests that we can execute a profile with acceleration and deceleration not
+// matching in magnitude.
+TEST_F(TrapezoidProfileTest, AsymmetricAccelDecel) {
+ // Accelerates up until t=1. Will be at x=0.5
+ profile_.set_maximum_acceleration(1.0);
+ // Decelerates in t=0.5 Will take x=0.25
+ profile_.set_maximum_deceleration(2.0);
+ profile_.set_maximum_velocity(1.0);
+
+ RunFor(1.75, 0, std::chrono::seconds(1));
+
+ EXPECT_TRUE(At(0.5, 1.0));
+
+ RunFor(1.75, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(1.5, 1.0));
+ RunFor(1.75, 0, std::chrono::milliseconds(500));
+ EXPECT_TRUE(At(1.75, 0.0));
+}
+
+// Tests that we can execute a profile with acceleration and deceleration not
+// matching in magnitude, and hitting saturation.
+TEST_F(TrapezoidProfileTest, AsymmetricAccelDecelUnconstrained) {
+ // Accelerates up until t=1. Will be at x=0.5
+ profile_.set_maximum_acceleration(1.0);
+ // Decelerates in t=0.5 Will take x=0.25
+ profile_.set_maximum_deceleration(2.0);
+ profile_.set_maximum_velocity(2.0);
+
+ RunFor(0.75, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(0.5, 1.0));
+
+ RunFor(0.75, 0, std::chrono::milliseconds(500));
+ EXPECT_TRUE(At(0.75, 0.0));
+}
+
+// Tests that we can execute a profile with acceleration and deceleration not
+// matching in magnitude, and hitting saturation.
+TEST_F(TrapezoidProfileTest, AsymmetricAccelDecelUnconstrainedNegative) {
+ // Accelerates up until t=1. Will be at x=0.5
+ profile_.set_maximum_acceleration(1.0);
+ // Decelerates in t=0.5 Will take x=0.25
+ profile_.set_maximum_deceleration(2.0);
+ profile_.set_maximum_velocity(2.0);
+
+ RunFor(-0.75, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(-0.5, -1.0));
+
+ RunFor(-0.75, 0, std::chrono::milliseconds(500));
+ EXPECT_TRUE(At(-0.75, 0.0));
+}
+
+// Tests that we can execute a profile with acceleration and deceleration not
+// matching in magnitude when going in the negative direction.
+TEST_F(TrapezoidProfileTest, AsymmetricAccelDecelNegative) {
+ // Accelerates up until t=1. Will be at x=0.5
+ profile_.set_maximum_acceleration(1.0);
+ // Decelerates in t=0.5 Will take x=0.25
+ profile_.set_maximum_deceleration(2.0);
+ profile_.set_maximum_velocity(1.0);
+
+ RunFor(-1.75, 0, std::chrono::seconds(1));
+
+ EXPECT_TRUE(At(-0.5, -1.0));
+
+ RunFor(-1.75, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(-1.5, -1.0));
+ RunFor(-1.75, 0, std::chrono::milliseconds(500));
+ EXPECT_TRUE(At(-1.75, 0.0));
+}
+
+// Tests that we can move the goal when an asymmetric profile is executing.
+TEST_F(TrapezoidProfileTest, AsymmetricAccelDecelMoveGoal) {
+ // Accelerates up until t=1. Will be at x=0.5
+ profile_.set_maximum_acceleration(1.0);
+ // Decelerates in t=0.5 Will take x=0.25
+ profile_.set_maximum_deceleration(2.0);
+ profile_.set_maximum_velocity(1.0);
+
+ RunFor(1.75, 0, std::chrono::seconds(1));
+
+ EXPECT_TRUE(At(0.5, 1.0));
+
+ RunFor(1.75, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(1.5, 1.0));
+ RunFor(1.6, 0, std::chrono::milliseconds(500));
+ EXPECT_TRUE(At(1.75, 0.0));
+ RunFor(1.6, 0, std::chrono::milliseconds(520));
+ RunFor(1.6, 0, std::chrono::milliseconds(2500));
+ EXPECT_TRUE(At(1.6, 0.0));
+}
+
+// Tests that we can move the goal when an asymmetric profile is executing in
+// the negative direction.
+TEST_F(TrapezoidProfileTest, AsymmetricAccelDecelMoveGoalFar) {
+ // Accelerates up until t=1. Will be at x=0.5
+ profile_.set_maximum_acceleration(1.0);
+ // Decelerates in t=0.5 Will take x=0.25
+ profile_.set_maximum_deceleration(2.0);
+ profile_.set_maximum_velocity(1.0);
+
+ RunFor(1.75, 0, std::chrono::seconds(1));
+
+ EXPECT_TRUE(At(0.5, 1.0));
+
+ RunFor(1.75, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(1.5, 1.0));
+ RunFor(1.0, 0, std::chrono::milliseconds(500));
+ EXPECT_TRUE(At(1.75, 0.0));
+ RunFor(1.0, 0, std::chrono::milliseconds(2500));
+ EXPECT_TRUE(At(1.0, 0.0));
+}
+
} // namespace aos::util::testing