Copy balls shot from y2020
Signed-off-by: Ravago Jones <ravagojones@gmail.com>
Change-Id: I9eb08610caafcab3692e25603d11494db0a09140
diff --git a/y2022/actors/autonomous_actor.cc b/y2022/actors/autonomous_actor.cc
index 1ee34f7..3cfb3e7 100644
--- a/y2022/actors/autonomous_actor.cc
+++ b/y2022/actors/autonomous_actor.cc
@@ -282,5 +282,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 deeaa20..b214ac8 100644
--- a/y2022/actors/autonomous_actor.h
+++ b/y2022/actors/autonomous_actor.h
@@ -48,6 +48,7 @@
void set_preloaded(bool preloaded) { preloaded_ = preloaded; }
void SendSuperstructureGoal();
+ bool WaitForBallsShot(int num_shot);
void ExtendFrontIntake();
void RetractFrontIntake();
void ExtendBackIntake();