fixed ignored self check error

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

@@ -23,14 +23,6 @@ public abstract class AdvancedSubsystem extends SubsystemBase
 
   public AdvancedSubsystem() {}
 
-  /** 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) {
     _hasError = (faultType == FaultType.ERROR);
@@ -41,11 +33,24 @@ protected final void addFault(String description, FaultType faultType) {
     _faultsTable.set(_faults);
   }
 
+  /** Clears this subsystem's faults. */
+  public final void clearFaults() {
+    _faults.clear();
+    _faultsTable.set(_faults);
+
+    _hasError = false;
+  }
+
   /** Returns the faults belonging to this subsystem. */
   public final Set<Fault> getFaults() {
     return _faults;
   }
 
+  /** Returns whether this subsystem contains the following fault. */
+  public final boolean hasFault(String description, FaultType faultType) {
+    return _faults.contains(new Fault(description, faultType));
+  }
+
   /** Returns whether this subsystem has errors (has fault type of error). */
   public final boolean hasError() {
     return _hasError;

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

@@ -3,17 +3,34 @@
 import static edu.wpi.first.wpilibj2.command.Commands.*;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.lib.FaultsTable.FaultType;
 import java.util.function.BiConsumer;
 
 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
+   * @param faults A consumer that adds a given fault to the subsystem with the specified fault
    *     type.
    */
-  public default Command selfCheck(BiConsumer<String, FaultType> faultAdder) {
+  public default Command selfCheck(BiConsumer<String, FaultType> faults) {
     return none();
   }
+
+  /**
+   * Places an empty instant command at the front of a sequential command group.
+   *
+   * <p>This is useful because the second self check command gets called in the same iteration as
+   * the first one (due to the nature of sequential command groups in wpilib).
+   *
+   * @param commands The commands to include in this composition.
+   */
+  public default Command shiftSequence(Command... commands) {
+    var shiftedSequence = new SequentialCommandGroup(none());
+
+    shiftedSequence.addCommands(commands);
+
+    return shiftedSequence;
+  }
 }

src/test/java/frc/lib/AdvancedSubsystemTest.java

@@ -6,10 +6,14 @@
 
 import static edu.wpi.first.wpilibj2.command.Commands.*;
 import static frc.lib.UnitTestingUtil.*;
+import static frc.lib.UnitTestingUtil.run;
+import static org.junit.jupiter.api.Assertions.assertEquals;
 
+import dev.doglog.DogLog;
+import dev.doglog.DogLogOptions;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.lib.FaultsTable.Fault;
 import frc.lib.FaultsTable.FaultType;
 import java.util.function.BiConsumer;
 import org.junit.jupiter.api.AfterEach;
@@ -39,15 +43,66 @@ public void robotIsEnabled() {
   }
 
   @Test
-  public void subsystemSelfCheck() {
-    runToCompletion(_sub.fullSelfCheck());
+  public void addFault() {
+    _sub.addFault("FAULT 1", FaultType.ERROR);
+
+    assertEquals(_sub.getFaults().size(), 1, DELTA);
+
+    assert _sub.hasFault("FAULT 1", FaultType.ERROR);
+  }
+
+  @Test
+  public void duplicateFaults() {
+    _sub.addFault("Fault 1", FaultType.ERROR);
+    _sub.addFault("Fault 1", FaultType.ERROR);
+
+    assertEquals(_sub.getFaults().size(), 1, DELTA);
+  }
+
+  @Test
+  public void clearFaults() {
+    _sub.addFault("FAULT 1", FaultType.WARNING);
+    _sub.addFault("FAULT 2", FaultType.ERROR);
+
+    _sub.clearFaults();
+
+    assertEquals(_sub.getFaults().size(), 0, DELTA);
+  }
+
+  @Test
+  public void hasError() {
+    _sub.addFault("FAULT 1", FaultType.ERROR);
 
-    // should give FAULT 1 error
     assert _sub.hasError();
+  }
+
+  @Test
+  public void selfCheckFinish() {
+    runToCompletion(_sub.fullSelfCheck());
+
+    // should give FAULT 3 error
+    assertEquals(_sub.getFaults().size(), 3, DELTA);
+
+    // should contain these faults
+    assert _sub.hasFault("FAULT 1", FaultType.WARNING);
+    assert _sub.hasFault("FAULT 2", FaultType.WARNING);
+    assert _sub.hasFault("FAULT 3", FaultType.ERROR);
+  }
+
+  @Test
+  public void currentCommandName() {
+    DogLog.setOptions(new DogLogOptions().withNtPublish(true));
+
+    var name =
+        NetworkTableInstance.getDefault()
+            .getTable("/Robot/TestImpl")
+            .getStringTopic("Current Command")
+            .subscribe("");
+    var test = idle(_sub).withName("Test Command");
+
+    run(test, 3);
 
-    // should only give FAULT 1 error and stop there
-    assert _sub.getFaults().contains(new Fault("FAULT 1", FaultType.ERROR));
-    assert !_sub.getFaults().contains(new Fault("FAULT 2", FaultType.ERROR));
+    assertEquals("Test Command", name.get());
   }
 
   public class TestImpl extends AdvancedSubsystem {
@@ -61,27 +116,27 @@ private interface BaseIO extends SelfChecked {
     private class TestIO implements BaseIO {
       private final boolean _fault1 = true;
       private final boolean _fault2 = true;
-      private final boolean _fault3 = false;
+      private final boolean _fault3 = true;
 
       @Override
       public double getEncoderSpeed() {
         return 1.0;
       }
 
       @Override
-      public Command selfCheck(BiConsumer<String, FaultType> faultAdder) {
-        return sequence(
+      public Command selfCheck(BiConsumer<String, FaultType> faults) {
+        return shiftSequence(
             runOnce(
                 () -> {
-                  if (_fault1) faultAdder.accept("FAULT 1", FaultType.ERROR);
+                  if (_fault1) faults.accept("FAULT 1", FaultType.WARNING);
                 }),
             runOnce(
                 () -> {
-                  if (_fault2) faultAdder.accept("FAULT 2", FaultType.ERROR);
+                  if (_fault2) faults.accept("FAULT 2", FaultType.WARNING);
                 }),
             runOnce(
                 () -> {
-                  if (_fault3) faultAdder.accept("FAULT 3", FaultType.WARNING);
+                  if (_fault3) faults.accept("FAULT 3", FaultType.ERROR);
                 }));
       }
     }
@@ -92,12 +147,12 @@ public double speed() {
     }
 
     @Override
-    public Command selfCheck(BiConsumer<String, FaultType> faultAdder) {
-      return sequence(
-          _io.selfCheck(faultAdder), // self check io devices first
+    public Command selfCheck(BiConsumer<String, FaultType> faults) {
+      return shiftSequence(
+          _io.selfCheck(faults), // self check io devices first
           runOnce(
               () -> {
-                if (speed() < 2) faultAdder.accept("TOO SLOW", FaultType.WARNING);
+                if (speed() < 2) faults.accept("TOO SLOW", FaultType.WARNING);
               }) // then check the whole subsystem
           );
     }