commit 77bf1fd443e97751e9bddc7ee5744afab20c68e6
author Niko Sohmers <nikolai@sohmers.com> Sat Mar 16 23:22:57 2024 -0700
committer Niko Sohmers <nikolai@sohmers.com> Tue Mar 19 16:50:53 2024 -0700
tree 50af9833408ca417e390bd55defecfb7b34ca54e
parent 6153b39adb8b84df7967c9d4ce239c881c418c1c

Add "disable_extend" bool to constants

Change-Id: Ibc839f913aa573655ad27aaeef9635ef2071d883
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>

y2024/control_loops/superstructure/superstructure.cc

diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index d3e98ab..5727546 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -159,9 +159,12 @@
       robot_constants_->common()->extend_set_points();
 
   // Checks if the extend is close enough to the retracted position to be
-  // considered ready to accept note from the transfer rollers.
-  const bool extend_at_retracted = PositionNear(
-      extend_.position(), extend_set_points->retracted(), kExtendThreshold);
+  // considered ready to accept note from the transfer rollers. If disable
+  // extend is triggered, this will autoatically be false.
+  const bool extend_at_retracted =
+      (!robot_constants_->common()->disable_extend() &&
+       PositionNear(extend_.position(), extend_set_points->retracted(),
+                    kExtendThreshold));
 
   // Check if the turret is at the position to accept the note from extend
   const bool turret_ready_for_load =
@@ -525,10 +528,13 @@
 
   drivetrain_status_fetcher_.Fetch();
 
+  // Zero out extend position if "disable_extend" is true
   const bool collided = collision_avoidance_.IsCollided({
       .intake_pivot_position = intake_pivot_.estimated_position(),
       .turret_position = shooter_.turret().estimated_position(),
-      .extend_position = extend_.estimated_position(),
+      .extend_position = ((!robot_constants_->common()->disable_extend())
+                              ? extend_.estimated_position()
+                              : 0.0),
   });
 
   aos::FlatbufferFixedAllocatorArray<
@@ -635,6 +641,11 @@
           output != nullptr ? &output_struct.extend_voltage : nullptr,
           status->fbb());
 
+  // Zero out extend voltage if "disable_extend" is true
+  if (robot_constants_->common()->disable_extend()) {
+    output_struct.extend_voltage = 0.0;
+  }
+
   if (output) {
     output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
   }