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/actors/autonomous_actor.cc b/y2022/actors/autonomous_actor.cc
index ed1a9ee..1ee34f7 100644
--- a/y2022/actors/autonomous_actor.cc
+++ b/y2022/actors/autonomous_actor.cc
@@ -180,6 +180,34 @@
if (!test_spline_->WaitForSplineDistanceRemaining(0.02)) return;
}
+bool AutonomousActor::WaitForPreloaded() {
+ set_preloaded(true);
+ SendSuperstructureGoal();
+
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
+ event_loop()->monotonic_now(),
+ ActorBase::kLoopOffset);
+
+ bool loaded = false;
+ while (!loaded) {
+ if (ShouldCancel()) {
+ return false;
+ }
+
+ phased_loop.SleepUntilNext();
+ superstructure_status_fetcher_.Fetch();
+ CHECK(superstructure_status_fetcher_.get() != nullptr);
+
+ loaded = (superstructure_status_fetcher_->state() ==
+ control_loops::superstructure::SuperstructureState::LOADED);
+ }
+
+ set_preloaded(false);
+ SendSuperstructureGoal();
+
+ return true;
+}
+
void AutonomousActor::SendSuperstructureGoal() {
auto builder = superstructure_goal_sender_.MakeBuilder();
@@ -217,6 +245,7 @@
superstructure_builder.add_transfer_roller_speed(transfer_roller_voltage_);
superstructure_builder.add_catapult(catapult_goal_offset);
superstructure_builder.add_fire(fire_);
+ superstructure_builder.add_preloaded(preloaded_);
superstructure_builder.add_auto_aim(true);
if (builder.Send(superstructure_builder.Finish()) !=
diff --git a/y2022/actors/autonomous_actor.h b/y2022/actors/autonomous_actor.h
index f01da02..deeaa20 100644
--- a/y2022/actors/autonomous_actor.h
+++ b/y2022/actors/autonomous_actor.h
@@ -45,6 +45,7 @@
}
void set_fire_at_will(bool fire) { fire_ = fire; }
+ void set_preloaded(bool preloaded) { preloaded_ = preloaded; }
void SendSuperstructureGoal();
void ExtendFrontIntake();
@@ -54,6 +55,10 @@
void SendStartingPosition(const Eigen::Vector3d &start);
void MaybeSendStartingPosition();
+ // Tells the superstructure the ball was preloaded and waits until it updates
+ // the state
+ bool WaitForPreloaded();
+
void SplineAuto();
void Replan();
@@ -64,6 +69,7 @@
double roller_back_voltage_ = 0.0;
double transfer_roller_voltage_ = 0.0;
bool fire_ = false;
+ bool preloaded_ = false;
aos::Sender<frc971::control_loops::drivetrain::LocalizerControl>
localizer_control_sender_;
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 {