Aim motor adjustments in case of failure

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

@@ -448,6 +448,12 @@ public static final class ScoringConstants {
         public static final double aimerkV = 1.51;
         public static final double aimerkA = 0.01;
 
+        // TODO: find real values
+        public static final double aimerFaultkS = 0.4;
+        public static final double aimerFaultkG = 0.2;
+        public static final double aimerFaultkV = 3;
+        public static final double aimerFaultkA = 0.02;
+
         public static final double shooterkP = 0.05;
         public static final double shooterkI = 0.2;
         public static final double shooterkD = 0.0;
@@ -595,6 +601,11 @@ public static HashMap<Double, Double> aimerAvoidElevatorTable() {
 
             return map;
         }
+
+        // TODO: tune
+        public static final double allotedArmMotorErrorTime = 3.0;
+        public static final double allottedArmMotorCurrentDifference = 10;
+        public static final double voltageErrorCheckingThreshold = 2;
     }
 
     public static final class LEDConstants {

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

@@ -11,6 +11,7 @@
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.DutyCycleEncoder;
+import edu.wpi.first.wpilibj.Timer;
 import frc.robot.Constants.ScoringConstants;
 
 public class AimerIOTalon implements AimerIO {
@@ -34,6 +35,12 @@ public class AimerIOTalon implements AimerIO {
     double lastPosition = 0.0;
     double lastTime = Utils.getCurrentTimeSeconds();
 
+    boolean motorLeftFailure = false, motorRightFailure = false;
+    boolean motorCheckOverriden = false;
+
+    private Timer errorTimeLeft = new Timer();
+    private Timer errorTimeRight = new Timer();
+
     public AimerIOTalon() {
         aimerRight.setControl(new Follower(ScoringConstants.aimLeftMotorId, true));
 
@@ -75,6 +82,11 @@ public AimerIOTalon() {
 
         encoder.setDistancePerRotation(2 * Math.PI);
         encoder.setPositionOffset(ScoringConstants.aimerEncoderOffset);
+
+        errorTimeLeft.reset();
+        errorTimeLeft.start();
+        errorTimeRight.reset();
+        errorTimeRight.start();
     }
 
     @Override
@@ -126,12 +138,93 @@ public void setFF(double kS, double kV, double kA, double kG) {
         aimerRight.getConfigurator().apply(slot0);
     }
 
+    private boolean checkForAimMotorFailure() {
+
+        // LEFT MOTOR
+        if (!motorFailureCheckPair(aimerLeft, aimerRight)) {
+            errorTimeLeft.reset();
+            errorTimeLeft.start();
+        } else {
+            if (errorTimeLeft.get() > ScoringConstants.allotedArmMotorErrorTime) {
+                motorLeftFailure = true;
+            }
+        }
+
+        // RIGHT MOTOR
+        if (!motorFailureCheckPair(aimerRight, aimerLeft)) {
+            errorTimeRight.reset();
+            errorTimeRight.start();
+        } else {
+            if (errorTimeRight.get() > ScoringConstants.allotedArmMotorErrorTime) {
+                motorRightFailure = true;
+            }
+        }
+
+        if (!motorCheckOverriden) {
+            shutOffFaultyAimMotors();
+        }
+
+        return motorLeftFailure || motorRightFailure;
+    }
+
+    private boolean motorFailureCheckPair(TalonFX check, TalonFX compare) {
+        if (!check.isAlive()) {
+            // motor no longer communicating with robot
+            return true;
+        }
+
+        if (Math.abs(compare.getStatorCurrent().getValueAsDouble())
+                                - Math.abs(check.getStatorCurrent().getValueAsDouble())
+                        > ScoringConstants.allottedArmMotorCurrentDifference
+                && Math.abs(check.getMotorVoltage().getValueAsDouble())
+                        > ScoringConstants.voltageErrorCheckingThreshold) {
+            // motor voltage is really small when it shouldn't be
+            return true;
+        }
+
+        if (Math.abs(check.getStatorCurrent().getValueAsDouble())
+                                - Math.abs(compare.getStatorCurrent().getValueAsDouble())
+                        > ScoringConstants.allottedArmMotorCurrentDifference
+                && Math.abs(check.getMotorVoltage().getValueAsDouble())
+                        < ScoringConstants.voltageErrorCheckingThreshold) {
+            // motor voltage is really large when it shouldn't be
+            return true;
+        }
+        return false;
+    }
+
+    private void shutOffFaultyAimMotors() {
+        if (motorLeftFailure) {
+            aimerLeft.setVoltage(0);
+        }
+        if (motorRightFailure) {
+            aimerRight.setVoltage(0);
+        }
+        if (motorLeftFailure || motorRightFailure) {
+            setFF(
+                    ScoringConstants.aimerFaultkS,
+                    ScoringConstants.aimerFaultkV,
+                    ScoringConstants.aimerFaultkA,
+                    ScoringConstants.aimerFaultkG);
+        }
+    }
+
     @Override
     public void updateInputs(AimerIOInputs inputs) {
+        checkForAimMotorFailure();
+
         if (override) {
-            aimerLeft.setVoltage(overrideVolts);
+            if (!motorLeftFailure || motorCheckOverriden) {
+                aimerLeft.setVoltage(overrideVolts);
+            } else {
+                aimerRight.setVoltage(overrideVolts);
+            }
         } else {
-            aimerLeft.setControl(controller.withPosition(goalAngleRad));
+            if (!motorLeftFailure || motorCheckOverriden) {
+                aimerLeft.setControl(controller.withPosition(goalAngleRad));
+            } else {
+                aimerRight.setControl(controller.withPosition(goalAngleRad));
+            }
         }
 
         inputs.aimGoalAngleRad = goalAngleRad;
@@ -146,8 +239,14 @@ public void updateInputs(AimerIOInputs inputs) {
         lastPosition = encoder.getAbsolutePosition();
         inputs.aimVelocityRadPerSec = 0.0;
 
-        inputs.aimAppliedVolts = aimerLeft.getMotorVoltage().getValueAsDouble();
-        inputs.aimStatorCurrentAmps = aimerLeft.getStatorCurrent().getValueAsDouble();
-        inputs.aimSupplyCurrentAmps = aimerLeft.getSupplyCurrent().getValueAsDouble();
+        if (!motorLeftFailure || motorCheckOverriden) {
+            inputs.aimAppliedVolts = aimerLeft.getMotorVoltage().getValueAsDouble();
+            inputs.aimStatorCurrentAmps = aimerLeft.getStatorCurrent().getValueAsDouble();
+            inputs.aimSupplyCurrentAmps = aimerLeft.getSupplyCurrent().getValueAsDouble();
+        } else {
+            inputs.aimAppliedVolts = aimerRight.getMotorVoltage().getValueAsDouble();
+            inputs.aimStatorCurrentAmps = aimerLeft.getStatorCurrent().getValueAsDouble();
+            inputs.aimSupplyCurrentAmps = aimerLeft.getSupplyCurrent().getValueAsDouble();
+        }
     }
 }