commit 1f32d62d4593bae431492f5be03d614df0698602
author Ravago Jones <ravagojones@gmail.com> Thu Aug 26 12:20:36 2021 -0700
committer Ravago Jones <ravagojones@gmail.com> Sat Oct 09 15:19:02 2021 -0700
tree 5f6a9fe357cbf6049264df167341baa2b28b7ba5
parent a4782512ae742279143f8fb428a064f6df051ec1

Add WaitForBallsShot to 2020 autonomous

Change-Id: Ia426978c68866a7b5673306de584f9808c595926
Signed-off-by: Ravago Jones <ravagojones@gmail.com>

y2020/actors/autonomous_actor.cc

diff --git a/y2020/actors/autonomous_actor.cc b/y2020/actors/autonomous_actor.cc
index 794d1e7..30e1612 100644
--- a/y2020/actors/autonomous_actor.cc
+++ b/y2020/actors/autonomous_actor.cc
@@ -40,6 +40,9 @@
           event_loop->MakeFetcher<aos::JoystickState>("/aos")),
       path_fetcher_(event_loop->MakeFetcher<y2020::vision::GalacticSearchPath>(
           "/pi2/camera")),
+      superstructure_status_fetcher_(
+          event_loop->MakeFetcher<y2020::control_loops::superstructure::Status>(
+              "/superstructure")),
       auto_splines_() {
   set_max_drivetrain_voltage(2.0);
   replan_timer_ = event_loop->AddTimer([this]() { Replan(); });
@@ -321,5 +324,28 @@
   SendSuperstructureGoal();
 }
 
+bool AutonomousActor::WaitForBallsShot(int num_wanted) {
+  superstructure_status_fetcher_.Fetch();
+  CHECK(superstructure_status_fetcher_.get() != nullptr);
+  const int initial_balls_shot =
+      superstructure_status_fetcher_->shooter()->balls_shot();
+  int balls_shot = initial_balls_shot;
+
+  ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
+                                      event_loop()->monotonic_now(),
+                                      frc971::controls::kLoopFrequency / 2);
+  while (true) {
+    if (ShouldCancel()) {
+      return false;
+    }
+    phased_loop.SleepUntilNext();
+    superstructure_status_fetcher_.Fetch();
+    balls_shot = superstructure_status_fetcher_->shooter()->balls_shot();
+    if ((balls_shot - initial_balls_shot) >= num_wanted) {
+      return true;
+    }
+  }
+}
+
 }  // namespace actors
 }  // namespace y2020

y2020/actors/autonomous_actor.h

diff --git a/y2020/actors/autonomous_actor.h b/y2020/actors/autonomous_actor.h
index 7bdb44b..9c69d57 100644
--- a/y2020/actors/autonomous_actor.h
+++ b/y2020/actors/autonomous_actor.h
@@ -43,6 +43,7 @@
   void AutoNavBarrel();
   void AutoNavSlalom();
   bool DriveFwd();
+  bool WaitForBallsShot(int num_shot);
 
   void Replan();
 
@@ -56,6 +57,8 @@
       superstructure_goal_sender_;
   aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
   aos::Fetcher<y2020::vision::GalacticSearchPath> path_fetcher_;
+  aos::Fetcher<y2020::control_loops::superstructure::Status>
+      superstructure_status_fetcher_;
 
   aos::TimerHandler *replan_timer_;