Final competition code
Change-Id: I67dfc37c3159b405dc441fed37baa94a6f388e81
diff --git a/motors/seems_reasonable/spring.cc b/motors/seems_reasonable/spring.cc
index 7cf80c1..1127aa9 100644
--- a/motors/seems_reasonable/spring.cc
+++ b/motors/seems_reasonable/spring.cc
@@ -29,7 +29,7 @@
}
void Spring::Iterate(bool unload, bool prime, bool fire, bool force_reset,
- bool encoder_valid, float angle) {
+ bool force_move, 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);
@@ -39,12 +39,21 @@
// Go to the previous unload from where we are.
goal_ = angle_;
goal_ = PreviousGoal(kUnloadGoal);
- if (prime && fire) {
+ if (force_move) {
+ ForceMove();
+ } else if (prime && fire) {
Unload();
}
break;
+ case State::FORCE_MOVE:
+ if (!force_move) {
+ state_ = State::UNINITIALIZED;
+ }
+ break;
case State::UNLOAD:
- if (!encoder_valid) {
+ if (force_move) {
+ ForceMove();
+ } else 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
@@ -54,14 +63,19 @@
Load();
}
case State::STUCK_UNLOAD:
- if (force_reset && encoder_valid && state_ == State::STUCK_UNLOAD) {
+ if (force_move) {
+ ForceMove();
+ } else if (force_reset && encoder_valid &&
+ state_ == State::STUCK_UNLOAD) {
state_ = State::UNINITIALIZED;
} else if (timeout_ > 0) {
--timeout_;
}
break;
case State::LOAD:
- if (!encoder_valid) {
+ if (force_move) {
+ ForceMove();
+ } else if (!encoder_valid) {
goal_ = PreviousGoal(kUnloadGoal);
StuckUnload();
} else if (unload) {
@@ -79,7 +93,9 @@
}
break;
case State::PRIME:
- if (!encoder_valid) {
+ if (force_move) {
+ ForceMove();
+ } else if (!encoder_valid) {
goal_ = PreviousGoal(kUnloadGoal);
StuckUnload();
} else if (unload) {
@@ -101,7 +117,9 @@
break;
case State::FIRE:
- if (!encoder_valid) {
+ if (force_move) {
+ ForceMove();
+ } else if (!encoder_valid) {
goal_ = PreviousGoal(kUnloadGoal);
StuckUnload();
} else if (!Near()) {
@@ -120,7 +138,9 @@
}
break;
case State::WAIT_FOR_LOAD:
- if (!encoder_valid) {
+ if (force_move) {
+ ForceMove();
+ } else if (!encoder_valid) {
StuckUnload();
} else if (unload) {
// Goal is as good as it is going to get since unload is the fire
@@ -131,7 +151,9 @@
}
break;
case State::WAIT_FOR_LOAD_RELEASE:
- if (!encoder_valid) {
+ if (force_move) {
+ ForceMove();
+ } else if (!encoder_valid) {
StuckUnload();
} else if (unload) {
// Goal is as good as it is going to get since unload is the fire
@@ -159,6 +181,10 @@
}
break;
+ case State::FORCE_MOVE:
+ output_ = 1.0f;
+ break;
+
case State::LOAD:
case State::PRIME:
case State::FIRE:
diff --git a/motors/seems_reasonable/spring.h b/motors/seems_reasonable/spring.h
index c7c923a..90c9cd1 100644
--- a/motors/seems_reasonable/spring.h
+++ b/motors/seems_reasonable/spring.h
@@ -21,7 +21,7 @@
// 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);
+ bool force_move, bool encoder_valid, float angle);
enum class State {
UNINITIALIZED = 0,
@@ -32,6 +32,7 @@
FIRE = 5,
WAIT_FOR_LOAD = 6,
WAIT_FOR_LOAD_RELEASE = 7,
+ FORCE_MOVE = 8,
};
// Returns the current to output to the spring motors.
@@ -58,13 +59,18 @@
state_ = State::PRIME;
}
+ void ForceMove() {
+ timeout_ = 0;
+ state_ = State::FORCE_MOVE;
+ }
+
void Unload() {
- timeout_ = 10 * 200;
+ timeout_ = 14 * 200;
state_ = State::UNLOAD;
}
void StuckUnload() {
- timeout_ = 10 * 200;
+ timeout_ = 14 * 200;
state_ = State::STUCK_UNLOAD;
}
@@ -86,7 +92,7 @@
// Note, these need to be (-M_PI, M_PI]
constexpr static float kLoadGoal = -0.345f;
constexpr static float kPrimeGoal = -0.269f;
- constexpr static float kFireGoal = -0.163f;
+ constexpr static float kFireGoal = -0.063f;
constexpr static float kUnloadGoal = kFireGoal;
float angle_ = 0.0f;