commit 0a1282b8aca3d124477b82780989e119c460c5a7
author Filip Kujawa <filip.j.kujawa@gmail.com> Sat Mar 23 12:36:51 2024 -0700
committer Filip Kujawa <filip.j.kujawa@gmail.com> Sat Mar 23 12:36:51 2024 -0700
tree f6d0da0cd90efad3e7bfe9f0ed33b1637bbcbef1
parent 99faf63ea072e3b734c5dd791aa6ef83d7acd9c7

Stop intaking when transfer beam break triggered

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

y2024/control_loops/superstructure/superstructure.cc

diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index 5727546..af3f9de 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -51,8 +51,10 @@
           robot_constants_->common()->extend(),
           robot_constants_->robot()->extend_constants()->zeroing_constants()),
       extend_debouncer_(std::chrono::milliseconds(30),
-                        std::chrono::milliseconds(8)) {
-  event_loop->SetRuntimeRealtimePriority(37);
+                        std::chrono::milliseconds(8)),
+      transfer_debouncer_(std::chrono::milliseconds(30),
+                          std::chrono::milliseconds(8)) {
+  event_loop->SetRuntimeRealtimePriority(30);
 }
 
 bool PositionNear(double position, double goal, double threshold) {
@@ -79,6 +81,9 @@
   extend_debouncer_.Update(position->extend_beambreak(), timestamp);
   const bool extend_beambreak = extend_debouncer_.state();
 
+  transfer_debouncer_.Update(position->transfer_beambreak(), timestamp);
+  const bool transfer_beambreak = transfer_debouncer_.state();
+
   // Handle Climber Goal separately from main superstructure state machine
   double climber_position =
       robot_constants_->common()->climber_set_points()->retract();
@@ -221,6 +226,7 @@
           unsafe_goal->intake_goal() == IntakeGoal::INTAKE &&
           extend_at_retracted) {
         state_ = SuperstructureState::INTAKING;
+        note_in_transfer_ = false;
       }
 
       extend_goal_location = ExtendStatus::RETRACTED;
@@ -231,7 +237,18 @@
       if (extend_beambreak) {
         state_ = SuperstructureState::LOADED;
       }
-      intake_roller_state = IntakeRollerStatus::INTAKING;
+
+      if (transfer_beambreak) {
+        note_in_transfer_ = true;
+      }
+
+      // Once the note is in the transfer, stop the intake rollers
+      if (note_in_transfer_) {
+        intake_roller_state = IntakeRollerStatus::NONE;
+      } else {
+        intake_roller_state = IntakeRollerStatus::INTAKING;
+      }
+
       transfer_roller_status = TransferRollerStatus::TRANSFERING_IN;
       extend_roller_status = ExtendRollerStatus::TRANSFERING_TO_EXTEND;
       extend_goal_location = ExtendStatus::RETRACTED;