TrapezoidFix:
- Decelerate profile if we are over velocity (ie max velocity changed.)
Final cleanup by Brian.
Change-Id: Iec5a2f5451b7fcb5987e0331e9ea524d8b47f911
diff --git a/aos/common/util/trapezoid_profile.cc b/aos/common/util/trapezoid_profile.cc
index 4aa5285..cd8d02c 100644
--- a/aos/common/util/trapezoid_profile.cc
+++ b/aos/common/util/trapezoid_profile.cc
@@ -117,6 +117,11 @@
CHECK_GT(top_velocity, -maximum_velocity_);
+ if (output_(1) > maximum_velocity_) {
+ constant_time_ = 0;
+ acceleration_time_ = 0;
+ }
+
deceleration_time_ = (goal_velocity - top_velocity) /
deceleration_;
}