Spring works.
Change-Id: I370012cc80e9019467100bec631c488c10a73141
diff --git a/motors/seems_reasonable/BUILD b/motors/seems_reasonable/BUILD
index 47ca065..a1613eb 100644
--- a/motors/seems_reasonable/BUILD
+++ b/motors/seems_reasonable/BUILD
@@ -74,3 +74,23 @@
"//frc971/control_loops/drivetrain:polydrivetrain_uc",
],
)
+
+cc_library(
+ name = "spring",
+ srcs = ["spring.cc"],
+ hdrs = ["spring.h"],
+ compatible_with = mcu_cpus,
+ visibility = ["//visibility:public"],
+ deps = ["//frc971/zeroing:wrap"],
+)
+
+cc_test(
+ name = "spring_test",
+ srcs = [
+ "spring_test.cc",
+ ],
+ deps = [
+ ":spring",
+ "//aos/testing:googletest",
+ ],
+)
diff --git a/motors/seems_reasonable/spring.cc b/motors/seems_reasonable/spring.cc
new file mode 100644
index 0000000..d66e950
--- /dev/null
+++ b/motors/seems_reasonable/spring.cc
@@ -0,0 +1,152 @@
+#include "motors/seems_reasonable/spring.h"
+
+#include "frc971/zeroing/wrap.h"
+
+#include <cmath>
+
+namespace motors {
+namespace seems_reasonable {
+namespace {
+
+constexpr float kTwoPi = 2.0 * M_PI;
+
+} // namespace
+
+float NextGoal(float current_goal, float goal) {
+ float remainder = remainderf(current_goal - goal, kTwoPi);
+ if (remainder >= 0.0f) {
+ remainder -= kTwoPi;
+ }
+ return -remainder + current_goal;
+}
+
+float PreviousGoal(float current_goal, float goal) {
+ float remainder = remainderf(current_goal - goal, kTwoPi);
+ if (remainder <= 0.0f) {
+ remainder += kTwoPi;
+ }
+ return -remainder + current_goal;
+}
+
+void Spring::Iterate(bool unload, bool prime, bool fire, bool force_reset,
+ bool encoder_valid, float angle) {
+ // Angle is +- M_PI. So, we need to find the nearest angle to the previous
+ // one, and that's our new angle.
+ angle_ = ::frc971::zeroing::Wrap(angle_, angle, kTwoPi);
+
+ switch (state_) {
+ case State::UNINITIALIZED:
+ // Go to the previous unload from where we are.
+ goal_ = angle_;
+ goal_ = PreviousGoal(kUnloadGoal);
+ if (prime && fire) {
+ Unload();
+ }
+ break;
+ case State::UNLOAD:
+ if (!encoder_valid) {
+ state_ = State::STUCK_UNLOAD;
+ } else if (!unload && prime && fire) {
+ // Go to the next goal from the current location. This handles if we
+ // fired or didn't on the previous cycle.
+ goal_ = angle_;
+ goal_ = NextGoal(kLoadGoal);
+ Load();
+ }
+ case State::STUCK_UNLOAD:
+ if (force_reset && encoder_valid && state_ == State::STUCK_UNLOAD) {
+ state_ = State::UNINITIALIZED;
+ } else if (timeout_ > 0) {
+ --timeout_;
+ }
+ break;
+ case State::LOAD:
+ if (!encoder_valid) {
+ goal_ = PreviousGoal(kUnloadGoal);
+ StuckUnload();
+ } else if (unload) {
+ goal_ = PreviousGoal(kUnloadGoal);
+ Unload();
+ } else if (!Near()) {
+ if (timeout_ > 0) {
+ --timeout_;
+ } else {
+ StuckUnload();
+ }
+ } else if (prime) {
+ goal_ = NextGoal(kPrimeGoal);
+ Prime();
+ }
+ break;
+ case State::PRIME:
+ if (!encoder_valid) {
+ goal_ = PreviousGoal(kUnloadGoal);
+ StuckUnload();
+ } else if (unload) {
+ goal_ = PreviousGoal(kUnloadGoal);
+ Unload();
+ } else if (!prime) {
+ goal_ = PreviousGoal(kLoadGoal);
+ Load();
+ } else if (!Near()) {
+ if (timeout_ > 0) {
+ --timeout_;
+ } else {
+ StuckUnload();
+ }
+ } else if (fire) {
+ goal_ = NextGoal(kFireGoal);
+ Fire();
+ }
+ break;
+
+ case State::FIRE:
+ if (!encoder_valid) {
+ goal_ = PreviousGoal(kUnloadGoal);
+ StuckUnload();
+ } else if (!Near()) {
+ if (timeout_ > 0) {
+ --timeout_;
+ } else {
+ StuckUnload();
+ }
+ } else {
+ // TODO(austin): Maybe have a different timeout for success.
+ if (timeout_ > 0) {
+ timeout_--;
+ } else {
+ Load();
+ goal_ = NextGoal(kLoadGoal);
+ }
+ }
+ break;
+ }
+ const float error = goal_ - angle_;
+ const float derror = (error - last_error_) * 200.0f;
+
+ switch (state_) {
+ case State::UNINITIALIZED:
+ output_ = 0.0f;
+ break;
+ case State::STUCK_UNLOAD:
+ case State::UNLOAD:
+ if (timeout_ > 0) {
+ output_ = -0.1f;
+ } else {
+ output_ = 0.0f;
+ }
+ break;
+
+ case State::LOAD:
+ case State::PRIME:
+ case State::FIRE: {
+ constexpr float kP = 3.00f;
+ constexpr float kD = 0.00f;
+ output_ = kP * error + kD * derror;
+ } break;
+ }
+ last_error_ = error;
+}
+
+} // namespace seems_reasonable
+} // namespace motors
diff --git a/motors/seems_reasonable/spring.h b/motors/seems_reasonable/spring.h
new file mode 100644
index 0000000..908ecc4
--- /dev/null
+++ b/motors/seems_reasonable/spring.h
@@ -0,0 +1,102 @@
+#ifndef MOTORS_SEEMS_REASONABLE_SPRING_H_
+#define MOTORS_SEEMS_REASONABLE_SPRING_H_
+
+#include <cmath>
+
+namespace motors {
+namespace seems_reasonable {
+
+float NextGoal(float current_goal, float goal);
+float PreviousGoal(float current_goal, float goal);
+
+class Spring {
+ public:
+ Spring() = default;
+ Spring(const Spring &) = delete;
+ Spring &operator=(const Spring &) = delete;
+
+ // Iterates the loop.
+ // If unload is true, unload.
+ // If the encoder isn't valid, unload.
+ // If prime is true, go to primed state.
+ // If prime and fire are true, fire.
+ void Iterate(bool unload, bool prime, bool fire, bool force_reset,
+ bool encoder_valid, float angle);
+
+ enum class State {
+ UNINITIALIZED = 0,
+ STUCK_UNLOAD = 1,
+ UNLOAD = 2,
+ LOAD = 3,
+ PRIME = 4,
+ FIRE = 5,
+ };
+
+ // Returns the current to output to the spring motors.
+ float output() const { return output_; }
+
+ // Returns true if the motor is near the goal.
+ bool Near() { return ::std::abs(angle_ - goal_) < 0.2f; }
+
+ State state() const { return state_; }
+
+ float angle() const { return angle_; }
+ float goal() const { return goal_; }
+
+ int timeout() const { return timeout_; }
+
+ private:
+ void Load() {
+ timeout_ = 5 * 200;
+ state_ = State::LOAD;
+ }
+
+ void Prime() {
+ timeout_ = 1 * 200;
+ state_ = State::PRIME;
+ }
+
+ void Unload() {
+ timeout_ = 10 * 200;
+ state_ = State::UNLOAD;
+ }
+
+ void StuckUnload() {
+ timeout_ = 10 * 200;
+ state_ = State::STUCK_UNLOAD;
+ }
+
+ void Fire() {
+ timeout_ = 100;
+ state_ = State::FIRE;
+ }
+
+ float NextGoal(float goal) {
+ return ::motors::seems_reasonable::NextGoal(goal_, goal);
+ }
+
+ float PreviousGoal(float goal) {
+ return ::motors::seems_reasonable::PreviousGoal(goal_, goal);
+ }
+
+ State state_ = State::UNINITIALIZED;
+
+ // Note, these need to be (-M_PI, M_PI]
+ constexpr static float kLoadGoal = -0.18f;
+ constexpr static float kPrimeGoal = -0.10f;
+ constexpr static float kFireGoal = -0.0f;
+ constexpr static float kUnloadGoal = kFireGoal;
+
+ float angle_ = 0.0f;
+ float goal_ = 0.0f;
+
+ int timeout_ = 0;
+
+ float output_ = 0.0f;
+ float last_error_ = 0.0f;
+};
+
+} // namespace seems_reasonable
+} // namespace motors
+
+#endif // MOTORS_SEEMS_REASONABLE_SPRING_H_
diff --git a/motors/seems_reasonable/spring_test.cc b/motors/seems_reasonable/spring_test.cc
new file mode 100644
index 0000000..bcc2a8a
--- /dev/null
+++ b/motors/seems_reasonable/spring_test.cc
@@ -0,0 +1,35 @@
+#include "motors/seems_reasonable/spring.h"
+
+#include "gtest/gtest.h"
+
+namespace motors {
+namespace seems_reasonable {
+namespace testing {
+
+// Tests that NextGoal always returns the next goal.
+TEST(GoalTest, TestNextGoal) {
+ EXPECT_NEAR(1.0, NextGoal(0.0, 1.0), 1e-6);
+
+ EXPECT_NEAR(2.0 * M_PI + 1.0, NextGoal(1.1, 1.0), 1e-6);
+
+ EXPECT_NEAR(6.0 * M_PI + 1.0, NextGoal(1.1 + 4.0 * M_PI, 1.0), 1e-6);
+
+ EXPECT_NEAR(2.0 * M_PI + 1.0, NextGoal(1.0, 1.0), 1e-6);
+
+ EXPECT_NEAR(2.0 * M_PI + 6.0, NextGoal(6.1, 6.0), 1e-6);
+}
+
+// Tests that PreviousGoal always returns the previous goal.
+TEST(GoalTest, TestPreviousGoal) {
+ EXPECT_NEAR(-2.0 * M_PI + 1.0, PreviousGoal(0.0, 1.0), 1e-6);
+
+ EXPECT_NEAR(1.0, PreviousGoal(1.1, 1.0), 1e-6);
+
+ EXPECT_NEAR(4.0 * M_PI + 1.0, PreviousGoal(1.1 + 4.0 * M_PI, 1.0), 1e-6);
+
+ EXPECT_NEAR(-2.0 * M_PI + 1.0, PreviousGoal(1.0, 1.0), 1e-6);
+}
+
+} // namespace testing
+} // namespace seems_reasonable
+} // namespace motors