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 &&
diff --git a/y2022/control_loops/superstructure/superstructure_goal.fbs b/y2022/control_loops/superstructure/superstructure_goal.fbs
index c86f338..2868e98 100644
--- a/y2022/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2022/control_loops/superstructure/superstructure_goal.fbs
@@ -48,6 +48,9 @@
// If true, auto-track the turret to point at the goal.
auto_aim:bool (id: 11);
+
+ // If true, we started with the ball loaded and should proceed to that state.
+ preloaded:bool (id: 12);
}
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index 1ccdd51..e2e5c91 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -1200,6 +1200,25 @@
EXPECT_EQ(superstructure_status_fetcher_->state(), SuperstructureState::IDLE);
}
+// Test that we are able to signal that the ball was preloaded
+TEST_F(SuperstructureTest, Preloaded) {
+ SetEnabled(true);
+ WaitUntilZeroed();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_preloaded(true);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(dt());
+
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::LOADED);
+}
+
// Tests that the turret switches to auto-aiming when we set auto_aim to
// true.
TEST_F(SuperstructureTest, TurretAutoAim) {
diff --git a/y2022/control_loops/superstructure/superstructure_status.fbs b/y2022/control_loops/superstructure/superstructure_status.fbs
index 1005c46..4b215ac 100644
--- a/y2022/control_loops/superstructure/superstructure_status.fbs
+++ b/y2022/control_loops/superstructure/superstructure_status.fbs
@@ -13,16 +13,16 @@
// State of the superstructure state machine
enum SuperstructureState : ubyte {
// Before a ball is intaked, when neither intake beambreak is triggered
- IDLE,
+ IDLE = 0,
// Transferring ball with transfer rollers. Moves turret to loading position.
- TRANSFERRING,
+ TRANSFERRING = 1,
// Loading the ball into the catapult
- LOADING,
+ LOADING = 2,
// The ball is loaded into the catapult
- LOADED,
+ LOADED = 3,
// Waiting for the turret to be at shooting goal and then telling the
// catapult to fire.
- SHOOTING,
+ SHOOTING = 4,
}
table AimerStatus {