SCRIPT Move java files

wpilibjExamples/src/test/java/org/wpilib/examples/armsimulation/ArmSimulationTest.java

@@ -0,0 +1,148 @@
+// 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 edu.wpi.first.wpilibj.examples.armsimulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Preferences;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.simulation.JoystickSim;
+import edu.wpi.first.wpilibj.simulation.PWMMotorControllerSim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.parallel.ResourceLock;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.ValueSource;
+
+@ResourceLock("timing")
+class ArmSimulationTest {
+  private Robot m_robot;
+  private Thread m_thread;
+
+  private PWMMotorControllerSim m_motorSim;
+  private EncoderSim m_encoderSim;
+  private JoystickSim m_joystickSim;
+
+  @BeforeEach
+  void startThread() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+    DriverStationSim.resetData();
+    m_robot = new Robot();
+    m_thread = new Thread(m_robot::startCompetition);
+    m_encoderSim = EncoderSim.createForChannel(Constants.kEncoderAChannel);
+    m_motorSim = new PWMMotorControllerSim(Constants.kMotorPort);
+    m_joystickSim = new JoystickSim(Constants.kJoystickPort);
+
+    m_thread.start();
+    SimHooks.stepTiming(0.0); // Wait for Notifiers
+  }
+
+  @AfterEach
+  void stopThread() {
+    m_robot.endCompetition();
+    try {
+      m_thread.interrupt();
+      m_thread.join();
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    m_robot.close();
+    m_encoderSim.resetData();
+    Preferences.remove(Constants.kArmPKey);
+    Preferences.remove(Constants.kArmPositionKey);
+    Preferences.removeAll();
+    RoboRioSim.resetData();
+    DriverStationSim.resetData();
+    DriverStationSim.notifyNewData();
+  }
+
+  @ValueSource(doubles = {Constants.kDefaultArmSetpointDegrees, 25.0, 50.0})
+  @ParameterizedTest
+  void teleopTest(double setpoint) {
+    assertTrue(Preferences.containsKey(Constants.kArmPositionKey));
+    assertTrue(Preferences.containsKey(Constants.kArmPKey));
+    Preferences.setDouble(Constants.kArmPositionKey, setpoint);
+    assertEquals(setpoint, Preferences.getDouble(Constants.kArmPositionKey, Double.NaN));
+    // teleop init
+    {
+      DriverStationSim.setAutonomous(false);
+      DriverStationSim.setEnabled(true);
+      DriverStationSim.notifyNewData();
+
+      assertTrue(m_encoderSim.getInitialized());
+    }
+
+    {
+      // advance 150 timesteps
+      SimHooks.stepTiming(3);
+
+      // Ensure arm is still at minimum angle.
+      assertEquals(Constants.kMinAngleRads, m_encoderSim.getDistance(), 2.0);
+    }
+
+    {
+      // Press button to reach setpoint
+      m_joystickSim.setTrigger(true);
+      m_joystickSim.notifyNewData();
+
+      // advance 75 timesteps
+      SimHooks.stepTiming(1.5);
+
+      assertEquals(setpoint, Units.radiansToDegrees(m_encoderSim.getDistance()), 2.0);
+
+      // advance 25 timesteps to see setpoint is held.
+      SimHooks.stepTiming(0.5);
+
+      assertEquals(setpoint, Units.radiansToDegrees(m_encoderSim.getDistance()), 2.0);
+    }
+
+    {
+      // Unpress the button to go back down
+      m_joystickSim.setTrigger(false);
+      m_joystickSim.notifyNewData();
+
+      // advance 150 timesteps
+      SimHooks.stepTiming(3.0);
+
+      assertEquals(Constants.kMinAngleRads, m_encoderSim.getDistance(), 2.0);
+    }
+
+    {
+      // Press button to go back up
+      m_joystickSim.setTrigger(true);
+      m_joystickSim.notifyNewData();
+
+      // advance 75 timesteps
+      SimHooks.stepTiming(1.5);
+
+      assertEquals(setpoint, Units.radiansToDegrees(m_encoderSim.getDistance()), 2.0);
+
+      // advance 25 timesteps to see setpoint is held.
+      SimHooks.stepTiming(0.5);
+
+      assertEquals(setpoint, Units.radiansToDegrees(m_encoderSim.getDistance()), 2.0);
+    }
+
+    {
+      // Disable
+      DriverStationSim.setAutonomous(false);
+      DriverStationSim.setEnabled(false);
+      DriverStationSim.notifyNewData();
+
+      // advance 75 timesteps
+      SimHooks.stepTiming(3.5);
+
+      assertEquals(0.0, m_motorSim.getSpeed(), 0.01);
+      assertEquals(Constants.kMinAngleRads, m_encoderSim.getDistance(), 2.0);
+    }
+  }
+}

wpilibjExamples/src/test/java/org/wpilib/examples/digitalcommunication/DigitalCommunicationTest.java

@@ -0,0 +1,114 @@
+// 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 edu.wpi.first.wpilibj.examples.digitalcommunication;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.AllianceStationID;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.DIOSim;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.parallel.ResourceLock;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.EnumSource;
+import org.junit.jupiter.params.provider.ValueSource;
+
+@ResourceLock("timing")
+class DigitalCommunicationTest {
+  private Robot m_robot;
+  private Thread m_thread;
+  private final DIOSim m_allianceOutput = new DIOSim(Robot.kAlliancePort);
+  private final DIOSim m_enabledOutput = new DIOSim(Robot.kEnabledPort);
+  private final DIOSim m_autonomousOutput = new DIOSim(Robot.kAutonomousPort);
+  private final DIOSim m_alertOutput = new DIOSim(Robot.kAlertPort);
+
+  @BeforeEach
+  void startThread() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+    DriverStationSim.resetData();
+    m_robot = new Robot();
+    m_thread = new Thread(m_robot::startCompetition);
+    m_thread.start();
+    SimHooks.stepTiming(0.0); // Wait for Notifiers
+  }
+
+  @AfterEach
+  void stopThread() {
+    m_robot.endCompetition();
+    try {
+      m_thread.interrupt();
+      m_thread.join();
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    m_robot.close();
+    m_allianceOutput.resetData();
+    m_enabledOutput.resetData();
+    m_autonomousOutput.resetData();
+    m_alertOutput.resetData();
+  }
+
+  @EnumSource(AllianceStationID.class)
+  @ParameterizedTest(name = "alliance[{index}]: {0}")
+  void allianceTest(AllianceStationID alliance) {
+    DriverStationSim.setAllianceStationId(alliance);
+    DriverStationSim.notifyNewData();
+
+    assertTrue(m_allianceOutput.getInitialized());
+    assertFalse(m_allianceOutput.getIsInput());
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(alliance.name().startsWith("Red"), m_allianceOutput.getValue());
+  }
+
+  @ValueSource(booleans = {true, false})
+  @ParameterizedTest(name = "enabled[{index}]: {0}")
+  void enabledTest(boolean enabled) {
+    DriverStationSim.setEnabled(enabled);
+    DriverStationSim.notifyNewData();
+
+    assertTrue(m_enabledOutput.getInitialized());
+    assertFalse(m_enabledOutput.getIsInput());
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(enabled, m_enabledOutput.getValue());
+  }
+
+  @ValueSource(booleans = {true, false})
+  @ParameterizedTest(name = "autonomous[{index}]: {0}")
+  void autonomousTest(boolean autonomous) {
+    DriverStationSim.setAutonomous(autonomous);
+    DriverStationSim.notifyNewData();
+
+    assertTrue(m_autonomousOutput.getInitialized());
+    assertFalse(m_autonomousOutput.getIsInput());
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(autonomous, m_autonomousOutput.getValue());
+  }
+
+  @ValueSource(doubles = {45.0, 27.0, 23.0})
+  @ParameterizedTest(name = "alert[{index}]: {0}s")
+  void alertTest(double matchTime) {
+    DriverStationSim.setMatchTime(matchTime);
+    DriverStationSim.notifyNewData();
+
+    assertTrue(m_alertOutput.getInitialized());
+    assertFalse(m_alertOutput.getIsInput());
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(matchTime <= 30 && matchTime >= 25, m_alertOutput.getValue());
+  }
+}

wpilibjExamples/src/test/java/org/wpilib/examples/elevatorsimulation/ElevatorSimulationTest.java

@@ -0,0 +1,137 @@
+// 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 edu.wpi.first.wpilibj.examples.elevatorsimulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.simulation.JoystickSim;
+import edu.wpi.first.wpilibj.simulation.PWMMotorControllerSim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.parallel.ResourceLock;
+
+@ResourceLock("timing")
+class ElevatorSimulationTest {
+  private Robot m_robot;
+  private Thread m_thread;
+
+  private PWMMotorControllerSim m_motorSim;
+  private EncoderSim m_encoderSim;
+  private JoystickSim m_joystickSim;
+
+  @BeforeEach
+  void startThread() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+    DriverStationSim.resetData();
+    m_robot = new Robot();
+    m_thread = new Thread(m_robot::startCompetition);
+    m_encoderSim = EncoderSim.createForChannel(Constants.kEncoderAChannel);
+    m_motorSim = new PWMMotorControllerSim(Constants.kMotorPort);
+    m_joystickSim = new JoystickSim(Constants.kJoystickPort);
+
+    m_thread.start();
+    SimHooks.stepTiming(0.0); // Wait for Notifiers
+  }
+
+  @AfterEach
+  void stopThread() {
+    m_robot.endCompetition();
+    try {
+      m_thread.interrupt();
+      m_thread.join();
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    m_robot.close();
+    m_encoderSim.resetData();
+    RoboRioSim.resetData();
+    DriverStationSim.resetData();
+    DriverStationSim.notifyNewData();
+  }
+
+  @Test
+  void teleopTest() {
+    // teleop init
+    {
+      DriverStationSim.setAutonomous(false);
+      DriverStationSim.setEnabled(true);
+      DriverStationSim.notifyNewData();
+
+      assertTrue(m_encoderSim.getInitialized());
+    }
+
+    {
+      // advance 50 timesteps
+      SimHooks.stepTiming(1);
+
+      // Ensure elevator is still at 0.
+      assertEquals(0.0, m_encoderSim.getDistance(), 0.05);
+    }
+
+    {
+      // Press button to reach setpoint
+      m_joystickSim.setTrigger(true);
+      m_joystickSim.notifyNewData();
+
+      // advance 75 timesteps
+      SimHooks.stepTiming(1.5);
+
+      assertEquals(Constants.kSetpoint, m_encoderSim.getDistance(), 0.05);
+
+      // advance 25 timesteps to see setpoint is held.
+      SimHooks.stepTiming(0.5);
+
+      assertEquals(Constants.kSetpoint, m_encoderSim.getDistance(), 0.05);
+    }
+
+    {
+      // Unpress the button to go back down
+      m_joystickSim.setTrigger(false);
+      m_joystickSim.notifyNewData();
+
+      // advance 75 timesteps
+      SimHooks.stepTiming(1.5);
+
+      assertEquals(0.0, m_encoderSim.getDistance(), 0.05);
+    }
+
+    {
+      // Press button to go back up
+      m_joystickSim.setTrigger(true);
+      m_joystickSim.notifyNewData();
+
+      // advance 75 timesteps
+      SimHooks.stepTiming(1.5);
+
+      assertEquals(Constants.kSetpoint, m_encoderSim.getDistance(), 0.05);
+
+      // advance 25 timesteps to see setpoint is held.
+      SimHooks.stepTiming(0.5);
+
+      assertEquals(Constants.kSetpoint, m_encoderSim.getDistance(), 0.05);
+    }
+
+    {
+      // Disable
+      DriverStationSim.setAutonomous(false);
+      DriverStationSim.setEnabled(false);
+      DriverStationSim.notifyNewData();
+
+      // advance 75 timesteps
+      SimHooks.stepTiming(1.5);
+
+      assertEquals(0.0, m_motorSim.getSpeed(), 0.05);
+      assertEquals(0.0, m_encoderSim.getDistance(), 0.05);
+    }
+  }
+}

wpilibjExamples/src/test/java/org/wpilib/examples/i2ccommunication/I2CCommunicationTest.java

@@ -0,0 +1,129 @@
+// 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 edu.wpi.first.wpilibj.examples.i2ccommunication;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTimeoutPreemptively;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.AllianceStationID;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.simulation.CallbackStore;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import edu.wpi.first.wpilibj.simulation.I2CSim;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import java.time.Duration;
+import java.util.concurrent.CompletableFuture;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.parallel.ResourceLock;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.EnumSource;
+import org.junit.jupiter.params.provider.ValueSource;
+
+@ResourceLock("timing")
+class I2CCommunicationTest {
+  private Robot m_robot;
+  private Thread m_thread;
+  private final I2CSim m_i2c = new I2CSim(Robot.kPort.value);
+  private CompletableFuture<String> m_future;
+  private CallbackStore m_callback;
+
+  @BeforeEach
+  void startThread() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+    DriverStationSim.resetData();
+    m_future = new CompletableFuture<>();
+    m_callback =
+        m_i2c.registerWriteCallback(
+            (name, buffer, count) -> m_future.complete(new String(buffer, 0, count)));
+    m_robot = new Robot();
+    m_thread = new Thread(m_robot::startCompetition);
+    m_thread.start();
+    SimHooks.stepTiming(0.0); // Wait for Notifiers
+  }
+
+  @AfterEach
+  void stopThread() {
+    m_robot.endCompetition();
+    try {
+      m_thread.interrupt();
+      m_thread.join();
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    m_robot.close();
+    m_callback.close();
+    m_i2c.resetData();
+  }
+
+  @EnumSource(AllianceStationID.class)
+  @ParameterizedTest(name = "alliance[{index}]: {0}")
+  void allianceTest(AllianceStationID alliance) {
+    DriverStationSim.setAllianceStationId(alliance);
+    DriverStationSim.notifyNewData();
+
+    assertTrue(m_i2c.getInitialized());
+
+    SimHooks.stepTiming(0.02);
+
+    String str = assertTimeoutPreemptively(Duration.ofMillis(20L), () -> m_future.get());
+    char expected = alliance.name().startsWith("Red") ? 'R' : 'B';
+    if (alliance.name().startsWith("Unknown")) {
+      expected = 'U';
+    }
+
+    assertEquals(expected, str.charAt(0));
+  }
+
+  @ValueSource(booleans = {true, false})
+  @ParameterizedTest(name = "enabled[{index}]: {0}")
+  void enabledTest(boolean enabled) {
+    DriverStationSim.setEnabled(enabled);
+    DriverStationSim.notifyNewData();
+
+    assertTrue(m_i2c.getInitialized());
+
+    SimHooks.stepTiming(0.02);
+
+    String str = assertTimeoutPreemptively(Duration.ofMillis(20L), () -> m_future.get());
+    char expected = enabled ? 'E' : 'D';
+
+    assertEquals(expected, str.charAt(1));
+  }
+
+  @ValueSource(booleans = {true, false})
+  @ParameterizedTest(name = "autonomous[{index}]: {0}")
+  void autonomousTest(boolean autonomous) {
+    DriverStationSim.setAutonomous(autonomous);
+    DriverStationSim.notifyNewData();
+
+    assertTrue(m_i2c.getInitialized());
+
+    SimHooks.stepTiming(0.02);
+
+    String str = assertTimeoutPreemptively(Duration.ofMillis(20L), () -> m_future.get());
+    char expected = autonomous ? 'A' : 'T';
+
+    assertEquals(expected, str.charAt(2));
+  }
+
+  @ValueSource(doubles = {112.0, 45.0, 27.0, 23.0, 3.0})
+  @ParameterizedTest(name = "matchTime[{index}]: {0}s")
+  void matchTimeTest(double matchTime) {
+    DriverStationSim.setMatchTime(matchTime);
+    DriverStationSim.notifyNewData();
+    assertTrue(m_i2c.getInitialized());
+
+    SimHooks.stepTiming(0.02);
+
+    String str = assertTimeoutPreemptively(Duration.ofMillis(20L), () -> m_future.get());
+    String expected = String.format("%03d", (int) DriverStation.getMatchTime());
+
+    assertEquals(expected, str.substring(3));
+  }
+}

wpilibjExamples/src/test/java/org/wpilib/examples/potentiometerpid/PotentiometerPIDTest.java

@@ -0,0 +1,171 @@
+// 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 edu.wpi.first.wpilibj.examples.potentiometerpid;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.HAL.SimPeriodicBeforeCallback;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.simulation.AnalogInputSim;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import edu.wpi.first.wpilibj.simulation.ElevatorSim;
+import edu.wpi.first.wpilibj.simulation.JoystickSim;
+import edu.wpi.first.wpilibj.simulation.PWMMotorControllerSim;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.parallel.ResourceLock;
+
+@ResourceLock("timing")
+class PotentiometerPIDTest {
+  private final DCMotor m_elevatorGearbox = DCMotor.getVex775Pro(4);
+  private static final double kElevatorGearing = 10.0;
+  private static final double kElevatorDrumRadius = Units.inchesToMeters(2.0);
+  private static final double kCarriageMassKg = 4.0; // kg
+
+  private Robot m_robot;
+  private Thread m_thread;
+
+  private ElevatorSim m_elevatorSim;
+  private PWMMotorControllerSim m_motorSim;
+  private AnalogInputSim m_analogSim;
+  private SimPeriodicBeforeCallback m_callback;
+  private JoystickSim m_joystickSim;
+
+  @BeforeEach
+  void startThread() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+    DriverStationSim.resetData();
+    m_robot = new Robot();
+    m_thread = new Thread(m_robot::startCompetition);
+    m_elevatorSim =
+        new ElevatorSim(
+            m_elevatorGearbox,
+            kElevatorGearing,
+            kCarriageMassKg,
+            kElevatorDrumRadius,
+            0.0,
+            Robot.kFullHeight,
+            true,
+            0);
+    m_analogSim = new AnalogInputSim(Robot.kPotChannel);
+    m_motorSim = new PWMMotorControllerSim(Robot.kMotorChannel);
+    m_joystickSim = new JoystickSim(Robot.kJoystickChannel);
+
+    m_callback =
+        HAL.registerSimPeriodicBeforeCallback(
+            () -> {
+              m_elevatorSim.setInputVoltage(
+                  m_motorSim.getSpeed() * RobotController.getBatteryVoltage());
+              m_elevatorSim.update(0.02);
+
+              /*
+              meters = (v / 3.3v) * range
+              meters / range = v / 3.3v
+              3.3v * (meters / range) = v
+               */
+              m_analogSim.setVoltage(
+                  RobotController.getVoltage3V3()
+                      * (m_elevatorSim.getPosition() / Robot.kFullHeight));
+            });
+
+    m_thread.start();
+    SimHooks.stepTiming(0.0); // Wait for Notifiers
+  }
+
+  @AfterEach
+  void stopThread() {
+    m_robot.endCompetition();
+    try {
+      m_thread.interrupt();
+      m_thread.join();
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    m_robot.close();
+    m_callback.close();
+    m_analogSim.resetData();
+  }
+
+  @Test
+  void teleopTest() {
+    // teleop init
+    {
+      DriverStationSim.setAutonomous(false);
+      DriverStationSim.setEnabled(true);
+      DriverStationSim.notifyNewData();
+
+      assertTrue(m_analogSim.getInitialized());
+    }
+
+    // first setpoint
+    {
+      // advance 50 timesteps
+      SimHooks.stepTiming(1);
+
+      assertEquals(Robot.kSetpoints[0], m_elevatorSim.getPosition(), 0.1);
+    }
+
+    // second setpoint
+    {
+      // press button to advance setpoint
+      m_joystickSim.setTrigger(true);
+      m_joystickSim.notifyNewData();
+
+      // advance 50 timesteps
+      SimHooks.stepTiming(1);
+
+      assertEquals(Robot.kSetpoints[1], m_elevatorSim.getPosition(), 0.1);
+    }
+
+    // we need to unpress the button
+    {
+      m_joystickSim.setTrigger(false);
+      m_joystickSim.notifyNewData();
+
+      // advance 10 timesteps
+      SimHooks.stepTiming(0.2);
+    }
+
+    // third setpoint
+    {
+      // press button to advance setpoint
+      m_joystickSim.setTrigger(true);
+      m_joystickSim.notifyNewData();
+
+      // advance 50 timesteps
+      SimHooks.stepTiming(1);
+
+      assertEquals(Robot.kSetpoints[2], m_elevatorSim.getPosition(), 0.1);
+    }
+
+    // we need to unpress the button
+    {
+      m_joystickSim.setTrigger(false);
+      m_joystickSim.notifyNewData();
+
+      // advance 10 timesteps
+      SimHooks.stepTiming(0.2);
+    }
+
+    // rollover: first setpoint
+    {
+      // press button to advance setpoint
+      m_joystickSim.setTrigger(true);
+      m_joystickSim.notifyNewData();
+
+      // advance 60 timesteps
+      SimHooks.stepTiming(1.2);
+
+      assertEquals(Robot.kSetpoints[0], m_elevatorSim.getPosition(), 0.1);
+    }
+  }
+}

wpilibjExamples/src/test/java/org/wpilib/examples/unittest/subsystems/IntakeTest.java

@@ -0,0 +1,71 @@
+// 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 edu.wpi.first.wpilibj.examples.unittest.subsystems;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.PneumaticsModuleType;
+import edu.wpi.first.wpilibj.examples.unittest.Constants.IntakeConstants;
+import edu.wpi.first.wpilibj.simulation.DoubleSolenoidSim;
+import edu.wpi.first.wpilibj.simulation.PWMMotorControllerSim;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+class IntakeTest {
+  static final double DELTA = 1e-2; // acceptable deviation range
+  Intake m_intake;
+  PWMMotorControllerSim m_simMotor;
+  DoubleSolenoidSim m_simPiston;
+
+  @BeforeEach // this method will run before each test
+  void setup() {
+    assert HAL.initialize(500, 0); // initialize the HAL, crash if failed
+    m_intake = new Intake(); // create our intake
+    m_simMotor =
+        new PWMMotorControllerSim(
+            IntakeConstants.kMotorPort); // create our simulation PWM motor controller
+    m_simPiston =
+        new DoubleSolenoidSim(
+            PneumaticsModuleType.CTREPCM,
+            IntakeConstants.kPistonFwdChannel,
+            IntakeConstants.kPistonRevChannel); // create our simulation solenoid
+  }
+
+  @SuppressWarnings("PMD.SignatureDeclareThrowsException")
+  @AfterEach // this method will run after each test
+  void shutdown() throws Exception {
+    m_intake.close(); // destroy our intake object
+  }
+
+  @Test // marks this method as a test
+  void doesntWorkWhenClosed() {
+    m_intake.retract(); // close the intake
+    m_intake.activate(0.5); // try to activate the motor
+    assertEquals(
+        0.0, m_simMotor.getSpeed(), DELTA); // make sure that the value set to the motor is 0
+  }
+
+  @Test
+  void worksWhenOpen() {
+    m_intake.deploy();
+    m_intake.activate(0.5);
+    assertEquals(0.5, m_simMotor.getSpeed(), DELTA);
+  }
+
+  @Test
+  void retractTest() {
+    m_intake.retract();
+    assertEquals(DoubleSolenoid.Value.kReverse, m_simPiston.get());
+  }
+
+  @Test
+  void deployTest() {
+    m_intake.deploy();
+    assertEquals(DoubleSolenoid.Value.kForward, m_simPiston.get());
+  }
+}