Added center peg auto.
Also did some minor bugfixes. Wait for the hood to finish in auto, etc.
Change-Id: Ia827872fe6b9ee16ddf268c48c803dccc836f7e1
diff --git a/y2017/actors/autonomous_actor.h b/y2017/actors/autonomous_actor.h
index 8dadf0c..d099725 100644
--- a/y2017/actors/autonomous_actor.h
+++ b/y2017/actors/autonomous_actor.h
@@ -69,6 +69,22 @@
use_vision_for_shots_ = use_vision_for_shots;
}
+ void WaitForHoodZeroed() {
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::std::chrono::milliseconds(5) / 2);
+ while (true) {
+ if (ShouldCancel()) return;
+
+ superstructure_queue.status.FetchLatest();
+ if (superstructure_queue.status.get()) {
+ if (superstructure_queue.status->hood.zeroed) {
+ return;
+ }
+ }
+ phased_loop.SleepUntilNext();
+ }
+ }
+
void SendSuperstructureGoal() {
auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
new_superstructure_goal->intake.distance = intake_goal_;