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;