commit 77e3d8a9fae9926c7f823b8c9c1cdf85904327bb
author Filip Kujawa <filip.j.kujawa@gmail.com> Sat Mar 02 11:34:56 2024 -0800
committer Filip Kujawa <filip.j.kujawa@gmail.com> Sat Mar 02 11:41:21 2024 -0800
tree 34ee7d3a16b6c8898add0b3c850b347b6d255eca
parent 73f739e1b22ca0c3e7c740be784f1a854816e268

Add timeout for exiting catapult loading state

Signed-off-by: Filip Kujawa <filip.j.kujawa@gmail.com>
Change-Id: I99041676157f0558aaa5f04f79e9205383ce48ac

y2024/control_loops/superstructure/superstructure.cc

diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index ad91614..6b262c1 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -22,6 +22,11 @@
 constexpr std::chrono::milliseconds kExtraIntakingTime =
     std::chrono::milliseconds(500);
 
+// Exit catapult loading state after this much time if we never
+// trigger any beambreaks.
+constexpr std::chrono::milliseconds kMaxCatapultLoadingTime =
+    std::chrono::milliseconds(3000);
+
 namespace y2024::control_loops::superstructure {
 
 using ::aos::monotonic_clock;
@@ -262,6 +267,7 @@
           extend_goal_location = ExtendStatus::CATAPULT;
           if (extend_ready_for_catapult_transfer && turret_ready_for_load &&
               altitude_ready_for_load) {
+            loading_catapult_start_time_ = timestamp;
             state_ = SuperstructureState::LOADING_CATAPULT;
           }
           break;
@@ -292,6 +298,13 @@
       extend_goal_location = ExtendStatus::CATAPULT;
       extend_roller_status = ExtendRollerStatus::TRANSFERING_TO_CATAPULT;
 
+      // If we lost the game piece, reset state to idle.
+      if (((timestamp - loading_catapult_start_time_) >
+           kMaxCatapultLoadingTime) &&
+          !position->catapult_beambreak() && !position->extend_beambreak()) {
+        state_ = SuperstructureState::IDLE;
+      }
+
       // Switch to READY state when the catapult beambreak is triggered
       if (position->catapult_beambreak()) {
         state_ = SuperstructureState::READY;

y2024/control_loops/superstructure/superstructure.h

diff --git a/y2024/control_loops/superstructure/superstructure.h b/y2024/control_loops/superstructure/superstructure.h
index e1856a3..1c2d119 100644
--- a/y2024/control_loops/superstructure/superstructure.h
+++ b/y2024/control_loops/superstructure/superstructure.h
@@ -73,10 +73,10 @@
 
   NoteGoal requested_note_goal_ = NoteGoal::NONE;
 
-  aos::monotonic_clock::time_point transfer_start_time_ =
+  aos::monotonic_clock::time_point intake_end_time_ =
       aos::monotonic_clock::time_point::min();
 
-  aos::monotonic_clock::time_point intake_end_time_ =
+  aos::monotonic_clock::time_point loading_catapult_start_time_ =
       aos::monotonic_clock::time_point::min();
 
   AbsoluteEncoderSubsystem intake_pivot_;