Allow skipping to superstructure loaded state
In auto a ball will start preloaded, and we need to cheat the
statemachine.
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I95567299ac6f7f76941446b7c57d880a73e6dd6d
diff --git a/y2022/actors/autonomous_actor.h b/y2022/actors/autonomous_actor.h
index f01da02..deeaa20 100644
--- a/y2022/actors/autonomous_actor.h
+++ b/y2022/actors/autonomous_actor.h
@@ -45,6 +45,7 @@
}
void set_fire_at_will(bool fire) { fire_ = fire; }
+ void set_preloaded(bool preloaded) { preloaded_ = preloaded; }
void SendSuperstructureGoal();
void ExtendFrontIntake();
@@ -54,6 +55,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();
@@ -64,6 +69,7 @@
double roller_back_voltage_ = 0.0;
double transfer_roller_voltage_ = 0.0;
bool fire_ = false;
+ bool preloaded_ = false;
aos::Sender<frc971::control_loops::drivetrain::LocalizerControl>
localizer_control_sender_;