Spring works.
Change-Id: I370012cc80e9019467100bec631c488c10a73141
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