commit 4567e2818fb2381a5b8fbcc7ed814f767cd75e0f
author Maxwell Henderson <mxwhenderson@gmail.com> Sun Feb 25 15:38:15 2024 -0800
committer Maxwell Henderson <mxwhenderson@gmail.com> Wed Feb 28 20:54:51 2024 -0800
tree b52d74ecd4f15f7116247b52976ea223ace8fa4a
parent 9778928a98103f54ee2bf233692e314f2d1b9f1d

Fix logic for handling beambreak value in extend

Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Ia87b562faab84ce82c416672902a9ed46e541315

y2024/control_loops/superstructure/superstructure.cc

diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index 56980d5..ff25bbe 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -175,6 +175,7 @@
           extend_at_retracted) {
         state_ = SuperstructureState::INTAKING;
       }
+
       extend_goal = ExtendStatus::RETRACTED;
       catapult_requested_ = false;
       break;
@@ -197,6 +198,10 @@
 
       break;
     case SuperstructureState::LOADED:
+      if (!position->extend_beambreak() && !position->catapult_beambreak()) {
+        state_ = SuperstructureState::IDLE;
+      }
+
       if (catapult_requested_ == true) {
         state_ = SuperstructureState::MOVING;
         break;

y2024/control_loops/superstructure/superstructure_lib_test.cc

diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index ec29ea2..bafe93f 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -803,7 +803,7 @@
     ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
   }
 
-  superstructure_plant_.set_extend_beambreak(false);
+  superstructure_plant_.set_extend_beambreak(true);
 
   RunFor(chrono::seconds(10));
   VerifyNearGoal();
@@ -1135,6 +1135,7 @@
   EXPECT_NEAR(superstructure_status_fetcher_->shooter()->turret()->position(),
               simulated_robot_constants_->common()->turret_loading_position(),
               0.01);
+
   {
     auto builder = superstructure_goal_sender_.MakeBuilder();