Add button to execute a one-shot atGoal override, bypassing positiona…

…l loop goal checking (useful if a mechanism is close but not at setpoint because of damage)
frc868 · Mar 19, 2024 · 3285bca · 3285bca
1 parent 6dfb69f
commit 3285bca
Show file tree

Hide file tree

Showing 5 changed files with 23 additions and 7 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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() {