Require the prime button to be pressed (and released) to reload.
Safety feature
Change-Id: If6fe3ea052d720b3069843788588fb43061ef180
diff --git a/motors/seems_reasonable/spring.cc b/motors/seems_reasonable/spring.cc
index d66e950..7cf80c1 100644
--- a/motors/seems_reasonable/spring.cc
+++ b/motors/seems_reasonable/spring.cc
@@ -114,12 +114,34 @@
// TODO(austin): Maybe have a different timeout for success.
if (timeout_ > 0) {
timeout_--;
- } else {
- Load();
- goal_ = NextGoal(kLoadGoal);
+ } else if (!prime) {
+ state_ = State::WAIT_FOR_LOAD;
}
}
break;
+ case State::WAIT_FOR_LOAD:
+ if (!encoder_valid) {
+ StuckUnload();
+ } else if (unload) {
+ // Goal is as good as it is going to get since unload is the fire
+ // position.
+ Unload();
+ } else if (prime) {
+ state_ = State::WAIT_FOR_LOAD_RELEASE;
+ }
+ break;
+ case State::WAIT_FOR_LOAD_RELEASE:
+ if (!encoder_valid) {
+ StuckUnload();
+ } else if (unload) {
+ // Goal is as good as it is going to get since unload is the fire
+ // position.
+ Unload();
+ } else if (!prime) {
+ Load();
+ goal_ = NextGoal(kLoadGoal);
+ }
+ break;
}
const float error = goal_ - angle_;
const float derror = (error - last_error_) * 200.0f;
@@ -139,7 +161,9 @@
case State::LOAD:
case State::PRIME:
- case State::FIRE: {
+ case State::FIRE:
+ case State::WAIT_FOR_LOAD:
+ case State::WAIT_FOR_LOAD_RELEASE: {
constexpr float kP = 3.00f;
constexpr float kD = 0.00f;
output_ = kP * error + kD * derror;
diff --git a/motors/seems_reasonable/spring.h b/motors/seems_reasonable/spring.h
index 9f8f879..c7c923a 100644
--- a/motors/seems_reasonable/spring.h
+++ b/motors/seems_reasonable/spring.h
@@ -30,6 +30,8 @@
LOAD = 3,
PRIME = 4,
FIRE = 5,
+ WAIT_FOR_LOAD = 6,
+ WAIT_FOR_LOAD_RELEASE = 7,
};
// Returns the current to output to the spring motors.