Aim motor adjustments in case of failure

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

@@ -445,6 +445,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;

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

@@ -34,6 +34,9 @@ public class AimerIOTalon implements AimerIO {
     double lastPosition = 0.0;
     double lastTime = Utils.getCurrentTimeSeconds();
 
+    boolean motorLeftFailure = false, motorRightFailure = false;
+    boolean motorCheckOverriden = false;
+
     public AimerIOTalon() {
         aimerRight.setControl(new Follower(ScoringConstants.aimLeftMotorId, true));
 
@@ -126,12 +129,81 @@ public void setFF(double kS, double kV, double kA, double kG) {
         aimerRight.getConfigurator().apply(slot0);
     }
 
+    private boolean checkForAimMotorFailure() {
+
+        /*if (!aimerLeft.isAlive() || !aimerRight.isAlive()) {
+            if (!motorCheckOverriden) setFF(ScoringConstants.aimerFaultkS, ScoringConstants.aimerFaultkV, ScoringConstants.aimerFaultkA, ScoringConstants.aimerFaultkG);
+            motorFailure = true;
+        } else if (motorFailure) {
+            if (!motorCheckOverriden) setFF(ScoringConstants.aimerkS, ScoringConstants.aimerkV, ScoringConstants.aimerkA, ScoringConstants.aimerkG);
+            motorFailure = false;
+        }
+        return motorFailure;*/
+
+        // LEFT MOTOR
+        if ((!aimerLeft.isAlive())
+                || // if motor disconnected
+                (Math.abs(aimerRight.getStatorCurrent().getValueAsDouble())
+                                        - Math.abs(aimerLeft.getStatorCurrent().getValueAsDouble())
+                                > 1
+                        && Math.abs(aimerLeft.getMotorVoltage().getValueAsDouble()) > 0.5)
+                || // if motor voltage is really small when it shouldn't be
+                (Math.abs(aimerLeft.getStatorCurrent().getValueAsDouble())
+                                        - Math.abs(aimerRight.getStatorCurrent().getValueAsDouble())
+                                > 1
+                        && Math.abs(aimerLeft.getMotorVoltage().getValueAsDouble())
+                                < 0.5) // if motor voltage is really large when it shouldn't be
+        ) {
+            motorLeftFailure = true;
+        }
+
+        // RIGHT MOTOR
+        if ((!aimerRight.isAlive())
+                || // if motor disconnected
+                (Math.abs(aimerLeft.getStatorCurrent().getValueAsDouble())
+                                        - Math.abs(aimerRight.getStatorCurrent().getValueAsDouble())
+                                > 1
+                        && Math.abs(aimerRight.getMotorVoltage().getValueAsDouble()) > 0.5)
+                || // if motor voltage is really small when it shouldn't be
+                (Math.abs(aimerRight.getStatorCurrent().getValueAsDouble())
+                                        - Math.abs(aimerLeft.getStatorCurrent().getValueAsDouble())
+                                > 1
+                        && Math.abs(aimerRight.getMotorVoltage().getValueAsDouble())
+                                < 0.5) // if motor voltage is really large when it shouldn't be
+        ) {
+            motorRightFailure = true;
+        }
+
+        if (!motorCheckOverriden) shutOffFaultyAimMotors();
+
+        return motorLeftFailure || motorRightFailure;
+    }
+
+    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) {
         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 +218,16 @@ 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();
+        }
+
+        checkForAimMotorFailure();
     }
 }