Add fault detection and auto-shutoff to shooter if encoder is unplugged

src/main/java/frc/robot/Constants.java

@@ -53,7 +53,7 @@ public static final class FeatureFlags {
         public static final boolean runLocalizer = true;
 
         public static final boolean runIntake = false;
-        public static final boolean runScoring = false;
+        public static final boolean runScoring = true;
         public static final boolean runEndgame = false;
         public static final boolean runDrive = true;
 
@@ -480,6 +480,9 @@ public static final class ScoringConstants {
         public static final double aimerkI = 10.0; // 5.0
         public static final double aimerkD = 0.0;
 
+        public static final double aimerMovementThresholdRadPerSec = 0.05;
+        public static final double maxAimUnresponsiveTimeSeconds = 0.1;
+
         public static final double aimerkS = 0.265;
         public static final double aimerkG = 0.1;
         public static final double aimerkV = 1.51;

src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java

@@ -213,7 +213,7 @@ public void updateInputs(AimerIOInputs inputs) {
                     feedforward.calculate(controlSetpoint, velocitySetpoint) + controllerVolts;
         }
 
-        appliedVolts = MathUtil.clamp(appliedVolts, -12.0, 12.0);
+        appliedVolts = MathUtil.clamp(appliedVolts, -4.0, 4.0);
         if (!motorDisabled || override) {
             aimerRight.setVoltage(appliedVolts);
         } else {

src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java

@@ -126,6 +126,8 @@ public void updateInputs(AimerIOInputs inputs) {
             sim.setInputVoltage(appliedVolts);
         }
 
+        sim.setInputVoltage(0.0);
+
         inputs.aimGoalAngleRad = goalAngleRad;
         inputs.aimProfileGoalAngleRad = trapezoidSetpoint.position;
         inputs.aimAngleRad = sim.getAngleRads();

src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java

@@ -111,6 +111,12 @@ public enum ScoringAction {
 
     private boolean readyToShoot = false;
 
+    // Keep track of the amount of time the arm has been unresponsive for
+    private Timer armUnresponsiveTime = new Timer();
+
+    // Something has gone wrong: disable the arm until code is restarted
+    private boolean faulted = false;
+
     public ScoringSubsystem(ShooterIO shooterIo, AimerIO aimerIo, HoodIO hoodIo) {
 
         this.shooterIo = shooterIo;
@@ -169,6 +175,8 @@ private void idle() {
 
         Logger.recordOutput("scoring/aimGoal", 0.0);
 
+        Logger.recordOutput("scoring/faulted", faulted);
+
         SmartDashboard.putBoolean("Has Note", shooterInputs.bannerSensor);
         SmartDashboard.putNumber("Aimer Location", aimerInputs.aimAngleRad);
 
@@ -479,6 +487,8 @@ public void enabledInit() {
 
     @Override
     public void periodic() {
+        Logger.recordOutput("scoring/faulted", faulted);
+
         if (!SmartDashboard.containsKey("Aimer Offset")) {
             SmartDashboard.putNumber("Aimer Offset", ScoringConstants.aimerStaticOffset);
         }
@@ -591,7 +601,27 @@ && willHitStage()) {
         }
 
         shooterIo.updateInputs(shooterInputs);
+
+        if (faulted && DriverStation.isEnabled()) {
+            aimerIo.setOverrideMode(true);
+            aimerIo.setOverrideVolts(0.0);
+        }
         aimerIo.updateInputs(aimerInputs);
+
+        if (DriverStation.isEnabled()
+                && (aimerInputs.aimAppliedVolts > ScoringConstants.aimerkS * 2
+                        && aimerInputs.aimVelocityRadPerSec
+                                < ScoringConstants.aimerMovementThresholdRadPerSec)) {
+            armUnresponsiveTime.start();
+
+            if (armUnresponsiveTime.get() > ScoringConstants.maxAimUnresponsiveTimeSeconds) {
+                faulted = true;
+            }
+        } else {
+            armUnresponsiveTime.reset();
+            armUnresponsiveTime.stop();
+        }
+
         hoodIo.updateInputs(hoodInputs);
 
         Logger.processInputs("scoring/shooter", shooterInputs);