From 3285bcaa0622621e64f8b9e4a145d443a33f7866 Mon Sep 17 00:00:00 2001
From: superpenguin612 <74030080+superpenguin612@users.noreply.github.com>
Date: Tue, 19 Mar 2024 16:06:57 -0400
Subject: [PATCH] Add button to execute a one-shot atGoal override, bypassing
 positional loop goal checking (useful if a mechanism is close but not at
 setpoint because of damage)

---
 src/main/java/frc/robot/subsystems/Climber.java     |  4 ++++
 src/main/java/frc/robot/subsystems/Intake.java      |  4 ++++
 src/main/java/frc/robot/subsystems/NoteLift.java    |  4 ++++
 src/main/java/frc/robot/subsystems/Shooter.java     | 12 +++++++-----
 src/main/java/frc/robot/subsystems/ShooterTilt.java |  6 ++++--
 5 files changed, 23 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java
index 04cee11..9b34ba0 100644
--- a/src/main/java/frc/robot/subsystems/Climber.java
+++ b/src/main/java/frc/robot/subsystems/Climber.java
@@ -266,4 +266,8 @@ public Command zeroMechanismCommand() {
         }).until(() -> motor.getOutputCurrent() > 10)
                 .andThen(resetPositionCommand());
     }
+
+    public boolean atGoal() {
+        return pidController.atGoal() || GlobalStates.AT_GOAL_OVERRIDE.enabled();
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java
index d0e5cd9..3295022 100644
--- a/src/main/java/frc/robot/subsystems/Intake.java
+++ b/src/main/java/frc/robot/subsystems/Intake.java
@@ -395,4 +395,8 @@ public Command zeroMechanismCommand() {
                 .until(() -> (leftArmMotor.getOutputCurrent() > 20) && (rightArmMotor.getOutputCurrent() > 20))
                 .andThen(resetPositionCommand());
     }
+
+    public boolean atGoal() {
+        return pidController.atGoal() || GlobalStates.AT_GOAL_OVERRIDE.enabled();
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/NoteLift.java b/src/main/java/frc/robot/subsystems/NoteLift.java
index c8f1954..0a00fb0 100644
--- a/src/main/java/frc/robot/subsystems/NoteLift.java
+++ b/src/main/java/frc/robot/subsystems/NoteLift.java
@@ -262,4 +262,8 @@ public Command zeroMechanismCommand() {
         }).until(() -> motor.getOutputCurrent() > 10)
                 .andThen(resetPositionCommand());
     }
+
+    public boolean atGoal() {
+        return pidController.atGoal() || GlobalStates.AT_GOAL_OVERRIDE.enabled();
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java
index 9eb8a2e..ed83326 100644
--- a/src/main/java/frc/robot/subsystems/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/Shooter.java
@@ -37,6 +37,7 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.FieldConstants;
+import frc.robot.GlobalStates;
 
 @LoggedObject
 public class Shooter extends SubsystemBase implements BaseShooter {
@@ -254,12 +255,13 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) {
         return sysIdRoutine.dynamic(direction).withName("shooter.sysIdDynamic");
     }
 
-    @Log
-    public boolean atGoal() {
-        return leftPidController.atSetpoint() && rightPidController.atSetpoint();
-    }
-
     public Command stopCommand() {
         return run(this::stop);
     }
+
+    @Log
+    public boolean atGoal() {
+        return leftPidController.atSetpoint() && rightPidController.atSetpoint()
+                || GlobalStates.AT_GOAL_OVERRIDE.enabled();
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/ShooterTilt.java b/src/main/java/frc/robot/subsystems/ShooterTilt.java
index ac8ec96..d1f45c9 100644
--- a/src/main/java/frc/robot/subsystems/ShooterTilt.java
+++ b/src/main/java/frc/robot/subsystems/ShooterTilt.java
@@ -289,9 +289,11 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) {
 
     @Log
     public boolean atGoal() {
-        return pidController.atSetpoint()
+        return (pidController.atSetpoint()
                 && Math.abs(pidController.getGoal().position - pidController.getSetpoint().position) <= TOLERANCE
-                && !beyondMaxDistance;
+                && !beyondMaxDistance)
+
+                || GlobalStates.AT_GOAL_OVERRIDE.enabled();
     }
 
     public Command resetControllersCommand() {