commit 61a876e623ef65e25e8ff98b3d67026ab953cdb1
author James Kuszmaul <jabukuszmaul@gmail.com> Thu Mar 17 07:34:08 2022 -0700
committer Gerrit Code Review <gerrit@software.frc971.org> Thu Mar 17 07:34:08 2022 -0700
tree 7fb2af08d764b8bb5c13c378752864334691ca2a
parent af3ea960f4134bdfc417bee5a388223324f693c1
parent a391dde09277e479cb8ef00e7435475c19e95e9a

Merge "Lower rt priority on localizer"

y2022/actors/autonomous_actor.cc

diff --git a/y2022/actors/autonomous_actor.cc b/y2022/actors/autonomous_actor.cc
index 5e344ac..8bfb2d9 100644
--- a/y2022/actors/autonomous_actor.cc
+++ b/y2022/actors/autonomous_actor.cc
@@ -212,7 +212,7 @@
   set_turret_goal(constants::Values::kTurretFrontIntakePos());
   set_fire_at_will(true);
   SendSuperstructureGoal();
-  if (!WaitForBallsShot(1)) return;
+  if (!WaitForBallsShot()) return;
   set_fire_at_will(false);
   SendSuperstructureGoal();
 
@@ -227,7 +227,7 @@
   RetractBackIntake();
   set_fire_at_will(true);
   SendSuperstructureGoal();
-  if (!WaitForBallsShot(2)) return;
+  if (!WaitForBallsShot()) return;
   set_fire_at_will(false);
   SendSuperstructureGoal();
 
@@ -250,7 +250,7 @@
   // Fire the two balls once we stopped
   set_fire_at_will(true);
   SendSuperstructureGoal();
-  if (!WaitForBallsShot(2)) return;
+  if (!WaitForBallsShot()) return;
   set_fire_at_will(false);
   SendSuperstructureGoal();
 
@@ -383,7 +383,27 @@
   SendSuperstructureGoal();
 }
 
-[[nodiscard]] bool AutonomousActor::WaitForBallsShot(int num_wanted) {
+[[nodiscard]] bool AutonomousActor::WaitForBallsShot() {
+  CHECK(superstructure_status_fetcher_.Fetch());
+
+  // Don't do anything if we aren't loaded
+  if (superstructure_status_fetcher_->state() !=
+          control_loops::superstructure::SuperstructureState::LOADED &&
+      superstructure_status_fetcher_->state() !=
+          control_loops::superstructure::SuperstructureState::SHOOTING) {
+    LOG(WARNING) << "No balls to shoot";
+    return true;
+  }
+
+  // Since we're loaded, there will atleast be 1 ball to shoot
+  int num_wanted = 1;
+
+  // If we have another ball, we will shoot 2
+  if (superstructure_status_fetcher_->front_intake_has_ball() ||
+      superstructure_status_fetcher_->back_intake_has_ball()) {
+    num_wanted++;
+  }
+
   ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
                                       event_loop()->monotonic_now(),
                                       ActorBase::kLoopOffset);

y2022/actors/autonomous_actor.h

diff --git a/y2022/actors/autonomous_actor.h b/y2022/actors/autonomous_actor.h
index 461cfc1..0168630 100644
--- a/y2022/actors/autonomous_actor.h
+++ b/y2022/actors/autonomous_actor.h
@@ -50,9 +50,7 @@
   void set_requested_intake(std::optional<RequestedIntake> requested_intake) {
     requested_intake_ = requested_intake;
   }
-  void set_turret_goal(double turret_goal) {
-    turret_goal_ = turret_goal;
-  }
+  void set_turret_goal(double turret_goal) { turret_goal_ = turret_goal; }
 
   void set_fire_at_will(bool fire) { fire_ = fire; }
   void set_preloaded(bool preloaded) { preloaded_ = preloaded; }
@@ -68,8 +66,8 @@
   // Tells the superstructure the ball was preloaded and waits until it updates
   // the state
   [[nodiscard]] bool WaitForPreloaded();
-  // Waits for a certain number of balls to be shot
-  [[nodiscard]] bool WaitForBallsShot(int num_shot);
+  // Waits for the intaked balls to be shot
+  [[nodiscard]] bool WaitForBallsShot();
 
   void SplineAuto();
   void RapidReact();