added back in old swervebase self check, wpilib bug still present

src/main/java/frc/lib/AdvancedSubsystem.java

@@ -0,0 +1,69 @@
+package frc.lib;
+
+import java.util.HashSet;
+import java.util.Set;
+
+import dev.doglog.DogLog;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
+import frc.lib.FaultsTable.Fault;
+import frc.lib.FaultsTable.FaultType;
+
+public class AdvancedSubsystem extends SubsystemBase implements SelfChecked {
+  // faults and the table containing them
+  private Set<Fault> _faults = new HashSet<Fault>();
+  private FaultsTable _faultsTable = new FaultsTable(NetworkTableInstance.getDefault().getTable("Self Check"), getName() + " Faults");
+
+  private boolean _hasError = false;
+
+  public AdvancedSubsystem() {
+    RobotModeTriggers.test().onFalse(runOnce(this::clearFaults));
+  }
+
+  /** Clears this subsystem's faults. */
+  protected final void clearFaults() {
+    _faults.clear();
+    _faultsTable.set(_faults);
+
+    _hasError = false;
+  }
+
+  /** Adds a new fault under this subsystem. */
+  protected final void addFault(String description, FaultType faultType) {
+    Fault fault = new Fault(description, faultType);
+
+    _faults.add(fault);
+    _faultsTable.set(_faults);
+
+    _hasError = faultType == FaultType.ERROR;
+  }
+
+  /** Returns whether this subsystem has errors (has fault type of error). */
+  public final boolean hasError() {
+    return _hasError;
+  }
+
+  /** Returns a full Command that self checks this Subsystem for pre-match. */
+  public final Command fullSelfCheck() {    
+    Command selfCheck = Commands.sequence(
+      runOnce(this::clearFaults), // clear all faults and hasError (also adds this subsystem as a requirement)
+      selfCheck(this::addFault).until(this::hasError) // self check this subsystem
+    ).withName(getName() + " Self Check");
+
+    return selfCheck;
+  }
+
+  @Override
+  public void periodic() {
+    String currentCommandName = "None";
+
+    if (getCurrentCommand() != null) {
+      currentCommandName = getCurrentCommand().getName();
+    }
+
+    DogLog.log(getName() + "/Current Command", currentCommandName);
+  }
+}

src/main/java/frc/lib/SelfChecked.java

@@ -0,0 +1,18 @@
+package frc.lib;
+
+import java.util.function.BiConsumer;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.lib.FaultsTable.FaultType;
+
+public interface SelfChecked {
+    /**
+     * Returns a Command that self checks this system.
+     * 
+     * @param faultAdder A consumer that adds a given fault to the subsystem with the specified fault type.
+     */
+    public default Command selfCheck(BiConsumer<String, FaultType> faultAdder) {
+      return Commands.none();
+    }
+}

src/main/java/frc/robot/Robot.java

@@ -5,16 +5,19 @@
 package frc.robot;
 
 import static edu.wpi.first.units.Units.*;
+import static edu.wpi.first.wpilibj2.command.Commands.*;
 
 import com.ctre.phoenix6.SignalLogger;
 import dev.doglog.DogLog;
 import dev.doglog.DogLogOptions;
 import edu.wpi.first.epilogue.Epilogue;
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.epilogue.Logged.Strategy;
+import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -25,6 +28,7 @@
 import frc.robot.commands.Autos;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
+import frc.robot.subsystems.AdvancedTest;
 
 /**
  * The methods in this class are called automatically corresponding to each mode, as described in
@@ -41,6 +45,8 @@ public class Robot extends TimedRobot {
   @Logged(name = "Swerve")
   private CommandSwerveDrivetrain _swerve = TunerConstants.createDrivetrain();
 
+  private AdvancedTest _advancedTest = new AdvancedTest();
+
   private Command _autonomousCommand = Autos.none();
 
   /**
@@ -57,6 +63,12 @@ public Robot() {
 
     configureBindings();
 
+    SmartDashboard.putData("Robot Self Check", sequence(
+      runOnce(() -> DataLogManager.log("Robot Self Check Started!")),
+      _advancedTest.fullSelfCheck(),
+      runOnce(() -> DataLogManager.log("Robot Self Check Successful!"))
+    ));
+
     addPeriodic(FaultLogger::update, 1);
   }
 

src/main/java/frc/robot/subsystems/AdvancedTest.java

@@ -0,0 +1,52 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+import java.util.function.BiConsumer;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.lib.AdvancedSubsystem;
+import frc.lib.SelfChecked;
+import frc.lib.FaultsTable.FaultType;
+
+/** Advanced Subsystem Test (self check test) */
+public class AdvancedTest extends AdvancedSubsystem {
+    private final SomeIO _io = new SomeIO();
+
+    // represents IO, which can either be talonfx/rev or none
+    private class SomeIO implements SelfChecked {
+        private final boolean _fault1 = true;
+        private final boolean _fault2 = true;
+        private final boolean _fault3 = false;
+
+        public double readSomeSensor() {
+            return 1.0;
+        }
+
+        @Override
+        public Command selfCheck(BiConsumer<String, FaultType> faultAdder) {
+            return Commands.sequence(
+                Commands.runOnce(() -> { if (_fault1) faultAdder.accept("FAULT 1", FaultType.ERROR); }),
+                Commands.runOnce(() -> { if (_fault2) faultAdder.accept("FAULT 2", FaultType.ERROR); }),
+                Commands.runOnce(() -> { if (_fault3) faultAdder.accept("FAULT 3", FaultType.WARNING); })
+            );
+        }
+    }
+
+    public double speed() {
+        return _io.readSomeSensor();
+    }
+
+    @Override
+    public Command selfCheck(BiConsumer<String, FaultType> faultAdder) {
+        // return Commands.sequence(
+        //     _io.selfCheck(faultAdder), // self check io devices first
+        //     runOnce(() -> { if (speed() != 2) faultAdder.accept("TOO SLOW", FaultType.WARNING); }) // then check the whole subsystem
+        // );
+        return _io.selfCheck(faultAdder);
+    }
+}
+

src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java

@@ -31,12 +31,13 @@
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.lib.FaultLogger;
 import frc.lib.InputStream;
+import frc.lib.SelfChecked;
 import frc.robot.Constants;
 import frc.robot.Constants.SwerveConstants;
 import frc.robot.utils.SysId;
 
 @Logged(strategy = Strategy.OPT_IN)
-public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsystem {
+public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsystem, SelfChecked {
   // teleop requests
   private final RobotCentric _robotCentricRequest = new RobotCentric();
   private final FieldCentric _fieldCentricRequest = new FieldCentric();