Merge "Use setpoint in joystick reader"
diff --git a/y2022/actors/autonomous_actor.cc b/y2022/actors/autonomous_actor.cc
index 67156dd..60eabc2 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();
 
@@ -220,6 +248,7 @@
       transfer_roller_back_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()) !=
@@ -260,5 +289,27 @@
   SendSuperstructureGoal();
 }
 
+bool AutonomousActor::WaitForBallsShot(int num_wanted) {
+  ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
+                                      event_loop()->monotonic_now(),
+                                      ActorBase::kLoopOffset);
+  superstructure_status_fetcher_.Fetch();
+  CHECK(superstructure_status_fetcher_.get() != nullptr);
+  int initial_balls = superstructure_status_fetcher_->shot_count();
+  LOG(INFO) << "Waiting for balls, started with " << initial_balls;
+  while (true) {
+    if (ShouldCancel()) {
+      return false;
+    }
+    phased_loop.SleepUntilNext();
+    superstructure_status_fetcher_.Fetch();
+    CHECK(superstructure_status_fetcher_.get() != nullptr);
+    if (superstructure_status_fetcher_->shot_count() - initial_balls >=
+        num_wanted) {
+      return true;
+    }
+  }
+}
+
 }  // namespace actors
 }  // namespace y2022
diff --git a/y2022/actors/autonomous_actor.h b/y2022/actors/autonomous_actor.h
index 37a8cdb..8200f70 100644
--- a/y2022/actors/autonomous_actor.h
+++ b/y2022/actors/autonomous_actor.h
@@ -48,8 +48,10 @@
   }
 
   void set_fire_at_will(bool fire) { fire_ = fire; }
+  void set_preloaded(bool preloaded) { preloaded_ = preloaded; }
 
   void SendSuperstructureGoal();
+  bool WaitForBallsShot(int num_shot);
   void ExtendFrontIntake();
   void RetractFrontIntake();
   void ExtendBackIntake();
@@ -57,6 +59,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();
@@ -68,6 +74,7 @@
   double transfer_roller_front_voltage_ = 0.0;
   double transfer_roller_back_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 ce4ed01..e45917a 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -86,7 +86,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
@@ -113,6 +113,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_front < 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 6266b3c..8c7c862 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 ac4e079..66a615e 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -1199,6 +1199,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 {