diff --git a/src/main/java/frc/lib/AdvancedSubsystem.java b/src/main/java/frc/lib/AdvancedSubsystem.java index e1ab22d..f461d46 100644 --- a/src/main/java/frc/lib/AdvancedSubsystem.java +++ b/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 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; diff --git a/src/main/java/frc/lib/SelfChecked.java b/src/main/java/frc/lib/SelfChecked.java index 2b1e21d..1ae5e73 100644 --- a/src/main/java/frc/lib/SelfChecked.java +++ b/src/main/java/frc/lib/SelfChecked.java @@ -3,6 +3,7 @@ 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; @@ -10,10 +11,26 @@ 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 faultAdder) { + public default Command selfCheck(BiConsumer faults) { return none(); } + + /** + * Places an empty instant command at the front of a sequential command group. + * + *

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; + } } diff --git a/src/test/java/frc/lib/AdvancedSubsystemTest.java b/src/test/java/frc/lib/AdvancedSubsystemTest.java index 8f9f88e..db6deb6 100644 --- a/src/test/java/frc/lib/AdvancedSubsystemTest.java +++ b/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,7 +116,7 @@ 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() { @@ -69,19 +124,19 @@ public double getEncoderSpeed() { } @Override - public Command selfCheck(BiConsumer faultAdder) { - return sequence( + public Command selfCheck(BiConsumer 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 faultAdder) { - return sequence( - _io.selfCheck(faultAdder), // self check io devices first + public Command selfCheck(BiConsumer 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 ); }