Allow skipping to superstructure loaded state
In auto a ball will start preloaded, and we need to cheat the
statemachine.
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I95567299ac6f7f76941446b7c57d880a73e6dd6d
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index c2decea..92e9528 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -84,7 +84,7 @@
unsafe_goal->auto_aim() ? auto_aim_goal : unsafe_goal->turret();
}
- // Supersturcture state machine:
+ // Superstructure state machine:
// 1. IDLE: Wait until an intake beambreak is triggerred, meaning that a ball
// is being intaked. This means that the transfer rollers have a ball. If
// we've been waiting here for too long without any beambreak triggered, the
@@ -111,6 +111,22 @@
// goes back to its return position. We have now finished the shot, so return
// to IDLE.
+ // If we started off preloaded, skip to the loaded state.
+ // Make sure we weren't already there just in case.
+ if (unsafe_goal != nullptr && unsafe_goal->preloaded()) {
+ switch (state_) {
+ case SuperstructureState::IDLE:
+ case SuperstructureState::TRANSFERRING:
+ case SuperstructureState::LOADING:
+ state_ = SuperstructureState::LOADED;
+ loading_timer_ = timestamp;
+ break;
+ case SuperstructureState::LOADED:
+ case SuperstructureState::SHOOTING:
+ break;
+ }
+ }
+
const bool is_spitting = ((intake_state_ == IntakeState::INTAKE_FRONT_BALL &&
transfer_roller_speed < 0) ||
(intake_state_ == IntakeState::INTAKE_BACK_BALL &&