From 97c2b9631b0e7e00d5fb23e8a37e8a1df7f83257 Mon Sep 17 00:00:00 2001 From: PGgit08 Date: Sat, 14 Dec 2024 19:45:00 -0500 Subject: [PATCH] removed IO concept, and fixed fault logger and advanced subsystem to handle a non-default nt instance in unit tests --- src/main/java/frc/lib/AdvancedSubsystem.java | 36 +++++--- src/main/java/frc/lib/FaultLogger.java | 21 +++-- src/main/java/frc/lib/InputStream.java | 5 +- src/main/java/frc/lib/SelfChecked.java | 9 +- src/main/java/frc/lib/UnitTestingUtil.java | 7 +- src/main/java/frc/robot/Robot.java | 2 + .../java/frc/robot/subsystems/Swerve.java | 30 ++++--- .../java/frc/lib/AdvancedSubsystemTest.java | 82 +++++++------------ src/test/java/frc/lib/CTREUtilTest.java | 6 +- src/test/java/frc/robot/SwerveTest.java | 27 ------ 10 files changed, 103 insertions(+), 122 deletions(-) delete mode 100644 src/test/java/frc/robot/SwerveTest.java diff --git a/src/main/java/frc/lib/AdvancedSubsystem.java b/src/main/java/frc/lib/AdvancedSubsystem.java index 033d5d6..42a62f0 100644 --- a/src/main/java/frc/lib/AdvancedSubsystem.java +++ b/src/main/java/frc/lib/AdvancedSubsystem.java @@ -17,15 +17,31 @@ public abstract class AdvancedSubsystem extends SubsystemBase implements SelfChecked, AutoCloseable { // faults and the table containing them - private Set _faults = new HashSet(); - private FaultsTable _faultsTable = - new FaultsTable( - NetworkTableInstance.getDefault().getTable("Self Check"), getName() + " Faults"); + private final Set _faults = new HashSet(); + private final FaultsTable _faultsTable; @Logged(name = "Has Error") private boolean _hasError = false; - public AdvancedSubsystem() {} + public AdvancedSubsystem() { + this(NetworkTableInstance.getDefault()); + } + + public AdvancedSubsystem(NetworkTableInstance ntInst) { + _faultsTable = new FaultsTable(ntInst.getTable("Self Check"), getName() + " Faults"); + } + + /** + * Returns the name of the command that's currently requiring this subsystem. Is "None" when the + * command in null. + */ + public final String currentCommandName() { + if (getCurrentCommand() != null) { + return getCurrentCommand().getName(); + } + + return "None"; + } /** Adds a new fault under this subsystem. */ protected final void addFault(String description, FaultType faultType) { @@ -65,7 +81,7 @@ public final boolean hasError() { /** Returns a full Command that self checks this Subsystem for pre-match. */ public final Command fullSelfCheck() { Command selfCheck = - sequence(runOnce(this::clearFaults), selfCheck(this::addFault).until(this::hasError)) + sequence(runOnce(this::clearFaults), selfCheck().until(this::hasError)) .withName(getName() + " Self Check"); return selfCheck; @@ -73,12 +89,6 @@ public final Command fullSelfCheck() { @Override public void periodic() { - String currentCommandName = "None"; - - if (getCurrentCommand() != null) { - currentCommandName = getCurrentCommand().getName(); - } - - DogLog.log(getName() + "/Current Command", currentCommandName); + DogLog.log(getName() + "/Current Command", currentCommandName()); } } diff --git a/src/main/java/frc/lib/FaultLogger.java b/src/main/java/frc/lib/FaultLogger.java index 6690ad3..fb9bdfa 100644 --- a/src/main/java/frc/lib/FaultLogger.java +++ b/src/main/java/frc/lib/FaultLogger.java @@ -8,7 +8,6 @@ import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.hardware.TalonFX; import dev.doglog.DogLog; -import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; import frc.lib.FaultsTable.Fault; @@ -45,9 +44,21 @@ public static interface FaultReporter { private static final Set totalFaults = new HashSet<>(); // NETWORK TABLES - private static final NetworkTable base = NetworkTableInstance.getDefault().getTable("Faults"); - private static final FaultsTable activeAlerts = new FaultsTable(base, "Active Faults"); - private static final FaultsTable totalAlerts = new FaultsTable(base, "Total Faults"); + private static FaultsTable activeAlerts; + private static FaultsTable totalAlerts; + + /** Must be called to setup the fault logger. */ + public static void setup(NetworkTableInstance ntInst) { + var base = ntInst.getTable("Faults"); + + activeAlerts = new FaultsTable(base, "Active Faults"); + totalAlerts = new FaultsTable(base, "Total Faults"); + } + + /** Must be called to setup the fault logger. */ + public static void setup() { + setup(NetworkTableInstance.getDefault()); + } /** Whether or not to print new faults into the console. */ public static void enableConsole(boolean enable) { @@ -117,7 +128,7 @@ public static void report(Fault fault) { switch (fault.type()) { case ERROR -> DriverStation.reportError("[Fault Logger] " + fault.toString(), false); case WARNING -> DriverStation.reportWarning("[Fault Logger] " + fault.toString(), false); - case INFO -> System.out.println("[FaultLogger] " + fault.toString()); + case INFO -> System.out.println("[Fault Logger] " + fault.toString()); } } diff --git a/src/main/java/frc/lib/InputStream.java b/src/main/java/frc/lib/InputStream.java index 8b078fc..f2c0287 100644 --- a/src/main/java/frc/lib/InputStream.java +++ b/src/main/java/frc/lib/InputStream.java @@ -176,7 +176,10 @@ public default InputStream rateLimit(double rate) { * @return A stream with the same output as this one. */ public default InputStream log(String key) { - DoublePublisher pub = NetworkTableInstance.getDefault().getDoubleTopic(key).publish(); + DoublePublisher pub = + NetworkTableInstance.getDefault() + .getDoubleTopic(key) + .publish(); // TODO: watch out unit tests return () -> { double val = this.get(); pub.set(val); diff --git a/src/main/java/frc/lib/SelfChecked.java b/src/main/java/frc/lib/SelfChecked.java index 1ae5e73..2dd3eae 100644 --- a/src/main/java/frc/lib/SelfChecked.java +++ b/src/main/java/frc/lib/SelfChecked.java @@ -5,16 +5,13 @@ 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 faults A consumer that adds a given fault to the subsystem with the specified fault - * type. + * Returns a Command that self checks this system. This command should utilize the advanced + * subsytem's {@link AdvancedSubsystem#addFault(String, FaultType)}. */ - public default Command selfCheck(BiConsumer faults) { + public default Command selfCheck() { return none(); } diff --git a/src/main/java/frc/lib/UnitTestingUtil.java b/src/main/java/frc/lib/UnitTestingUtil.java index 32f2630..a4921dc 100644 --- a/src/main/java/frc/lib/UnitTestingUtil.java +++ b/src/main/java/frc/lib/UnitTestingUtil.java @@ -6,6 +6,7 @@ import edu.wpi.first.hal.HAL; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.units.measure.Time; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.simulation.DriverStationSim; import edu.wpi.first.wpilibj.simulation.SimHooks; @@ -37,8 +38,12 @@ public static void setupTests() { DriverStationSim.setEnabled(true); DriverStationSim.notifyNewData(); + assert DriverStation.isEnabled(); + DogLog.setEnabled(false); // disabling doglog since it logs to the default nt instance + FaultLogger.setup(_ntInst); + FaultLogger.clear(); FaultLogger.unregisterAll(); FaultLogger.enableConsole(false); @@ -121,7 +126,7 @@ public static void run(Command command, int runs) { /** * Schedules a command and runs it until it ends. Be careful -- if the command you give never - * ends, this will be an infinate loop! + * ends, this will be an infinite loop! * * @param command */ diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 82884df..41938b6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -57,6 +57,8 @@ public Robot() { DriverStation.silenceJoystickConnectionWarning(isSimulation()); + FaultLogger.setup(); + configureBindings(); SmartDashboard.putData( diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index d7489a0..0b2682a 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -49,7 +49,6 @@ import java.util.HashSet; import java.util.List; import java.util.Set; -import java.util.function.BiConsumer; import org.photonvision.simulation.VisionSystemSim; @Logged(strategy = Strategy.OPT_IN) @@ -58,7 +57,8 @@ public class Swerve extends SwerveDrivetrain implements Subsystem, SelfChecked { private Set _faults = new HashSet(); private FaultsTable _faultsTable = new FaultsTable( - NetworkTableInstance.getDefault().getTable("Self Check"), getName() + " Faults"); + NetworkTableInstance.getDefault().getTable("Self Check"), + getName() + " Faults"); // TODO: watch out if unit testing private boolean _hasError = false; @@ -209,6 +209,18 @@ public Swerve( // COPIED FROM ADVANCED SUBSYSTEM + /** + * Returns the name of the command that's currently requiring this subsystem. Is "None" when the + * command in null. + */ + public final String currentCommandName() { + if (getCurrentCommand() != null) { + return getCurrentCommand().getName(); + } + + return "None"; + } + /** Adds a new fault under this subsystem. */ private final void addFault(String description, FaultType faultType) { _hasError = (faultType == FaultType.ERROR); @@ -247,7 +259,7 @@ public final boolean hasError() { /** Returns a full Command that self checks this Subsystem for pre-match. */ public final Command fullSelfCheck() { Command selfCheck = - sequence(runOnce(this::clearFaults), selfCheck(this::addFault).until(this::hasError)) + sequence(runOnce(this::clearFaults), selfCheck().until(this::hasError)) .withName(getName() + " Self Check"); return selfCheck; @@ -430,13 +442,7 @@ private void updateVisionPoseEstimates() { @Override public void periodic() { // ---- advanced subsystem periodic ---- - String currentCommandName = "None"; - - if (getCurrentCommand() != null) { - currentCommandName = getCurrentCommand().getName(); - } - - DogLog.log(getName() + "/Current Command", currentCommandName); + DogLog.log(getName() + "/Current Command", currentCommandName()); // ---- this subsystem's periodic ---- updateVisionPoseEstimates(); @@ -470,11 +476,11 @@ public void simulationPeriodic() { } private Command selfCheckModule(String name, SwerveModule module) { - return shiftSequence(); + return shiftSequence(); // TODO } @Override - public Command selfCheck(BiConsumer faults) { + public Command selfCheck() { return shiftSequence( // check all modules individually selfCheckModule("Front Left", getModule(0)), diff --git a/src/test/java/frc/lib/AdvancedSubsystemTest.java b/src/test/java/frc/lib/AdvancedSubsystemTest.java index 4c0fc0d..95574d1 100644 --- a/src/test/java/frc/lib/AdvancedSubsystemTest.java +++ b/src/test/java/frc/lib/AdvancedSubsystemTest.java @@ -9,18 +9,14 @@ import static frc.lib.UnitTestingUtil.run; import static org.junit.jupiter.api.Assertions.assertEquals; -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; import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; public class AdvancedSubsystemTest { - static final double DELTA = 1e-2; // acceptable deviation range - private TestImpl _sub; @BeforeEach @@ -35,16 +31,11 @@ public void close() throws Exception { reset(_sub); } - @Test - public void robotIsEnabled() { - assert DriverStation.isEnabled(); - } - @Test public void addFault() { _sub.addFault("FAULT 1", FaultType.ERROR); - assertEquals(_sub.getFaults().size(), 1, DELTA); + assertEquals(1, _sub.getFaults().size()); assert _sub.hasFault("FAULT 1", FaultType.ERROR); } @@ -62,7 +53,7 @@ public void duplicateFaults() { _sub.addFault("FAULT 1", FaultType.ERROR); _sub.addFault("FAULT 1", FaultType.ERROR); - assertEquals(_sub.getFaults().size(), 1, DELTA); + assertEquals(1, _sub.getFaults().size()); } @Test @@ -72,7 +63,7 @@ public void clearFaults() { _sub.clearFaults(); - assertEquals(_sub.getFaults().size(), 0, DELTA); + assertEquals(0, _sub.getFaults().size()); } @Test @@ -87,7 +78,7 @@ public void selfCheckFinish() { runToCompletion(_sub.fullSelfCheck()); // should give FAULT 3 error - assertEquals(_sub.getFaults().size(), 3, DELTA); + assertEquals(3, _sub.getFaults().size()); // should contain these faults assert _sub.hasFault("FAULT 1", FaultType.WARNING); @@ -97,63 +88,46 @@ public void selfCheckFinish() { @Test public void currentCommandName() { + assertEquals("None", _sub.currentCommandName()); // doing nothing + var test = idle(_sub).withName("Test Command"); run(test, 3); - assertEquals("Test Command", _sub.getCurrentCommand().getName()); // TODO + assertEquals("Test Command", _sub.currentCommandName()); } public class TestImpl extends AdvancedSubsystem { - private final BaseIO _io = new TestIO(); - - private interface BaseIO extends SelfChecked { - public double getEncoderSpeed(); - } - - // irl this would be for example "TalonIO" (handles real/sim) or "NoneIO" (handles no subsystem) - private class TestIO implements BaseIO { - private final boolean _fault1 = true; - private final boolean _fault2 = true; - private final boolean _fault3 = true; - - @Override - public double getEncoderSpeed() { - return 1.0; - } - - @Override - public Command selfCheck(BiConsumer faults) { - return shiftSequence( - runOnce( - () -> { - if (_fault1) faults.accept("FAULT 1", FaultType.WARNING); - }), - runOnce( - () -> { - if (_fault2) faults.accept("FAULT 2", FaultType.WARNING); - }), - runOnce( - () -> { - if (_fault3) faults.accept("FAULT 3", FaultType.ERROR); - })); - } + public TestImpl() { + super(UnitTestingUtil.getNtInst()); } - // uses the io public double speed() { - return _io.getEncoderSpeed(); + return 0; } @Override - public Command selfCheck(BiConsumer faults) { + public Command selfCheck() { return shiftSequence( - _io.selfCheck(faults), // self check io devices first + // check devices first + runOnce( + () -> { + if (true) addFault("FAULT 1", FaultType.WARNING); + }), + runOnce( + () -> { + if (true) addFault("FAULT 2", FaultType.WARNING); + }), + runOnce( + () -> { + if (true) addFault("FAULT 3", FaultType.ERROR); + }), + + // then check the subsystem runOnce( () -> { - if (speed() < 2) faults.accept("TOO SLOW", FaultType.WARNING); - }) // then check the whole subsystem - ); + if (speed() < 2) addFault("TOO SLOW", FaultType.WARNING); + })); } @Override diff --git a/src/test/java/frc/lib/CTREUtilTest.java b/src/test/java/frc/lib/CTREUtilTest.java index bfd1c5a..0940c25 100644 --- a/src/test/java/frc/lib/CTREUtilTest.java +++ b/src/test/java/frc/lib/CTREUtilTest.java @@ -41,21 +41,21 @@ public void close() throws Exception { public void motorName() { var name = CTREUtil.getName(_motor); - assertEquals(name, "TalonFX (1)"); + assertEquals("TalonFX (1)", name); } @Test public void gyroName() { var name = CTREUtil.getName(_gyro); - assertEquals(name, "Pigeon (2)"); + assertEquals("Pigeon (2)", name); } @Test public void encoderName() { var name = CTREUtil.getName(_encoder); - assertEquals(name, "CANcoder (3)"); + assertEquals("CANcoder (3)", name); } @Test diff --git a/src/test/java/frc/robot/SwerveTest.java b/src/test/java/frc/robot/SwerveTest.java deleted file mode 100644 index 2efede0..0000000 --- a/src/test/java/frc/robot/SwerveTest.java +++ /dev/null @@ -1,27 +0,0 @@ -package frc.robot; - -import static frc.lib.UnitTestingUtil.reset; -import static frc.lib.UnitTestingUtil.setupTests; - -import frc.robot.generated.TunerConstants; -import frc.robot.subsystems.Swerve; -import org.junit.jupiter.api.AfterEach; -import org.junit.jupiter.api.BeforeEach; - -public class SwerveTest { - static final double DELTA = 1e-2; // acceptable deviation range - - private Swerve _swerve; - - @BeforeEach - public void setup() { - setupTests(); - - _swerve = TunerConstants.createDrivetrain(); - } - - @AfterEach - public void close() throws Exception { - reset(_swerve); - } -}