Make a DurationInSeconds function
Also, run clang-format on all the files I changed because I am too lazy
to figure out how to call clang-format on just the lines I changed.
Change-Id: I071c6f81dced533a0a269f25a258348132a7056a
diff --git a/aos/util/trapezoid_profile.cc b/aos/util/trapezoid_profile.cc
index e58324f..a1e8abb 100644
--- a/aos/util/trapezoid_profile.cc
+++ b/aos/util/trapezoid_profile.cc
@@ -12,21 +12,17 @@
output_.setZero();
}
-void TrapezoidProfile::UpdateVals(double acceleration,
- double delta_time) {
- output_(0) += output_(1) * delta_time +
- 0.5 * acceleration * delta_time * delta_time;
+void TrapezoidProfile::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) {
+const Matrix<double, 2, 1> &TrapezoidProfile::Update(double goal_position,
+ double goal_velocity) {
CalculateTimes(goal_position - output_(0), goal_velocity);
- double next_timestep =
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(timestep_)
- .count();
+ double next_timestep = ::aos::time::DurationInSeconds(timestep_);
if (acceleration_time_ > next_timestep) {
UpdateVals(acceleration_, next_timestep);
@@ -93,26 +89,23 @@
}
// We now know the top velocity we can get to.
- double top_velocity = sqrt((distance_to_target +
- (output_(1) * output_(1)) /
- (2.0 * acceleration_) +
- (goal_velocity * goal_velocity) /
- (2.0 * deceleration_)) /
- (-1.0 / (2.0 * deceleration_) +
- 1.0 / (2.0 * acceleration_)));
+ double top_velocity = sqrt(
+ (distance_to_target + (output_(1) * output_(1)) / (2.0 * acceleration_) +
+ (goal_velocity * goal_velocity) / (2.0 * deceleration_)) /
+ (-1.0 / (2.0 * deceleration_) + 1.0 / (2.0 * acceleration_)));
// If it can go too fast, we now know how long we get to accelerate for and
// how long to go at constant velocity.
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_;
+ 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_;
} else {
- acceleration_time_ = (top_velocity - output_(1)) /
- acceleration_;
+ acceleration_time_ = (top_velocity - output_(1)) / acceleration_;
}
CHECK_GT(top_velocity, -maximum_velocity_);
@@ -122,8 +115,7 @@
acceleration_time_ = 0;
}
- deceleration_time_ = (goal_velocity - top_velocity) /
- deceleration_;
+ deceleration_time_ = (goal_velocity - top_velocity) / deceleration_;
}
} // namespace util