Pit display (#48)

* adapted FaultReporter (originally AdvancedSubsystem), SubsystemFault, SelfChecking classes from Ranger Robotics
* FaultReporter is a singleton with which subsystem can register to periodically perform checks on hardware devices for faults and can register to perform automated subsystem tests (e.g., pre-match checks)
* FaultReporter publishes results via Network Tables and is intended to be used in conjunction with Ranger Robotics’s pit display.

---------

Co-authored-by: mrandal <[email protected]>
Co-authored-by: Stefan Ilic <[email protected]>

.vscode/extensions.json

@@ -2,6 +2,7 @@
     "recommendations": [
         "richardwillis.vscode-spotless-gradle",
         "sonarsource.sonarlint-vscode",
-        "streetsidesoftware.code-spell-checker"
+        "streetsidesoftware.code-spell-checker",
+        "onlyutkarsh.git-config-user-profiles"
     ]
 }

.vscode/settings.json

@@ -65,6 +65,7 @@
     "rezero",
     "scurve",
     "Sedgewick",
+    "selfcheck",
     "setpoint",
     "setpoints",
     "SIMBOT",

README.md

@@ -121,3 +121,4 @@ To add an additional robot, create a new subclass of ```RobotConfig``` (you can
 * AdvantageKit-enabled pneumatics classes from Mechanical Advantage's 2022 [robot code](https://github.com/Mechanical-Advantage/RobotCode2022)
 * FaultReporter (originally AdvancedSubsystem), SubsystemFault, SelfChecking classes from [Ranger Robotics](https://github.com/3015RangerRobotics/2023Public)
 * Talon factories from Citrus Circuits 2022 [robot code](https://github.com/frc1678/C2022)
+* FaultReporter (originally AdvancedSubsystem), SubsystemFault, SelfChecking classes from Ranger Robotics 2023 [robot code](https://github.com/3015RangerRobotics/2023Public)

networktables.json

@@ -0,0 +1 @@
+[]

simgui.json

@@ -15,6 +15,17 @@
       "/LiveWindow/Ungrouped/PIDController[9]": "PIDController",
       "/LiveWindow/Ungrouped/Scheduler": "Scheduler",
       "/Shuffleboard/MAIN/SendableChooser[0]": "String Chooser",
+      "/Shuffleboard/Subsystem/Subsystem": "Subsystem",
+      "/SmartDashboard//SystemStatus/Drivetrain/SystemCheck": "Command",
+      "/SmartDashboard//SystemStatus/SwerveModule0/SystemCheck": "Command",
+      "/SmartDashboard//SystemStatus/SwerveModule1/SystemCheck": "Command",
+      "/SmartDashboard//SystemStatus/SwerveModule2/SystemCheck": "Command",
+      "/SmartDashboard//SystemStatus/SwerveModule3/SystemCheck": "Command",
+      "/SmartDashboard//SystemStatusDrivetrain/SystemCheck": "Command",
+      "/SmartDashboard//SystemStatusSwerveModule0/SystemCheck": "Command",
+      "/SmartDashboard//SystemStatusSwerveModule1/SystemCheck": "Command",
+      "/SmartDashboard//SystemStatusSwerveModule2/SystemCheck": "Command",
+      "/SmartDashboard//SystemStatusSwerveModule3/SystemCheck": "Command",
       "/SmartDashboard/Auto Routine": "String Chooser",
       "/SmartDashboard/PPSwerveControllerCommand_field": "Field2d",
       "/SmartDashboard/simCamera Sim Field": "Field2d"

src/main/java/frc/lib/team3015/subsystem/FaultReporter.java

@@ -0,0 +1,234 @@
+package frc.lib.team3015.subsystem;
+
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.Pigeon2;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.revrobotics.CANSparkMax;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.motorcontrol.PWMMotorController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.lib.team3015.subsystem.selfcheck.*;
+import frc.lib.team6328.util.Alert;
+import java.util.ArrayList;
+import java.util.HashMap;
+import java.util.List;
+import java.util.Map;
+import org.littletonrobotics.junction.Logger;
+
+// derived from 3015's AdvancedSubsystem abstract class
+
+public class FaultReporter {
+  public enum SystemStatus {
+    OK,
+    WARNING,
+    ERROR
+  }
+
+  private static class SubsystemFaults {
+    private List<SubsystemFault> faults = new ArrayList<>();
+    private List<Alert> faultAlerts = new ArrayList<>();
+    private List<SelfChecking> hardware = new ArrayList<>();
+  }
+
+  private static FaultReporter instance = null;
+
+  private static final String CHECK_RAN = "/CheckRan";
+  private static final String SYSTEM_STATUS = "SystemStatus/";
+
+  private final Map<String, SubsystemFaults> subsystemsFaults = new HashMap<>();
+  private final boolean checkErrors;
+
+  private FaultReporter() {
+    this.checkErrors = RobotBase.isReal();
+    setupCallbacks();
+  }
+
+  public static FaultReporter getInstance() {
+    if (instance == null) {
+      instance = new FaultReporter();
+    }
+    return instance;
+  }
+
+  public CommandBase registerSystemCheck(String subsystemName, CommandBase systemCheckCommand) {
+    String statusTable = SYSTEM_STATUS + subsystemName;
+    SubsystemFaults subsystemFaults =
+        subsystemsFaults.getOrDefault(subsystemName, new SubsystemFaults());
+
+    CommandBase wrappedSystemCheckCommand =
+        wrapSystemCheckCommand(subsystemName, systemCheckCommand);
+    wrappedSystemCheckCommand.setName(subsystemName + "Check");
+    SmartDashboard.putData(statusTable + "/SystemCheck", wrappedSystemCheckCommand);
+    Logger.getInstance().recordOutput(statusTable + CHECK_RAN, false);
+
+    subsystemsFaults.put(subsystemName, subsystemFaults);
+
+    return wrappedSystemCheckCommand;
+  }
+
+  private CommandBase wrapSystemCheckCommand(String subsystemName, CommandBase systemCheckCommand) {
+    String statusTable = SYSTEM_STATUS + subsystemName;
+    return Commands.sequence(
+        Commands.runOnce(
+            () -> {
+              Logger.getInstance().recordOutput(statusTable + CHECK_RAN, false);
+              clearFaults(subsystemName);
+              publishStatus();
+            }),
+        systemCheckCommand,
+        Commands.runOnce(
+            () -> {
+              publishStatus();
+              Logger.getInstance().recordOutput(statusTable + CHECK_RAN, true);
+            }));
+  }
+
+  private void setupCallbacks() {
+    CommandScheduler.getInstance()
+        .schedule(
+            Commands.repeatingSequence(
+                    Commands.runOnce(this::checkForFaults), Commands.waitSeconds(0.25))
+                .ignoringDisable(true));
+
+    CommandScheduler.getInstance()
+        .schedule(
+            Commands.repeatingSequence(
+                    Commands.runOnce(this::publishStatus), Commands.waitSeconds(1.0))
+                .ignoringDisable(true));
+  }
+
+  private void publishStatus() {
+    for (Map.Entry<String, SubsystemFaults> entry : subsystemsFaults.entrySet()) {
+      String subsystemName = entry.getKey();
+      SubsystemFaults subsystemFaults = entry.getValue();
+
+      SystemStatus status = getSystemStatus(subsystemFaults.faults);
+
+      String statusTable = SYSTEM_STATUS + subsystemName;
+      Logger.getInstance().recordOutput(statusTable + "/Status", status.name());
+      Logger.getInstance().recordOutput(statusTable + "/SystemOK", status == SystemStatus.OK);
+
+      String[] faultStrings = new String[subsystemFaults.faults.size()];
+      for (int i = 0; i < subsystemFaults.faults.size(); i++) {
+        SubsystemFault fault = subsystemFaults.faults.get(i);
+        faultStrings[i] = String.format("[%.2f] %s", fault.timestamp, fault.description);
+      }
+      Logger.getInstance().recordOutput(statusTable + "/Faults", faultStrings);
+
+      if (faultStrings.length > 0) {
+        Logger.getInstance()
+            .recordOutput(statusTable + "/LastFault", faultStrings[faultStrings.length - 1]);
+      } else {
+        Logger.getInstance().recordOutput(statusTable + "/LastFault", "");
+      }
+    }
+  }
+
+  public void addFault(String subsystemName, SubsystemFault fault) {
+    SubsystemFaults subsystems = subsystemsFaults.get(subsystemName);
+    List<SubsystemFault> subsystemFaults = subsystems.faults;
+    List<Alert> subsystemAlerts = subsystems.faultAlerts;
+    if (!subsystemFaults.contains(fault)) {
+      subsystemFaults.add(fault);
+
+      Alert alert =
+          new Alert(
+              subsystemName + ": " + fault.description,
+              fault.isWarning ? Alert.AlertType.WARNING : Alert.AlertType.ERROR);
+      alert.set(true);
+      subsystemAlerts.add(alert);
+    }
+  }
+
+  public void addFault(String subsystemName, String description, boolean isWarning) {
+    this.addFault(subsystemName, new SubsystemFault(description, isWarning));
+  }
+
+  public void addFault(
+      String subsystemName, String description, boolean isWarning, boolean sticky) {
+    this.addFault(subsystemName, new SubsystemFault(description, isWarning, sticky));
+  }
+
+  public void addFault(String subsystemName, String description) {
+    this.addFault(subsystemName, description, false);
+  }
+
+  public List<SubsystemFault> getFaults(String subsystemName) {
+    return subsystemsFaults.get(subsystemName).faults;
+  }
+
+  public void clearFaults(String subsystemName) {
+    subsystemsFaults.get(subsystemName).faults.clear();
+  }
+
+  private SystemStatus getSystemStatus(List<SubsystemFault> subsystemFaults) {
+    SystemStatus worstStatus = SystemStatus.OK;
+
+    for (SubsystemFault f : subsystemFaults) {
+      if (f.sticky || f.timestamp > Timer.getFPGATimestamp() - 10) {
+        if (f.isWarning) {
+          if (worstStatus != SystemStatus.ERROR) {
+            worstStatus = SystemStatus.WARNING;
+          }
+        } else {
+          worstStatus = SystemStatus.ERROR;
+        }
+      }
+    }
+    return worstStatus;
+  }
+
+  public void registerHardware(String subsystemName, String label, TalonFX phoenixMotor) {
+    SubsystemFaults subsystemFaults =
+        subsystemsFaults.getOrDefault(subsystemName, new SubsystemFaults());
+    subsystemFaults.hardware.add(new SelfCheckingPhoenixMotor(label, phoenixMotor));
+    subsystemsFaults.put(subsystemName, subsystemFaults);
+  }
+
+  public void registerHardware(String subsystemName, String label, PWMMotorController pwmMotor) {
+    SubsystemFaults subsystemFaults =
+        subsystemsFaults.getOrDefault(subsystemName, new SubsystemFaults());
+    subsystemFaults.hardware.add(new SelfCheckingPWMMotor(label, pwmMotor));
+    subsystemsFaults.put(subsystemName, subsystemFaults);
+  }
+
+  public void registerHardware(String subsystemName, String label, CANSparkMax spark) {
+    SubsystemFaults subsystemFaults =
+        subsystemsFaults.getOrDefault(subsystemName, new SubsystemFaults());
+    subsystemFaults.hardware.add(new SelfCheckingSparkMax(label, spark));
+    subsystemsFaults.put(subsystemName, subsystemFaults);
+  }
+
+  public void registerHardware(String subsystemName, String label, Pigeon2 pigeon2) {
+    SubsystemFaults subsystemFaults =
+        subsystemsFaults.getOrDefault(subsystemName, new SubsystemFaults());
+    subsystemFaults.hardware.add(new SelfCheckingPigeon2(label, pigeon2));
+    subsystemsFaults.put(subsystemName, subsystemFaults);
+  }
+
+  public void registerHardware(String subsystemName, String label, CANcoder canCoder) {
+    SubsystemFaults subsystemFaults =
+        subsystemsFaults.getOrDefault(subsystemName, new SubsystemFaults());
+    subsystemFaults.hardware.add(new SelfCheckingCANCoder(label, canCoder));
+    subsystemsFaults.put(subsystemName, subsystemFaults);
+  }
+
+  // Method to check for faults while the robot is operating normally
+  private void checkForFaults() {
+    if (checkErrors) {
+      for (Map.Entry<String, SubsystemFaults> entry : subsystemsFaults.entrySet()) {
+        String subsystemName = entry.getKey();
+        SubsystemFaults subsystemFaults = entry.getValue();
+        for (SelfChecking device : subsystemFaults.hardware) {
+          for (SubsystemFault fault : device.checkForFaults()) {
+            addFault(subsystemName, fault);
+          }
+        }
+      }
+    }
+  }
+}

src/main/java/frc/lib/team3015/subsystem/SubsystemFault.java

@@ -0,0 +1,52 @@
+package frc.lib.team3015.subsystem;
+
+import edu.wpi.first.wpilibj.Timer;
+import java.util.Objects;
+
+public class SubsystemFault {
+  public final String description;
+  public final double timestamp;
+  public final boolean isWarning;
+  public final boolean sticky;
+
+  public SubsystemFault(String description, boolean isWarning) {
+    // default sticky to true
+    this(description, isWarning, true);
+  }
+
+  public SubsystemFault(String description) {
+    this(description, false);
+  }
+
+  public SubsystemFault(String description, boolean isWarning, boolean sticky) {
+    this.description = description;
+    this.timestamp = Timer.getFPGATimestamp();
+    this.isWarning = isWarning;
+    this.sticky = sticky;
+  }
+
+  @Override
+  public boolean equals(Object other) {
+    if (this == other) {
+      return true;
+    }
+
+    if (other == null) {
+      return false;
+    }
+
+    if (getClass() != other.getClass()) {
+      return false;
+    }
+
+    SubsystemFault otherSubsystemFault = (SubsystemFault) other;
+
+    return description.equals(otherSubsystemFault.description)
+        && isWarning == otherSubsystemFault.isWarning;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(description, timestamp, isWarning, sticky);
+  }
+}

src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfChecking.java

@@ -0,0 +1,8 @@
+package frc.lib.team3015.subsystem.selfcheck;
+
+import frc.lib.team3015.subsystem.SubsystemFault;
+import java.util.List;
+
+public interface SelfChecking {
+  List<SubsystemFault> checkForFaults();
+}

src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingCANCoder.java

@@ -0,0 +1,40 @@
+package frc.lib.team3015.subsystem.selfcheck;
+
+import com.ctre.phoenix6.hardware.CANcoder;
+import frc.lib.team3015.subsystem.SubsystemFault;
+import java.util.ArrayList;
+import java.util.List;
+
+public class SelfCheckingCANCoder implements SelfChecking {
+  private final String label;
+  private final CANcoder canCoder;
+
+  public SelfCheckingCANCoder(String label, CANcoder canCoder) {
+    this.label = label;
+    this.canCoder = canCoder;
+  }
+
+  @Override
+  public List<SubsystemFault> checkForFaults() {
+    List<SubsystemFault> faults = new ArrayList<>();
+
+    if (canCoder.getFault_Hardware().getValue()) {
+      faults.add(new SubsystemFault(String.format("[%s]: Hardware fault detected", label)));
+    }
+    if (canCoder.getFault_BootDuringEnable().getValue()) {
+      faults.add(new SubsystemFault(String.format("[%s]: Device booted while enabled", label)));
+    }
+    if (canCoder.getFault_BadMagnet().getValue()) {
+      faults.add(new SubsystemFault(String.format("[%s]: Bad magnet", label)));
+    }
+    if (canCoder.getFault_Undervoltage().getValue()) {
+      faults.add(
+          new SubsystemFault(String.format("[%s]: Device supply voltage near brownout", label)));
+    }
+    if (canCoder.getFault_UnlicensedFeatureInUse().getValue()) {
+      faults.add(new SubsystemFault(String.format("[%s]: Unlicensed feature in use", label)));
+    }
+
+    return faults;
+  }
+}