Make profile handle goal moving such that the profile overshoots
When the goal is moved such that a profile can't get there without
decelerating past the target and then re-accelerating back up to the
point, handle it correctly.
This is originally from Lee, I just added tests.
Change-Id: I4a9bd6354a3c7d4e97df9fc9141b24530739f9bc
Signed-off-by: Lee Mracek <lee@valkyrierobotics.com>
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/aos/util/trapezoid_profile.cc b/aos/util/trapezoid_profile.cc
index 938ca75..bba7696 100644
--- a/aos/util/trapezoid_profile.cc
+++ b/aos/util/trapezoid_profile.cc
@@ -98,11 +98,14 @@
if (top_velocity > maximum_velocity_) {
acceleration_time_ =
(maximum_velocity_ - output_(1)) / maximum_acceleration_;
- constant_time_ =
- (distance_to_target + (goal_velocity * goal_velocity -
- maximum_velocity_ * maximum_velocity_) /
- (2.0 * maximum_acceleration_)) /
- maximum_velocity_;
+ constant_time_ = ((-0.5 * maximum_acceleration_ * acceleration_time_ *
+ acceleration_time_ -
+ output_(1) * acceleration_time_) +
+ distance_to_target +
+ (goal_velocity * goal_velocity -
+ maximum_velocity_ * maximum_velocity_) /
+ (2.0 * maximum_acceleration_)) /
+ maximum_velocity_;
} else {
acceleration_time_ = (top_velocity - output_(1)) / acceleration_;
}
@@ -114,7 +117,12 @@
acceleration_time_ = 0;
}
- deceleration_time_ = (goal_velocity - top_velocity) / deceleration_;
+ deceleration_time_ =
+ (goal_velocity - ::std::min(top_velocity, maximum_velocity_)) /
+ deceleration_;
+ if (constant_time_ <= 0) constant_time_ = 0;
+ if (deceleration_time_ <= 0) deceleration_time_ = 0;
+ if (acceleration_time_ <= 0) acceleration_time_ = 0;
}
} // namespace aos::util