commit 803bbf0b824b036695f1450f2a92bd5bf74e5618
author Milind Upadhyay <milind.upadhyay@gmail.com> Fri Mar 11 17:56:26 2022 -0800
committer Milind Upadhyay <milind.upadhyay@gmail.com> Fri Mar 11 20:55:29 2022 -0800
tree 7cec4b25bcc3571ea10cf5a704246f1c60b06f6c
parent 2c39086edd763e4c605ebbc0cafda188dd032764

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

y2022/actors/autonomous_actor.cc

diff --git a/y2022/actors/autonomous_actor.cc b/y2022/actors/autonomous_actor.cc
index ed1a9ee..1ee34f7 100644
--- a/y2022/actors/autonomous_actor.cc
+++ b/y2022/actors/autonomous_actor.cc
@@ -180,6 +180,34 @@
   if (!test_spline_->WaitForSplineDistanceRemaining(0.02)) return;
 }
 
+bool AutonomousActor::WaitForPreloaded() {
+  set_preloaded(true);
+  SendSuperstructureGoal();
+
+  ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
+                                      event_loop()->monotonic_now(),
+                                      ActorBase::kLoopOffset);
+
+  bool loaded = false;
+  while (!loaded) {
+    if (ShouldCancel()) {
+      return false;
+    }
+
+    phased_loop.SleepUntilNext();
+    superstructure_status_fetcher_.Fetch();
+    CHECK(superstructure_status_fetcher_.get() != nullptr);
+
+    loaded = (superstructure_status_fetcher_->state() ==
+              control_loops::superstructure::SuperstructureState::LOADED);
+  }
+
+  set_preloaded(false);
+  SendSuperstructureGoal();
+
+  return true;
+}
+
 void AutonomousActor::SendSuperstructureGoal() {
   auto builder = superstructure_goal_sender_.MakeBuilder();
 
@@ -217,6 +245,7 @@
   superstructure_builder.add_transfer_roller_speed(transfer_roller_voltage_);
   superstructure_builder.add_catapult(catapult_goal_offset);
   superstructure_builder.add_fire(fire_);
+  superstructure_builder.add_preloaded(preloaded_);
   superstructure_builder.add_auto_aim(true);
 
   if (builder.Send(superstructure_builder.Finish()) !=

y2022/actors/autonomous_actor.h

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_;