From acba94c98c4b0fbeaa0395acca5798829222d403 Mon Sep 17 00:00:00 2001
From: aidnem <99768676+aidnem@users.noreply.github.com>
Date: Mon, 29 Apr 2024 20:04:15 -0400
Subject: [PATCH] add basic fault detection

---
 src/main/java/frc/robot/Constants.java        |  5 +++-
 .../subsystems/scoring/AimerIORoboRio.java    |  2 +-
 .../robot/subsystems/scoring/AimerIOSim.java  |  2 ++
 .../subsystems/scoring/ScoringSubsystem.java  | 30 +++++++++++++++++++
 4 files changed, 37 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 4ad21a28..b6aee7d8 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/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;
diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java b/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java
index 9d8415f0..67701a68 100644
--- a/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java
+++ b/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 {
diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java b/src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java
index 49be8787..6effaa32 100644
--- a/src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java
+++ b/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();
diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java
index 7ab08f44..d1c85e81 100644
--- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java
+++ b/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);