started adding unit tests, which will be used on to test the self che…

…ck feature outside main robot code

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

@@ -5,13 +5,12 @@
 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;
 import java.util.HashSet;
 import java.util.Set;
 
-public class AdvancedSubsystem extends SubsystemBase implements SelfChecked {
+public class AdvancedSubsystem extends SubsystemBase implements SelfChecked, AutoCloseable {
   // faults and the table containing them
   private Set<Fault> _faults = new HashSet<Fault>();
   private FaultsTable _faultsTable =
@@ -20,9 +19,7 @@ public class AdvancedSubsystem extends SubsystemBase implements SelfChecked {
 
   private boolean _hasError = false;
 
-  public AdvancedSubsystem() {
-    RobotModeTriggers.test().onFalse(runOnce(this::clearFaults));
-  }
+  public AdvancedSubsystem() {}
 
   /** Clears this subsystem's faults. */
   protected final void clearFaults() {
@@ -61,6 +58,9 @@ public final Command fullSelfCheck() {
     return selfCheck;
   }
 
+  @Override
+  public void close() throws Exception {}
+
   @Override
   public void periodic() {
     String currentCommandName = "None";

src/main/java/frc/lib/UnitTestingUtil.java

@@ -0,0 +1,105 @@
+package frc.lib;
+
+import static edu.wpi.first.units.Units.Seconds;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.units.measure.Time;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+// (from team 1155)
+
+/** Provides helper methods that run a command when performing unit tests. */
+public class UnitTestingUtil {
+  public static final Time TICK_RATE = Seconds.of(0.02);
+
+  /** Sets up DS and initializes HAL with default values and asserts that it doesn't fail. */
+  public static void setupTests() {
+    assert HAL.initialize(500, 0);
+    DriverStationSim.setEnabled(true);
+    DriverStationSim.setTest(true);
+    DriverStationSim.notifyNewData();
+    FaultLogger.clear();
+    FaultLogger.unregisterAll();
+  }
+
+  /**
+   * Resets CommandScheduler and closes all subsystems. Please call in an @AfterEach method!
+   *
+   * @param subsystems All subsystems that need to be closed.
+   */
+  public static void reset(AutoCloseable... subsystems) throws Exception {
+    CommandScheduler.getInstance().unregisterAllSubsystems();
+    CommandScheduler.getInstance().cancelAll();
+
+    for (AutoCloseable subsystem : subsystems) {
+      subsystem.close();
+    }
+  }
+
+  /**
+   * Runs CommandScheduler and updates timer repeatedly to fast forward subsystems and run commands.
+   *
+   * @param ticks The number of times CommandScheduler is run.
+   */
+  public static void fastForward(int ticks) {
+    for (int i = 0; i < ticks; i++) {
+      CommandScheduler.getInstance().run();
+      SimHooks.stepTiming(TICK_RATE.in(Seconds));
+    }
+  }
+
+  /**
+   * Fasts forward in time by running CommandScheduler and updating timer.
+   *
+   * @param time
+   */
+  public static void fastForward(Time time) {
+    fastForward((int) (time.in(Seconds) / TICK_RATE.in(Seconds)));
+  }
+
+  /**
+   * Runs CommandScheduler and updates timer to fast forward subsystems by 4 seconds and run
+   * commands.
+   */
+  public static void fastForward() {
+    fastForward(Seconds.of(4));
+  }
+
+  /**
+   * Schedules and runs a command.
+   *
+   * @param command The command to run.
+   */
+  public static void run(Command command) {
+    command.schedule();
+    CommandScheduler.getInstance().run();
+  }
+
+  /**
+   * Schedules and runs a command.
+   *
+   * @param command The command to run.
+   * @param runs The number of times CommandScheduler is run.
+   */
+  public static void run(Command command, int runs) {
+    command.schedule();
+    fastForward(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!
+   *
+   * @param command
+   */
+  public static void runToCompletion(Command command) {
+    command.schedule();
+    fastForward(1);
+    while (command.isScheduled()) {
+      fastForward(1);
+    }
+  }
+}

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

@@ -27,7 +27,6 @@
 import frc.robot.Constants.SwerveConstants;
 import frc.robot.commands.Autos;
 import frc.robot.generated.TunerConstants;
-import frc.robot.subsystems.AdvancedTest;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
 
 /**
@@ -45,8 +44,6 @@ public class Robot extends TimedRobot {
   @Logged(name = "Swerve")
   private CommandSwerveDrivetrain _swerve = TunerConstants.createDrivetrain();
 
-  private AdvancedTest _advancedTest = new AdvancedTest();
-
   private Command _autonomousCommand = Autos.none();
 
   /**
@@ -67,7 +64,6 @@ public Robot() {
         "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/test/java/frc/lib/AdvancedSubsystemTest.java

@@ -0,0 +1,89 @@
+// 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.lib;
+
+import static frc.lib.UnitTestingUtil.*;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+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 = new TestImpl();
+
+  @BeforeEach
+  public void setup() {
+    setupTests();
+  }
+
+  @AfterEach
+  public void close() throws Exception {
+    reset(sub);
+  }
+
+  @Test
+  public void subsystemSelfCheck() {
+    // TODO
+  }
+
+  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 = false;
+
+      @Override
+      public double getEncoderSpeed() {
+        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);
+                }));
+      }
+    }
+
+    // uses the io
+    public double speed() {
+      return _io.getEncoderSpeed();
+    }
+
+    @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
+          );
+    }
+  }
+}