added TrapezoidProfile (copied from frc254-2011)
I cleaned it up extensively and added a couple of quick tests.
git-svn-id: https://robotics.mvla.net/svn/frc971/2013/trunk/src@4172 f308d9b7-e957-4cde-b6ac-9a88185e7312
diff --git a/aos/common/util/trapezoid_profile.cc b/aos/common/util/trapezoid_profile.cc
new file mode 100644
index 0000000..9035f89
--- /dev/null
+++ b/aos/common/util/trapezoid_profile.cc
@@ -0,0 +1,125 @@
+#include "aos/common/util/trapezoid_profile.h"
+
+#include <assert.h>
+
+using ::Eigen::Matrix;
+
+namespace aos {
+namespace util {
+
+TrapezoidProfile::TrapezoidProfile(const time::Time &delta_time)
+ : maximum_acceleration_(0),
+ maximum_velocity_(0),
+ timestep_(delta_time) {
+ output_.setZero();
+}
+
+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) {
+ CalculateTimes(goal_position - output_(0), goal_velocity);
+
+ double next_timestep = timestep_.ToSeconds();
+
+ if (acceleration_time_ > next_timestep) {
+ UpdateVals(acceleration_, next_timestep);
+ } else {
+ UpdateVals(acceleration_, acceleration_time_);
+
+ 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);
+ }
+ }
+ }
+
+ return output_;
+}
+
+void TrapezoidProfile::CalculateTimes(double distance_to_target,
+ double goal_velocity) {
+ if (distance_to_target == 0) {
+ // We're there. Stop everything.
+ // TODO(aschuh): Deal with velocity not right.
+ acceleration_time_ = 0;
+ acceleration_ = 0;
+ constant_time_ = 0;
+ deceleration_time_ = 0;
+ deceleration_ = 0;
+ return;
+ } else if (distance_to_target < 0) {
+ // Recurse with everything inverted.
+ output_(1) *= -1;
+ CalculateTimes(-distance_to_target,
+ -goal_velocity);
+ output_(1) *= -1;
+ acceleration_ *= -1;
+ deceleration_ *= -1;
+ return;
+ }
+
+ constant_time_ = 0;
+ acceleration_ = maximum_acceleration_;
+ 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);
+ }
+
+ // Since we know what we'd have to do if we kept after it to decelerate, we
+ // know the sign of the acceleration.
+ if (maximum_acceleration_velocity > goal_velocity) {
+ deceleration_ = -maximum_acceleration_;
+ } else {
+ deceleration_ = maximum_acceleration_;
+ }
+
+ // 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_)));
+
+ // 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_;
+ } else {
+ acceleration_time_ = (top_velocity - output_(1)) /
+ acceleration_;
+ }
+
+ assert(top_velocity > -maximum_velocity_);
+
+ deceleration_time_ = (goal_velocity - top_velocity) /
+ deceleration_;
+}
+
+} // namespace util
+} // namespace aos
diff --git a/aos/common/util/trapezoid_profile.h b/aos/common/util/trapezoid_profile.h
new file mode 100644
index 0000000..1051eec
--- /dev/null
+++ b/aos/common/util/trapezoid_profile.h
@@ -0,0 +1,63 @@
+#ifndef AOS_COMMON_UTIL_TRAPEZOID_PROFILE_H_
+#define AOS_COMMON_UTIL_TRAPEZOID_PROFILE_H_
+
+#include "Eigen/Dense"
+
+#include "aos/common/macros.h"
+#include "aos/common/time.h"
+
+namespace aos {
+namespace util {
+
+// Calculates a trapezoidal motion profile (like for a control loop's goals).
+// Supports having the end speed and position changed in the middle.
+//
+// The only units assumption that this class makes is that the unit of time is
+// seconds.
+class TrapezoidProfile {
+ public:
+ // delta_time is how long between each call to Update.
+ TrapezoidProfile(const time::Time &delta_time);
+
+ // Updates the state. The names of the arguments are pretty easy to figure
+ // out.
+ const Eigen::Matrix<double, 2, 1> &Update(double goal_position,
+ double goal_velocity);
+
+ void set_maximum_acceleration(double maximum_acceleration) {
+ maximum_acceleration_ = maximum_acceleration;
+ }
+ 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.
+ void UpdateVals(double acceleration, double delta_time);
+ // Calculates how long to go for each segment.
+ void CalculateTimes(double distance_to_target,
+ double goal_velocity);
+ // output_ is where it should go at time_.
+ Eigen::Matrix<double, 2, 1> output_;
+
+ double acceleration_time_;
+ double acceleration_;
+ double constant_time_;
+ double deceleration_time_;
+ double deceleration_;
+ double current_velocity_;
+
+ double maximum_acceleration_;
+ double maximum_velocity_;
+
+ // How long between calls to Update.
+ const time::Time timestep_;
+
+ DISALLOW_COPY_AND_ASSIGN(TrapezoidProfile);
+};
+
+} // namespace util
+} // namespace aos
+
+#endif // AOS_COMMON_UTIL_TRAPEZOID_PROFILE_H_
diff --git a/aos/common/util/trapezoid_profile_test.cc b/aos/common/util/trapezoid_profile_test.cc
new file mode 100644
index 0000000..6a06942
--- /dev/null
+++ b/aos/common/util/trapezoid_profile_test.cc
@@ -0,0 +1,93 @@
+#include "gtest/gtest.h"
+
+#include "Eigen/Dense"
+
+#include "aos/common/util/trapezoid_profile.h"
+
+namespace aos {
+namespace util {
+namespace testing {
+
+class TrapezoidProfileTest : public ::testing::Test {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ protected:
+ TrapezoidProfileTest() : profile_(delta_time) {
+ position_.setZero();
+ profile_.set_maximum_acceleration(0.75);
+ profile_.set_maximum_velocity(1.75);
+ }
+
+ // Runs an iteration.
+ void RunIteration(double goal_position,
+ double goal_velocity) {
+ position_ = profile_.Update(goal_position,
+ goal_velocity);
+ }
+
+ const Eigen::Matrix<double, 2, 1> &position() { return position_; }
+
+ TrapezoidProfile profile_;
+
+ ::testing::AssertionResult At(double position, double velocity) {
+ if (velocity != position_(1)) {
+ return ::testing::AssertionFailure() << "velocity is " << position_(1) <<
+ " not " << velocity;
+ }
+ if (position != position_(0)) {
+ return ::testing::AssertionFailure() << "position is " << position_(0) <<
+ " not " << position;
+ }
+ return ::testing::AssertionSuccess() << "at " << position <<
+ " moving at " << velocity;
+ }
+
+ private:
+ static const time::Time delta_time;
+
+ Eigen::Matrix<double, 2, 1> position_;
+};
+const time::Time TrapezoidProfileTest::delta_time = time::Time::InSeconds(0.01);
+
+TEST_F(TrapezoidProfileTest, ReachesGoal) {
+ for (int i = 0; i < 450; ++i) {
+ RunIteration(3, 0);
+ }
+ EXPECT_TRUE(At(3, 0));
+}
+
+// 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);
+ }
+ EXPECT_TRUE(At(-2, 0));
+}
+
+TEST_F(TrapezoidProfileTest, SwitchGoalInMiddle) {
+ for (int i = 0; i < 200; ++i) {
+ RunIteration(-2, 0);
+ }
+ EXPECT_FALSE(At(-2, 0));
+ for (int i = 0; i < 550; ++i) {
+ RunIteration(0, 0);
+ }
+ 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);
+ }
+ EXPECT_NEAR(1.5, position()(1), 10e-5);
+ for (int i = 0; i < 2000; ++i) {
+ RunIteration(4, 0);
+ }
+ EXPECT_TRUE(At(4, 0));
+}
+
+} // namespace testing
+} // namespace util
+} // namespace aos
diff --git a/aos/common/util/util.gyp b/aos/common/util/util.gyp
new file mode 100644
index 0000000..15f868f
--- /dev/null
+++ b/aos/common/util/util.gyp
@@ -0,0 +1,35 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'trapezoid_profile',
+ 'type': 'static_library',
+ 'sources': [
+ 'trapezoid_profile.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):eigen',
+ '<(AOS)/common/common.gyp:time',
+ ],
+ 'export_dependent_settings': [
+ '<(EXTERNALS):eigen',
+ '<(AOS)/common/common.gyp:time',
+ ],
+ },
+ {
+ 'target_name': 'trapezoid_profile_test',
+ 'type': 'executable',
+ 'sources': [
+ 'trapezoid_profile_test.cc',
+ ],
+ 'dependencies': [
+ ':trapezoid_profile',
+ '<(EXTERNALS):gtest',
+ # TODO(brians): remove this when time no longer requires it
+ '<(AOS)/build/aos.gyp:logging',
+ # TODO(brians): remove this when logging no longer requires it
+ #'<(AOS)/common/common.gyp:die',
+ '<(AOS)/build/aos.gyp:libaos',
+ ],
+ },
+ ],
+}