SCRIPT Move java files

wpilibj/src/test/java/org/wpilib/MockHardwareExtension.java

@@ -0,0 +1,38 @@
+// 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;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import org.junit.jupiter.api.extension.BeforeAllCallback;
+import org.junit.jupiter.api.extension.ExtensionContext;
+import org.junit.jupiter.api.extension.ExtensionContext.Namespace;
+
+public final class MockHardwareExtension implements BeforeAllCallback {
+  private static ExtensionContext getRoot(ExtensionContext context) {
+    return context.getParent().map(MockHardwareExtension::getRoot).orElse(context);
+  }
+
+  @Override
+  public void beforeAll(ExtensionContext context) {
+    getRoot(context)
+        .getStore(Namespace.GLOBAL)
+        .getOrComputeIfAbsent(
+            "HAL Initialized",
+            key -> {
+              initializeHardware();
+              return true;
+            },
+            Boolean.class);
+  }
+
+  private void initializeHardware() {
+    HAL.initialize(500, 0);
+    DriverStationSim.setDsAttached(true);
+    DriverStationSim.setAutonomous(false);
+    DriverStationSim.setEnabled(true);
+    DriverStationSim.setTest(true);
+  }
+}

wpilibj/src/test/java/org/wpilib/UtilityClassTest.java

@@ -0,0 +1,65 @@
+// 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;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.junit.jupiter.api.DynamicTest.dynamicTest;
+
+import java.lang.reflect.Constructor;
+import java.lang.reflect.InvocationTargetException;
+import java.lang.reflect.Modifier;
+import java.util.Arrays;
+import java.util.stream.Stream;
+import org.junit.jupiter.api.DynamicTest;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.TestFactory;
+
+// Declaring this class abstract prevents UtilityClassTest from running on itself and throwing the
+// following exception:
+//
+// org.junit.jupiter.api.extension.ParameterResolutionException: No ParameterResolver registered
+// for parameter [java.lang.Class<T> arg0] in constructor [protected
+// edu.wpi.first.wpilibj.UtilityClassTest(java.lang.Class<T>)].
+@SuppressWarnings("PMD.AbstractClassWithoutAbstractMethod")
+public abstract class UtilityClassTest<T> {
+  private final Class<T> m_clazz;
+
+  protected UtilityClassTest(Class<T> clazz) {
+    m_clazz = clazz;
+  }
+
+  @Test
+  void singleConstructorTest() {
+    assertEquals(1, m_clazz.getDeclaredConstructors().length, "More than one constructor defined");
+  }
+
+  @Test
+  void constructorPrivateTest() {
+    Constructor<?> constructor = m_clazz.getDeclaredConstructors()[0];
+
+    assertFalse(constructor.canAccess(null), "Constructor is not private");
+  }
+
+  @Test
+  @SuppressWarnings("PMD.AvoidAccessibilityAlteration")
+  void constructorReflectionTest() {
+    Constructor<?> constructor = m_clazz.getDeclaredConstructors()[0];
+    constructor.setAccessible(true);
+    assertThrows(InvocationTargetException.class, constructor::newInstance);
+  }
+
+  @TestFactory
+  Stream<DynamicTest> publicMethodsStaticTestFactory() {
+    return Arrays.stream(m_clazz.getDeclaredMethods())
+        .filter(method -> Modifier.isPublic(method.getModifiers()))
+        .map(
+            method ->
+                dynamicTest(
+                    method.getName(), () -> assertTrue(Modifier.isStatic(method.getModifiers()))));
+  }
+}

wpilibj/src/test/java/org/wpilib/can/CANStatusTest.java

@@ -0,0 +1,21 @@
+// 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.can;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.can.CANJNI;
+import edu.wpi.first.hal.can.CANStatus;
+import org.junit.jupiter.api.Test;
+
+class CANStatusTest {
+  @Test
+  void canStatusGetDoesntThrow() {
+    HAL.initialize(500, 0);
+    CANStatus status = new CANStatus();
+    assertDoesNotThrow(() -> CANJNI.getCANStatus(0, status));
+  }
+}

wpilibj/src/test/java/org/wpilib/drive/DifferentialDriveTest.java

@@ -0,0 +1,479 @@
+// 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.drive;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.wpilibj.motorcontrol.MockPWMMotorController;
+import org.junit.jupiter.api.Test;
+
+@SuppressWarnings("resource")
+class DifferentialDriveTest {
+  @Test
+  void testArcadeDriveIK() {
+    // Forward
+    var speeds = DifferentialDrive.arcadeDriveIK(1.0, 0.0, false);
+    assertEquals(1.0, speeds.left, 1e-9);
+    assertEquals(1.0, speeds.right, 1e-9);
+
+    // Forward left turn
+    speeds = DifferentialDrive.arcadeDriveIK(0.5, 0.5, false);
+    assertEquals(0.0, speeds.left, 1e-9);
+    assertEquals(0.5, speeds.right, 1e-9);
+
+    // Forward right turn
+    speeds = DifferentialDrive.arcadeDriveIK(0.5, -0.5, false);
+    assertEquals(0.5, speeds.left, 1e-9);
+    assertEquals(0.0, speeds.right, 1e-9);
+
+    // Backward
+    speeds = DifferentialDrive.arcadeDriveIK(-1.0, 0.0, false);
+    assertEquals(-1.0, speeds.left, 1e-9);
+    assertEquals(-1.0, speeds.right, 1e-9);
+
+    // Backward left turn
+    speeds = DifferentialDrive.arcadeDriveIK(-0.5, 0.5, false);
+    assertEquals(-0.5, speeds.left, 1e-9);
+    assertEquals(0.0, speeds.right, 1e-9);
+
+    // Backward right turn
+    speeds = DifferentialDrive.arcadeDriveIK(-0.5, -0.5, false);
+    assertEquals(0.0, speeds.left, 1e-9);
+    assertEquals(-0.5, speeds.right, 1e-9);
+
+    // Left turn (xSpeed with negative sign)
+    speeds = DifferentialDrive.arcadeDriveIK(-0.0, 1.0, false);
+    assertEquals(-1.0, speeds.left, 1e-9);
+    assertEquals(1.0, speeds.right, 1e-9);
+
+    // Left turn (xSpeed with positive sign)
+    speeds = DifferentialDrive.arcadeDriveIK(0.0, 1.0, false);
+    assertEquals(-1.0, speeds.left, 1e-9);
+    assertEquals(1.0, speeds.right, 1e-9);
+
+    // Right turn (xSpeed with negative sign)
+    speeds = DifferentialDrive.arcadeDriveIK(-0.0, -1.0, false);
+    assertEquals(1.0, speeds.left, 1e-9);
+    assertEquals(-1.0, speeds.right, 1e-9);
+
+    // Right turn (xSpeed with positive sign)
+    speeds = DifferentialDrive.arcadeDriveIK(0.0, -1.0, false);
+    assertEquals(1.0, speeds.left, 1e-9);
+    assertEquals(-1.0, speeds.right, 1e-9);
+  }
+
+  @Test
+  void testArcadeDriveIKSquared() {
+    // Forward
+    var speeds = DifferentialDrive.arcadeDriveIK(1.0, 0.0, true);
+    assertEquals(1.0, speeds.left, 1e-9);
+    assertEquals(1.0, speeds.right, 1e-9);
+
+    // Forward left turn
+    speeds = DifferentialDrive.arcadeDriveIK(0.5, 0.5, true);
+    assertEquals(0.0, speeds.left, 1e-9);
+    assertEquals(0.25, speeds.right, 1e-9);
+
+    // Forward right turn
+    speeds = DifferentialDrive.arcadeDriveIK(0.5, -0.5, true);
+    assertEquals(0.25, speeds.left, 1e-9);
+    assertEquals(0.0, speeds.right, 1e-9);
+
+    // Backward
+    speeds = DifferentialDrive.arcadeDriveIK(-1.0, 0.0, true);
+    assertEquals(-1.0, speeds.left, 1e-9);
+    assertEquals(-1.0, speeds.right, 1e-9);
+
+    // Backward left turn
+    speeds = DifferentialDrive.arcadeDriveIK(-0.5, 0.5, true);
+    assertEquals(-0.25, speeds.left, 1e-9);
+    assertEquals(0.0, speeds.right, 1e-9);
+
+    // Backward right turn
+    speeds = DifferentialDrive.arcadeDriveIK(-0.5, -0.5, true);
+    assertEquals(0.0, speeds.left, 1e-9);
+    assertEquals(-0.25, speeds.right, 1e-9);
+
+    // Left turn (xSpeed with negative sign)
+    speeds = DifferentialDrive.arcadeDriveIK(-0.0, 1.0, false);
+    assertEquals(-1.0, speeds.left, 1e-9);
+    assertEquals(1.0, speeds.right, 1e-9);
+
+    // Left turn (xSpeed with positive sign)
+    speeds = DifferentialDrive.arcadeDriveIK(0.0, 1.0, false);
+    assertEquals(-1.0, speeds.left, 1e-9);
+    assertEquals(1.0, speeds.right, 1e-9);
+
+    // Right turn (xSpeed with negative sign)
+    speeds = DifferentialDrive.arcadeDriveIK(-0.0, -1.0, false);
+    assertEquals(1.0, speeds.left, 1e-9);
+    assertEquals(-1.0, speeds.right, 1e-9);
+
+    // Right turn (xSpeed with positive sign)
+    speeds = DifferentialDrive.arcadeDriveIK(0.0, -1.0, false);
+    assertEquals(1.0, speeds.left, 1e-9);
+    assertEquals(-1.0, speeds.right, 1e-9);
+  }
+
+  @Test
+  void testCurvatureDriveIK() {
+    // Forward
+    var speeds = DifferentialDrive.curvatureDriveIK(1.0, 0.0, false);
+    assertEquals(1.0, speeds.left, 1e-9);
+    assertEquals(1.0, speeds.right, 1e-9);
+
+    // Forward left turn
+    speeds = DifferentialDrive.curvatureDriveIK(0.5, 0.5, false);
+    assertEquals(0.25, speeds.left, 1e-9);
+    assertEquals(0.75, speeds.right, 1e-9);
+
+    // Forward right turn
+    speeds = DifferentialDrive.curvatureDriveIK(0.5, -0.5, false);
+    assertEquals(0.75, speeds.left, 1e-9);
+    assertEquals(0.25, speeds.right, 1e-9);
+
+    // Backward
+    speeds = DifferentialDrive.curvatureDriveIK(-1.0, 0.0, false);
+    assertEquals(-1.0, speeds.left, 1e-9);
+    assertEquals(-1.0, speeds.right, 1e-9);
+
+    // Backward left turn
+    speeds = DifferentialDrive.curvatureDriveIK(-0.5, 0.5, false);
+    assertEquals(-0.75, speeds.left, 1e-9);
+    assertEquals(-0.25, speeds.right, 1e-9);
+
+    // Backward right turn
+    speeds = DifferentialDrive.curvatureDriveIK(-0.5, -0.5, false);
+    assertEquals(-0.25, speeds.left, 1e-9);
+    assertEquals(-0.75, speeds.right, 1e-9);
+  }
+
+  @Test
+  void testCurvatureDriveIKTurnInPlace() {
+    // Forward
+    var speeds = DifferentialDrive.curvatureDriveIK(1.0, 0.0, true);
+    assertEquals(1.0, speeds.left, 1e-9);
+    assertEquals(1.0, speeds.right, 1e-9);
+
+    // Forward left turn
+    speeds = DifferentialDrive.curvatureDriveIK(0.5, 0.5, true);
+    assertEquals(0.0, speeds.left, 1e-9);
+    assertEquals(1.0, speeds.right, 1e-9);
+
+    // Forward right turn
+    speeds = DifferentialDrive.curvatureDriveIK(0.5, -0.5, true);
+    assertEquals(1.0, speeds.left, 1e-9);
+    assertEquals(0.0, speeds.right, 1e-9);
+
+    // Backward
+    speeds = DifferentialDrive.curvatureDriveIK(-1.0, 0.0, true);
+    assertEquals(-1.0, speeds.left, 1e-9);
+    assertEquals(-1.0, speeds.right, 1e-9);
+
+    // Backward left turn
+    speeds = DifferentialDrive.curvatureDriveIK(-0.5, 0.5, true);
+    assertEquals(-1.0, speeds.left, 1e-9);
+    assertEquals(0.0, speeds.right, 1e-9);
+
+    // Backward right turn
+    speeds = DifferentialDrive.curvatureDriveIK(-0.5, -0.5, true);
+    assertEquals(0.0, speeds.left, 1e-9);
+    assertEquals(-1.0, speeds.right, 1e-9);
+  }
+
+  @Test
+  void testTankDriveIK() {
+    // Forward
+    var speeds = DifferentialDrive.tankDriveIK(1.0, 1.0, false);
+    assertEquals(1.0, speeds.left, 1e-9);
+    assertEquals(1.0, speeds.right, 1e-9);
+
+    // Forward left turn
+    speeds = DifferentialDrive.tankDriveIK(0.5, 1.0, false);
+    assertEquals(0.5, speeds.left, 1e-9);
+    assertEquals(1.0, speeds.right, 1e-9);
+
+    // Forward right turn
+    speeds = DifferentialDrive.tankDriveIK(1.0, 0.5, false);
+    assertEquals(1.0, speeds.left, 1e-9);
+    assertEquals(0.5, speeds.right, 1e-9);
+
+    // Backward
+    speeds = DifferentialDrive.tankDriveIK(-1.0, -1.0, false);
+    assertEquals(-1.0, speeds.left, 1e-9);
+    assertEquals(-1.0, speeds.right, 1e-9);
+
+    // Backward left turn
+    speeds = DifferentialDrive.tankDriveIK(-0.5, -1.0, false);
+    assertEquals(-0.5, speeds.left, 1e-9);
+    assertEquals(-1.0, speeds.right, 1e-9);
+
+    // Backward right turn
+    speeds = DifferentialDrive.tankDriveIK(-0.5, 1.0, false);
+    assertEquals(-0.5, speeds.left, 1e-9);
+    assertEquals(1.0, speeds.right, 1e-9);
+  }
+
+  @Test
+  void testTankDriveIKSquared() {
+    // Forward
+    var speeds = DifferentialDrive.tankDriveIK(1.0, 1.0, true);
+    assertEquals(1.0, speeds.left, 1e-9);
+    assertEquals(1.0, speeds.right, 1e-9);
+
+    // Forward left turn
+    speeds = DifferentialDrive.tankDriveIK(0.5, 1.0, true);
+    assertEquals(0.25, speeds.left, 1e-9);
+    assertEquals(1.0, speeds.right, 1e-9);
+
+    // Forward right turn
+    speeds = DifferentialDrive.tankDriveIK(1.0, 0.5, true);
+    assertEquals(1.0, speeds.left, 1e-9);
+    assertEquals(0.25, speeds.right, 1e-9);
+
+    // Backward
+    speeds = DifferentialDrive.tankDriveIK(-1.0, -1.0, true);
+    assertEquals(-1.0, speeds.left, 1e-9);
+    assertEquals(-1.0, speeds.right, 1e-9);
+
+    // Backward left turn
+    speeds = DifferentialDrive.tankDriveIK(-0.5, -1.0, true);
+    assertEquals(-0.25, speeds.left, 1e-9);
+    assertEquals(-1.0, speeds.right, 1e-9);
+
+    // Backward right turn
+    speeds = DifferentialDrive.tankDriveIK(-1.0, -0.5, true);
+    assertEquals(-1.0, speeds.left, 1e-9);
+    assertEquals(-0.25, speeds.right, 1e-9);
+  }
+
+  @Test
+  void testArcadeDrive() {
+    var left = new MockPWMMotorController();
+    var right = new MockPWMMotorController();
+    var drive = new DifferentialDrive(left::set, right::set);
+    drive.setDeadband(0.0);
+
+    // Forward
+    drive.arcadeDrive(1.0, 0.0, false);
+    assertEquals(1.0, left.get(), 1e-9);
+    assertEquals(1.0, right.get(), 1e-9);
+
+    // Forward left turn
+    drive.arcadeDrive(0.5, 0.5, false);
+    assertEquals(0.0, left.get(), 1e-9);
+    assertEquals(0.5, right.get(), 1e-9);
+
+    // Forward right turn
+    drive.arcadeDrive(0.5, -0.5, false);
+    assertEquals(0.5, left.get(), 1e-9);
+    assertEquals(0.0, right.get(), 1e-9);
+
+    // Backward
+    drive.arcadeDrive(-1.0, 0.0, false);
+    assertEquals(-1.0, left.get(), 1e-9);
+    assertEquals(-1.0, right.get(), 1e-9);
+
+    // Backward left turn
+    drive.arcadeDrive(-0.5, 0.5, false);
+    assertEquals(-0.5, left.get(), 1e-9);
+    assertEquals(0.0, right.get(), 1e-9);
+
+    // Backward right turn
+    drive.arcadeDrive(-0.5, -0.5, false);
+    assertEquals(0.0, left.get(), 1e-9);
+    assertEquals(-0.5, right.get(), 1e-9);
+  }
+
+  @Test
+  void testArcadeDriveSquared() {
+    var left = new MockPWMMotorController();
+    var right = new MockPWMMotorController();
+    var drive = new DifferentialDrive(left::set, right::set);
+    drive.setDeadband(0.0);
+
+    // Forward
+    drive.arcadeDrive(1.0, 0.0, true);
+    assertEquals(1.0, left.get(), 1e-9);
+    assertEquals(1.0, right.get(), 1e-9);
+
+    // Forward left turn
+    drive.arcadeDrive(0.5, 0.5, true);
+    assertEquals(0.0, left.get(), 1e-9);
+    assertEquals(0.25, right.get(), 1e-9);
+
+    // Forward right turn
+    drive.arcadeDrive(0.5, -0.5, true);
+    assertEquals(0.25, left.get(), 1e-9);
+    assertEquals(0.0, right.get(), 1e-9);
+
+    // Backward
+    drive.arcadeDrive(-1.0, 0.0, true);
+    assertEquals(-1.0, left.get(), 1e-9);
+    assertEquals(-1.0, right.get(), 1e-9);
+
+    // Backward left turn
+    drive.arcadeDrive(-0.5, 0.5, true);
+    assertEquals(-0.25, left.get(), 1e-9);
+    assertEquals(0.0, right.get(), 1e-9);
+
+    // Backward right turn
+    drive.arcadeDrive(-0.5, -0.5, true);
+    assertEquals(0.0, left.get(), 1e-9);
+    assertEquals(-0.25, right.get(), 1e-9);
+  }
+
+  @Test
+  void testCurvatureDrive() {
+    var left = new MockPWMMotorController();
+    var right = new MockPWMMotorController();
+    var drive = new DifferentialDrive(left::set, right::set);
+    drive.setDeadband(0.0);
+
+    // Forward
+    drive.curvatureDrive(1.0, 0.0, false);
+    assertEquals(1.0, left.get(), 1e-9);
+    assertEquals(1.0, right.get(), 1e-9);
+
+    // Forward left turn
+    drive.curvatureDrive(0.5, 0.5, false);
+    assertEquals(0.25, left.get(), 1e-9);
+    assertEquals(0.75, right.get(), 1e-9);
+
+    // Forward right turn
+    drive.curvatureDrive(0.5, -0.5, false);
+    assertEquals(0.75, left.get(), 1e-9);
+    assertEquals(0.25, right.get(), 1e-9);
+
+    // Backward
+    drive.curvatureDrive(-1.0, 0.0, false);
+    assertEquals(-1.0, left.get(), 1e-9);
+    assertEquals(-1.0, right.get(), 1e-9);
+
+    // Backward left turn
+    drive.curvatureDrive(-0.5, 0.5, false);
+    assertEquals(-0.75, left.get(), 1e-9);
+    assertEquals(-0.25, right.get(), 1e-9);
+
+    // Backward right turn
+    drive.curvatureDrive(-0.5, -0.5, false);
+    assertEquals(-0.25, left.get(), 1e-9);
+    assertEquals(-0.75, right.get(), 1e-9);
+  }
+
+  @Test
+  void testCurvatureDriveTurnInPlace() {
+    var left = new MockPWMMotorController();
+    var right = new MockPWMMotorController();
+    var drive = new DifferentialDrive(left::set, right::set);
+    drive.setDeadband(0.0);
+
+    // Forward
+    drive.curvatureDrive(1.0, 0.0, true);
+    assertEquals(1.0, left.get(), 1e-9);
+    assertEquals(1.0, right.get(), 1e-9);
+
+    // Forward left turn
+    drive.curvatureDrive(0.5, 0.5, true);
+    assertEquals(0.0, left.get(), 1e-9);
+    assertEquals(1.0, right.get(), 1e-9);
+
+    // Forward right turn
+    drive.curvatureDrive(0.5, -0.5, true);
+    assertEquals(1.0, left.get(), 1e-9);
+    assertEquals(0.0, right.get(), 1e-9);
+
+    // Backward
+    drive.curvatureDrive(-1.0, 0.0, true);
+    assertEquals(-1.0, left.get(), 1e-9);
+    assertEquals(-1.0, right.get(), 1e-9);
+
+    // Backward left turn
+    drive.curvatureDrive(-0.5, 0.5, true);
+    assertEquals(-1.0, left.get(), 1e-9);
+    assertEquals(0.0, right.get(), 1e-9);
+
+    // Backward right turn
+    drive.curvatureDrive(-0.5, -0.5, true);
+    assertEquals(0.0, left.get(), 1e-9);
+    assertEquals(-1.0, right.get(), 1e-9);
+  }
+
+  @Test
+  void testTankDrive() {
+    var left = new MockPWMMotorController();
+    var right = new MockPWMMotorController();
+    var drive = new DifferentialDrive(left::set, right::set);
+    drive.setDeadband(0.0);
+
+    // Forward
+    drive.tankDrive(1.0, 1.0, false);
+    assertEquals(1.0, left.get(), 1e-9);
+    assertEquals(1.0, right.get(), 1e-9);
+
+    // Forward left turn
+    drive.tankDrive(0.5, 1.0, false);
+    assertEquals(0.5, left.get(), 1e-9);
+    assertEquals(1.0, right.get(), 1e-9);
+
+    // Forward right turn
+    drive.tankDrive(1.0, 0.5, false);
+    assertEquals(1.0, left.get(), 1e-9);
+    assertEquals(0.5, right.get(), 1e-9);
+
+    // Backward
+    drive.tankDrive(-1.0, -1.0, false);
+    assertEquals(-1.0, left.get(), 1e-9);
+    assertEquals(-1.0, right.get(), 1e-9);
+
+    // Backward left turn
+    drive.tankDrive(-0.5, -1.0, false);
+    assertEquals(-0.5, left.get(), 1e-9);
+    assertEquals(-1.0, right.get(), 1e-9);
+
+    // Backward right turn
+    drive.tankDrive(-0.5, 1.0, false);
+    assertEquals(-0.5, left.get(), 1e-9);
+    assertEquals(1.0, right.get(), 1e-9);
+  }
+
+  @Test
+  void testTankDriveSquared() {
+    var left = new MockPWMMotorController();
+    var right = new MockPWMMotorController();
+    var drive = new DifferentialDrive(left::set, right::set);
+    drive.setDeadband(0.0);
+
+    // Forward
+    drive.tankDrive(1.0, 1.0, true);
+    assertEquals(1.0, left.get(), 1e-9);
+    assertEquals(1.0, right.get(), 1e-9);
+
+    // Forward left turn
+    drive.tankDrive(0.5, 1.0, true);
+    assertEquals(0.25, left.get(), 1e-9);
+    assertEquals(1.0, right.get(), 1e-9);
+
+    // Forward right turn
+    drive.tankDrive(1.0, 0.5, true);
+    assertEquals(1.0, left.get(), 1e-9);
+    assertEquals(0.25, right.get(), 1e-9);
+
+    // Backward
+    drive.tankDrive(-1.0, -1.0, true);
+    assertEquals(-1.0, left.get(), 1e-9);
+    assertEquals(-1.0, right.get(), 1e-9);
+
+    // Backward left turn
+    drive.tankDrive(-0.5, -1.0, true);
+    assertEquals(-0.25, left.get(), 1e-9);
+    assertEquals(-1.0, right.get(), 1e-9);
+
+    // Backward right turn
+    drive.tankDrive(-1.0, -0.5, true);
+    assertEquals(-1.0, left.get(), 1e-9);
+    assertEquals(-0.25, right.get(), 1e-9);
+  }
+}

wpilibj/src/test/java/org/wpilib/drive/MecanumDriveTest.java

@@ -0,0 +1,225 @@
+// 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.drive;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.motorcontrol.MockPWMMotorController;
+import org.junit.jupiter.api.Test;
+
+@SuppressWarnings("resource")
+class MecanumDriveTest {
+  @Test
+  void testCartesianIK() {
+    // Forward
+    var speeds = MecanumDrive.driveCartesianIK(1.0, 0.0, 0.0);
+    assertEquals(1.0, speeds.frontLeft, 1e-9);
+    assertEquals(1.0, speeds.frontRight, 1e-9);
+    assertEquals(1.0, speeds.rearLeft, 1e-9);
+    assertEquals(1.0, speeds.rearRight, 1e-9);
+
+    // Left
+    speeds = MecanumDrive.driveCartesianIK(0.0, -1.0, 0.0);
+    assertEquals(-1.0, speeds.frontLeft, 1e-9);
+    assertEquals(1.0, speeds.frontRight, 1e-9);
+    assertEquals(1.0, speeds.rearLeft, 1e-9);
+    assertEquals(-1.0, speeds.rearRight, 1e-9);
+
+    // Right
+    speeds = MecanumDrive.driveCartesianIK(0.0, 1.0, 0.0);
+    assertEquals(1.0, speeds.frontLeft, 1e-9);
+    assertEquals(-1.0, speeds.frontRight, 1e-9);
+    assertEquals(-1.0, speeds.rearLeft, 1e-9);
+    assertEquals(1.0, speeds.rearRight, 1e-9);
+
+    // Rotate CCW
+    speeds = MecanumDrive.driveCartesianIK(0.0, 0.0, -1.0);
+    assertEquals(-1.0, speeds.frontLeft, 1e-9);
+    assertEquals(1.0, speeds.frontRight, 1e-9);
+    assertEquals(-1.0, speeds.rearLeft, 1e-9);
+    assertEquals(1.0, speeds.rearRight, 1e-9);
+
+    // Rotate CW
+    speeds = MecanumDrive.driveCartesianIK(0.0, 0.0, 1.0);
+    assertEquals(1.0, speeds.frontLeft, 1e-9);
+    assertEquals(-1.0, speeds.frontRight, 1e-9);
+    assertEquals(1.0, speeds.rearLeft, 1e-9);
+    assertEquals(-1.0, speeds.rearRight, 1e-9);
+  }
+
+  @Test
+  void testCartesianIKGyro90CW() {
+    // Forward in global frame; left in robot frame
+    var speeds = MecanumDrive.driveCartesianIK(1.0, 0.0, 0.0, Rotation2d.kCCW_Pi_2);
+    assertEquals(-1.0, speeds.frontLeft, 1e-9);
+    assertEquals(1.0, speeds.frontRight, 1e-9);
+    assertEquals(1.0, speeds.rearLeft, 1e-9);
+    assertEquals(-1.0, speeds.rearRight, 1e-9);
+
+    // Left in global frame; backward in robot frame
+    speeds = MecanumDrive.driveCartesianIK(0.0, -1.0, 0.0, Rotation2d.kCCW_Pi_2);
+    assertEquals(-1.0, speeds.frontLeft, 1e-9);
+    assertEquals(-1.0, speeds.frontRight, 1e-9);
+    assertEquals(-1.0, speeds.rearLeft, 1e-9);
+    assertEquals(-1.0, speeds.rearRight, 1e-9);
+
+    // Right in global frame; forward in robot frame
+    speeds = MecanumDrive.driveCartesianIK(0.0, 1.0, 0.0, Rotation2d.kCCW_Pi_2);
+    assertEquals(1.0, speeds.frontLeft, 1e-9);
+    assertEquals(1.0, speeds.frontRight, 1e-9);
+    assertEquals(1.0, speeds.rearLeft, 1e-9);
+    assertEquals(1.0, speeds.rearRight, 1e-9);
+
+    // Rotate CCW
+    speeds = MecanumDrive.driveCartesianIK(0.0, 0.0, -1.0, Rotation2d.kCCW_Pi_2);
+    assertEquals(-1.0, speeds.frontLeft, 1e-9);
+    assertEquals(1.0, speeds.frontRight, 1e-9);
+    assertEquals(-1.0, speeds.rearLeft, 1e-9);
+    assertEquals(1.0, speeds.rearRight, 1e-9);
+
+    // Rotate CW
+    speeds = MecanumDrive.driveCartesianIK(0.0, 0.0, 1.0, Rotation2d.kCCW_Pi_2);
+    assertEquals(1.0, speeds.frontLeft, 1e-9);
+    assertEquals(-1.0, speeds.frontRight, 1e-9);
+    assertEquals(1.0, speeds.rearLeft, 1e-9);
+    assertEquals(-1.0, speeds.rearRight, 1e-9);
+  }
+
+  @Test
+  void testCartesian() {
+    var fl = new MockPWMMotorController();
+    var rl = new MockPWMMotorController();
+    var fr = new MockPWMMotorController();
+    var rr = new MockPWMMotorController();
+    var drive = new MecanumDrive(fl::set, rl::set, fr::set, rr::set);
+    drive.setDeadband(0.0);
+
+    // Forward
+    drive.driveCartesian(1.0, 0.0, 0.0);
+    assertEquals(1.0, fl.get(), 1e-9);
+    assertEquals(1.0, fr.get(), 1e-9);
+    assertEquals(1.0, rl.get(), 1e-9);
+    assertEquals(1.0, rr.get(), 1e-9);
+
+    // Left
+    drive.driveCartesian(0.0, -1.0, 0.0);
+    assertEquals(-1.0, fl.get(), 1e-9);
+    assertEquals(1.0, fr.get(), 1e-9);
+    assertEquals(1.0, rl.get(), 1e-9);
+    assertEquals(-1.0, rr.get(), 1e-9);
+
+    // Right
+    drive.driveCartesian(0.0, 1.0, 0.0);
+    assertEquals(1.0, fl.get(), 1e-9);
+    assertEquals(-1.0, fr.get(), 1e-9);
+    assertEquals(-1.0, rl.get(), 1e-9);
+    assertEquals(1.0, rr.get(), 1e-9);
+
+    // Rotate CCW
+    drive.driveCartesian(0.0, 0.0, -1.0);
+    assertEquals(-1.0, fl.get(), 1e-9);
+    assertEquals(1.0, fr.get(), 1e-9);
+    assertEquals(-1.0, rl.get(), 1e-9);
+    assertEquals(1.0, rr.get(), 1e-9);
+
+    // Rotate CW
+    drive.driveCartesian(0.0, 0.0, 1.0);
+    assertEquals(1.0, fl.get(), 1e-9);
+    assertEquals(-1.0, fr.get(), 1e-9);
+    assertEquals(1.0, rl.get(), 1e-9);
+    assertEquals(-1.0, rr.get(), 1e-9);
+  }
+
+  @Test
+  void testCartesianGyro90CW() {
+    var fl = new MockPWMMotorController();
+    var rl = new MockPWMMotorController();
+    var fr = new MockPWMMotorController();
+    var rr = new MockPWMMotorController();
+    var drive = new MecanumDrive(fl::set, rl::set, fr::set, rr::set);
+    drive.setDeadband(0.0);
+
+    // Forward in global frame; left in robot frame
+    drive.driveCartesian(1.0, 0.0, 0.0, Rotation2d.kCCW_Pi_2);
+    assertEquals(-1.0, fl.get(), 1e-9);
+    assertEquals(1.0, fr.get(), 1e-9);
+    assertEquals(1.0, rl.get(), 1e-9);
+    assertEquals(-1.0, rr.get(), 1e-9);
+
+    // Left in global frame; backward in robot frame
+    drive.driveCartesian(0.0, -1.0, 0.0, Rotation2d.kCCW_Pi_2);
+    assertEquals(-1.0, fl.get(), 1e-9);
+    assertEquals(-1.0, fr.get(), 1e-9);
+    assertEquals(-1.0, rl.get(), 1e-9);
+    assertEquals(-1.0, rr.get(), 1e-9);
+
+    // Right in global frame; forward in robot frame
+    drive.driveCartesian(0.0, 1.0, 0.0, Rotation2d.kCCW_Pi_2);
+    assertEquals(1.0, fl.get(), 1e-9);
+    assertEquals(1.0, fr.get(), 1e-9);
+    assertEquals(1.0, rl.get(), 1e-9);
+    assertEquals(1.0, rr.get(), 1e-9);
+
+    // Rotate CCW
+    drive.driveCartesian(0.0, 0.0, -1.0, Rotation2d.kCCW_Pi_2);
+    assertEquals(-1.0, fl.get(), 1e-9);
+    assertEquals(1.0, fr.get(), 1e-9);
+    assertEquals(-1.0, rl.get(), 1e-9);
+    assertEquals(1.0, rr.get(), 1e-9);
+
+    // Rotate CW
+    drive.driveCartesian(0.0, 0.0, 1.0, Rotation2d.kCCW_Pi_2);
+    assertEquals(1.0, fl.get(), 1e-9);
+    assertEquals(-1.0, fr.get(), 1e-9);
+    assertEquals(1.0, rl.get(), 1e-9);
+    assertEquals(-1.0, rr.get(), 1e-9);
+  }
+
+  @Test
+  void testPolar() {
+    var fl = new MockPWMMotorController();
+    var rl = new MockPWMMotorController();
+    var fr = new MockPWMMotorController();
+    var rr = new MockPWMMotorController();
+    var drive = new MecanumDrive(fl::set, rl::set, fr::set, rr::set);
+    drive.setDeadband(0.0);
+
+    // Forward
+    drive.drivePolar(1.0, Rotation2d.kZero, 0.0);
+    assertEquals(1.0, fl.get(), 1e-9);
+    assertEquals(1.0, fr.get(), 1e-9);
+    assertEquals(1.0, rl.get(), 1e-9);
+    assertEquals(1.0, rr.get(), 1e-9);
+
+    // Left
+    drive.drivePolar(1.0, Rotation2d.kCW_Pi_2, 0.0);
+    assertEquals(-1.0, fl.get(), 1e-9);
+    assertEquals(1.0, fr.get(), 1e-9);
+    assertEquals(1.0, rl.get(), 1e-9);
+    assertEquals(-1.0, rr.get(), 1e-9);
+
+    // Right
+    drive.drivePolar(1.0, Rotation2d.kCCW_Pi_2, 0.0);
+    assertEquals(1.0, fl.get(), 1e-9);
+    assertEquals(-1.0, fr.get(), 1e-9);
+    assertEquals(-1.0, rl.get(), 1e-9);
+    assertEquals(1.0, rr.get(), 1e-9);
+
+    // Rotate CCW
+    drive.drivePolar(0.0, Rotation2d.kZero, -1.0);
+    assertEquals(-1.0, fl.get(), 1e-9);
+    assertEquals(1.0, fr.get(), 1e-9);
+    assertEquals(-1.0, rl.get(), 1e-9);
+    assertEquals(1.0, rr.get(), 1e-9);
+
+    // Rotate CW
+    drive.drivePolar(0.0, Rotation2d.kZero, 1.0);
+    assertEquals(1.0, fl.get(), 1e-9);
+    assertEquals(-1.0, fr.get(), 1e-9);
+    assertEquals(1.0, rl.get(), 1e-9);
+    assertEquals(-1.0, rr.get(), 1e-9);
+  }
+}

wpilibj/src/test/java/org/wpilib/driverstation/DriverStationTest.java

@@ -0,0 +1,55 @@
+// 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;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.params.provider.Arguments.arguments;
+
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import java.util.stream.Stream;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+
+class DriverStationTest {
+  @ParameterizedTest
+  @MethodSource("isConnectedProvider")
+  void testIsConnected(int axisCount, int buttonCount, int povCount, boolean expected) {
+    DriverStationSim.setJoystickAxesMaximumIndex(1, axisCount);
+    DriverStationSim.setJoystickButtonsMaximumIndex(1, buttonCount);
+    DriverStationSim.setJoystickPOVsMaximumIndex(1, povCount);
+
+    DriverStationSim.notifyNewData();
+
+    assertEquals(expected, DriverStation.isJoystickConnected(1));
+  }
+
+  static Stream<Arguments> isConnectedProvider() {
+    return Stream.of(
+        arguments(0, 0, 0, false),
+        arguments(1, 0, 0, true),
+        arguments(0, 1, 0, true),
+        arguments(0, 0, 1, true),
+        arguments(1, 1, 1, true),
+        arguments(4, 10, 1, true));
+  }
+
+  @MethodSource("connectionWarningProvider")
+  void testConnectionWarnings(boolean fms, boolean silence, boolean expected) {
+    DriverStationSim.setFmsAttached(fms);
+    DriverStationSim.notifyNewData();
+
+    DriverStation.silenceJoystickConnectionWarning(silence);
+    assertEquals(expected, DriverStation.isJoystickConnectionWarningSilenced());
+  }
+
+  static Stream<Arguments> connectionWarningProvider() {
+    return Stream.of(
+        arguments(false, true, true),
+        arguments(false, false, false),
+        arguments(true, true, false),
+        arguments(true, false, false));
+  }
+}

wpilibj/src/test/java/org/wpilib/driverstation/GenericHIDTest.java

@@ -0,0 +1,56 @@
+// 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;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
+import edu.wpi.first.wpilibj.simulation.GenericHIDSim;
+import org.junit.jupiter.api.Test;
+
+class GenericHIDTest {
+  private static final double kEpsilon = 0.0001;
+
+  @Test
+  void testRumbleRange() {
+    GenericHID hid = new GenericHID(0);
+    GenericHIDSim sim = new GenericHIDSim(0);
+
+    for (int i = 0; i <= 100; i++) {
+      double rumbleValue = i / 100.0;
+      hid.setRumble(RumbleType.kBothRumble, rumbleValue);
+      assertEquals(rumbleValue, sim.getRumble(RumbleType.kLeftRumble), kEpsilon);
+      assertEquals(rumbleValue, sim.getRumble(RumbleType.kRightRumble), kEpsilon);
+    }
+  }
+
+  @Test
+  void testRumbleTypes() {
+    GenericHID hid = new GenericHID(0);
+    GenericHIDSim sim = new GenericHIDSim(0);
+
+    // Make sure both are off
+    hid.setRumble(RumbleType.kBothRumble, 0);
+    assertEquals(0, sim.getRumble(RumbleType.kBothRumble), kEpsilon);
+
+    // test both
+    hid.setRumble(RumbleType.kBothRumble, 1);
+    assertEquals(1, sim.getRumble(RumbleType.kLeftRumble), kEpsilon);
+    assertEquals(1, sim.getRumble(RumbleType.kRightRumble), kEpsilon);
+    hid.setRumble(RumbleType.kBothRumble, 0);
+
+    // test left only
+    hid.setRumble(RumbleType.kLeftRumble, 1);
+    assertEquals(1, sim.getRumble(RumbleType.kLeftRumble), kEpsilon);
+    assertEquals(0, sim.getRumble(RumbleType.kRightRumble), kEpsilon);
+    hid.setRumble(RumbleType.kLeftRumble, 0);
+
+    // test right only
+    hid.setRumble(RumbleType.kRightRumble, 1);
+    assertEquals(0, sim.getRumble(RumbleType.kLeftRumble), kEpsilon);
+    assertEquals(1, sim.getRumble(RumbleType.kRightRumble), kEpsilon);
+    hid.setRumble(RumbleType.kRightRumble, 0);
+  }
+}

wpilibj/src/test/java/org/wpilib/driverstation/JoystickTest.java

@@ -0,0 +1,173 @@
+// 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;
+
+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.HAL;
+import edu.wpi.first.wpilibj.simulation.JoystickSim;
+import org.junit.jupiter.api.Test;
+
+class JoystickTest {
+  @Test
+  void testGetX() {
+    HAL.initialize(500, 0);
+    Joystick joy = new Joystick(1);
+    JoystickSim joysim = new JoystickSim(joy);
+
+    joysim.setX(0.25);
+    joysim.notifyNewData();
+    assertEquals(0.25, joy.getX(), 0.001);
+
+    joysim.setX(0);
+    joysim.notifyNewData();
+    assertEquals(0.0, joy.getX(), 0.001);
+  }
+
+  @Test
+  void testGetY() {
+    HAL.initialize(500, 0);
+    Joystick joy = new Joystick(1);
+    JoystickSim joysim = new JoystickSim(joy);
+
+    joysim.setY(0.25);
+    joysim.notifyNewData();
+    assertEquals(0.25, joy.getY(), 0.001);
+
+    joysim.setY(0);
+    joysim.notifyNewData();
+    assertEquals(0.0, joy.getY(), 0.001);
+  }
+
+  @Test
+  void testGetZ() {
+    HAL.initialize(500, 0);
+    Joystick joy = new Joystick(1);
+    JoystickSim joysim = new JoystickSim(joy);
+
+    joysim.setZ(0.25);
+    joysim.notifyNewData();
+    assertEquals(0.25, joy.getZ(), 0.001);
+
+    joysim.setZ(0);
+    joysim.notifyNewData();
+    assertEquals(0.0, joy.getZ(), 0.001);
+  }
+
+  @Test
+  void testGetTwist() {
+    HAL.initialize(500, 0);
+    Joystick joy = new Joystick(1);
+    JoystickSim joysim = new JoystickSim(joy);
+
+    joysim.setTwist(0.25);
+    joysim.notifyNewData();
+    assertEquals(0.25, joy.getTwist(), 0.001);
+
+    joysim.setTwist(0);
+    joysim.notifyNewData();
+    assertEquals(0.0, joy.getTwist(), 0.001);
+  }
+
+  @Test
+  void testGetThrottle() {
+    HAL.initialize(500, 0);
+    Joystick joy = new Joystick(1);
+    JoystickSim joysim = new JoystickSim(joy);
+
+    joysim.setThrottle(0.25);
+    joysim.notifyNewData();
+    assertEquals(0.25, joy.getThrottle(), 0.001);
+
+    joysim.setThrottle(0);
+    joysim.notifyNewData();
+    assertEquals(0.0, joy.getThrottle(), 0.001);
+  }
+
+  @Test
+  void testGetTrigger() {
+    HAL.initialize(500, 0);
+    Joystick joy = new Joystick(1);
+    JoystickSim joysim = new JoystickSim(joy);
+
+    joysim.setTrigger(true);
+    joysim.notifyNewData();
+    assertTrue(joy.getTrigger());
+
+    joysim.setTrigger(false);
+    joysim.notifyNewData();
+    assertFalse(joy.getTrigger());
+  }
+
+  @Test
+  void testGetTop() {
+    HAL.initialize(500, 0);
+    Joystick joy = new Joystick(1);
+    JoystickSim joysim = new JoystickSim(joy);
+
+    joysim.setTop(true);
+    joysim.notifyNewData();
+    assertTrue(joy.getTop());
+
+    joysim.setTop(false);
+    joysim.notifyNewData();
+    assertFalse(joy.getTop());
+  }
+
+  @Test
+  void testGetMagnitude() {
+    HAL.initialize(500, 0);
+    Joystick joy = new Joystick(1);
+    JoystickSim joysim = new JoystickSim(joy);
+
+    // X Only
+    joysim.setX(0.5);
+    joysim.setY(0.0);
+    joysim.notifyNewData();
+    assertEquals(0.5, joy.getMagnitude(), 0.001);
+
+    // Y Only
+    joysim.setX(0.0);
+    joysim.setY(-0.5);
+    joysim.notifyNewData();
+    assertEquals(0.5, joy.getMagnitude(), 0.001);
+
+    // Both
+    joysim.setX(0.5);
+    joysim.setY(-0.5);
+    joysim.notifyNewData();
+    assertEquals(0.70710678118, joy.getMagnitude(), 0.001);
+  }
+
+  @Test
+  void testGetDirection() {
+    HAL.initialize(500, 0);
+    Joystick joy = new Joystick(1);
+    JoystickSim joysim = new JoystickSim(joy);
+
+    // X Only
+    joysim.setX(0.5);
+    joysim.setY(0.0);
+    joysim.notifyNewData();
+    assertEquals(90, joy.getDirectionDegrees(), 0.001);
+    assertEquals(Math.toRadians(90), joy.getDirectionRadians(), 0.001);
+
+    // Y Only
+    joysim.setX(0.0);
+    joysim.setY(-0.5);
+    joysim.notifyNewData();
+    assertEquals(0, joy.getDirectionDegrees(), 0.001);
+    assertEquals(Math.toRadians(0), joy.getDirectionRadians(), 0.001);
+
+    // Both
+    joysim.setX(0.5);
+    joysim.setY(-0.5);
+    joysim.notifyNewData();
+    assertEquals(45, joy.getDirectionDegrees(), 0.001);
+    assertEquals(Math.toRadians(45), joy.getDirectionRadians(), 0.001);
+  }
+}

wpilibj/src/test/java/org/wpilib/driverstation/PS4ControllerTest.java

@@ -0,0 +1,79 @@
+// 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;
+
+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.HAL;
+import edu.wpi.first.wpilibj.simulation.PS4ControllerSim;
+import java.lang.reflect.InvocationTargetException;
+import java.lang.reflect.Method;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.EnumSource;
+
+class PS4ControllerTest {
+  @ParameterizedTest
+  @EnumSource(value = PS4Controller.Button.class)
+  void testButtons(PS4Controller.Button button)
+      throws NoSuchMethodException, InvocationTargetException, IllegalAccessException {
+    HAL.initialize(500, 0);
+    PS4Controller joy = new PS4Controller(2);
+    PS4ControllerSim joysim = new PS4ControllerSim(joy);
+
+    var buttonName = button.toString();
+
+    String simSetMethodName = "set" + buttonName;
+    String joyGetMethodName = "get" + buttonName;
+    String joyPressedMethodName = "get" + buttonName + "Pressed";
+    String joyReleasedMethodName = "get" + buttonName + "Released";
+
+    final Method simSetMethod = joysim.getClass().getMethod(simSetMethodName, boolean.class);
+    final Method joyGetMethod = joy.getClass().getMethod(joyGetMethodName);
+    final Method joyPressedMethod = joy.getClass().getMethod(joyPressedMethodName);
+    final Method joyReleasedMethod = joy.getClass().getMethod(joyReleasedMethodName);
+
+    simSetMethod.invoke(joysim, false);
+    joysim.notifyNewData();
+    assertFalse((Boolean) joyGetMethod.invoke(joy));
+    // need to call pressed and released to clear flags
+    joyPressedMethod.invoke(joy);
+    joyReleasedMethod.invoke(joy);
+
+    simSetMethod.invoke(joysim, true);
+    joysim.notifyNewData();
+    assertTrue((Boolean) joyGetMethod.invoke(joy));
+    assertTrue((Boolean) joyPressedMethod.invoke(joy));
+    assertFalse((Boolean) joyReleasedMethod.invoke(joy));
+
+    simSetMethod.invoke(joysim, false);
+    joysim.notifyNewData();
+    assertFalse((Boolean) joyGetMethod.invoke(joy));
+    assertFalse((Boolean) joyPressedMethod.invoke(joy));
+    assertTrue((Boolean) joyReleasedMethod.invoke(joy));
+  }
+
+  @ParameterizedTest
+  @EnumSource(value = PS4Controller.Axis.class)
+  void testAxes(PS4Controller.Axis axis)
+      throws NoSuchMethodException, InvocationTargetException, IllegalAccessException {
+    HAL.initialize(500, 0);
+    PS4Controller joy = new PS4Controller(2);
+    PS4ControllerSim joysim = new PS4ControllerSim(joy);
+
+    var axisName = axis.toString();
+
+    String simSetMethodName = "set" + axisName;
+    String joyGetMethodName = "get" + axisName;
+
+    Method simSetMethod = joysim.getClass().getMethod(simSetMethodName, double.class);
+    Method joyGetMethod = joy.getClass().getMethod(joyGetMethodName);
+
+    simSetMethod.invoke(joysim, 0.35);
+    joysim.notifyNewData();
+    assertEquals(0.35, (Double) joyGetMethod.invoke(joy), 0.001);
+  }
+}

wpilibj/src/test/java/org/wpilib/driverstation/PS5ControllerTest.java

@@ -0,0 +1,79 @@
+// 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;
+
+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.HAL;
+import edu.wpi.first.wpilibj.simulation.PS5ControllerSim;
+import java.lang.reflect.InvocationTargetException;
+import java.lang.reflect.Method;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.EnumSource;
+
+class PS5ControllerTest {
+  @ParameterizedTest
+  @EnumSource(value = PS5Controller.Button.class)
+  void testButtons(PS5Controller.Button button)
+      throws NoSuchMethodException, InvocationTargetException, IllegalAccessException {
+    HAL.initialize(500, 0);
+    PS5Controller joy = new PS5Controller(2);
+    PS5ControllerSim joysim = new PS5ControllerSim(joy);
+
+    var buttonName = button.toString();
+
+    String simSetMethodName = "set" + buttonName;
+    String joyGetMethodName = "get" + buttonName;
+    String joyPressedMethodName = "get" + buttonName + "Pressed";
+    String joyReleasedMethodName = "get" + buttonName + "Released";
+
+    final Method simSetMethod = joysim.getClass().getMethod(simSetMethodName, boolean.class);
+    final Method joyGetMethod = joy.getClass().getMethod(joyGetMethodName);
+    final Method joyPressedMethod = joy.getClass().getMethod(joyPressedMethodName);
+    final Method joyReleasedMethod = joy.getClass().getMethod(joyReleasedMethodName);
+
+    simSetMethod.invoke(joysim, false);
+    joysim.notifyNewData();
+    assertFalse((Boolean) joyGetMethod.invoke(joy));
+    // need to call pressed and released to clear flags
+    joyPressedMethod.invoke(joy);
+    joyReleasedMethod.invoke(joy);
+
+    simSetMethod.invoke(joysim, true);
+    joysim.notifyNewData();
+    assertTrue((Boolean) joyGetMethod.invoke(joy));
+    assertTrue((Boolean) joyPressedMethod.invoke(joy));
+    assertFalse((Boolean) joyReleasedMethod.invoke(joy));
+
+    simSetMethod.invoke(joysim, false);
+    joysim.notifyNewData();
+    assertFalse((Boolean) joyGetMethod.invoke(joy));
+    assertFalse((Boolean) joyPressedMethod.invoke(joy));
+    assertTrue((Boolean) joyReleasedMethod.invoke(joy));
+  }
+
+  @ParameterizedTest
+  @EnumSource(value = PS5Controller.Axis.class)
+  void testAxes(PS5Controller.Axis axis)
+      throws NoSuchMethodException, InvocationTargetException, IllegalAccessException {
+    HAL.initialize(500, 0);
+    PS5Controller joy = new PS5Controller(2);
+    PS5ControllerSim joysim = new PS5ControllerSim(joy);
+
+    var axisName = axis.toString();
+
+    String simSetMethodName = "set" + axisName;
+    String joyGetMethodName = "get" + axisName;
+
+    Method simSetMethod = joysim.getClass().getMethod(simSetMethodName, double.class);
+    Method joyGetMethod = joy.getClass().getMethod(joyGetMethodName);
+
+    simSetMethod.invoke(joysim, 0.35);
+    joysim.notifyNewData();
+    assertEquals(0.35, (Double) joyGetMethod.invoke(joy), 0.001);
+  }
+}

wpilibj/src/test/java/org/wpilib/driverstation/XboxControllerTest.java

@@ -0,0 +1,79 @@
+// 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;
+
+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.HAL;
+import edu.wpi.first.wpilibj.simulation.XboxControllerSim;
+import java.lang.reflect.InvocationTargetException;
+import java.lang.reflect.Method;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.EnumSource;
+
+class XboxControllerTest {
+  @ParameterizedTest
+  @EnumSource(value = XboxController.Button.class)
+  void testButtons(XboxController.Button button)
+      throws NoSuchMethodException, InvocationTargetException, IllegalAccessException {
+    HAL.initialize(500, 0);
+    XboxController joy = new XboxController(2);
+    XboxControllerSim joysim = new XboxControllerSim(joy);
+
+    var buttonName = button.toString();
+
+    String simSetMethodName = "set" + buttonName;
+    String joyGetMethodName = "get" + buttonName;
+    String joyPressedMethodName = "get" + buttonName + "Pressed";
+    String joyReleasedMethodName = "get" + buttonName + "Released";
+
+    final Method simSetMethod = joysim.getClass().getMethod(simSetMethodName, boolean.class);
+    final Method joyGetMethod = joy.getClass().getMethod(joyGetMethodName);
+    final Method joyPressedMethod = joy.getClass().getMethod(joyPressedMethodName);
+    final Method joyReleasedMethod = joy.getClass().getMethod(joyReleasedMethodName);
+
+    simSetMethod.invoke(joysim, false);
+    joysim.notifyNewData();
+    assertFalse((Boolean) joyGetMethod.invoke(joy));
+    // need to call pressed and released to clear flags
+    joyPressedMethod.invoke(joy);
+    joyReleasedMethod.invoke(joy);
+
+    simSetMethod.invoke(joysim, true);
+    joysim.notifyNewData();
+    assertTrue((Boolean) joyGetMethod.invoke(joy));
+    assertTrue((Boolean) joyPressedMethod.invoke(joy));
+    assertFalse((Boolean) joyReleasedMethod.invoke(joy));
+
+    simSetMethod.invoke(joysim, false);
+    joysim.notifyNewData();
+    assertFalse((Boolean) joyGetMethod.invoke(joy));
+    assertFalse((Boolean) joyPressedMethod.invoke(joy));
+    assertTrue((Boolean) joyReleasedMethod.invoke(joy));
+  }
+
+  @ParameterizedTest
+  @EnumSource(value = XboxController.Axis.class)
+  void testAxes(XboxController.Axis axis)
+      throws NoSuchMethodException, InvocationTargetException, IllegalAccessException {
+    HAL.initialize(500, 0);
+    XboxController joy = new XboxController(2);
+    XboxControllerSim joysim = new XboxControllerSim(joy);
+
+    var axisName = axis.toString();
+
+    String simSetMethodName = "set" + axisName;
+    String joyGetMethodName = "get" + axisName;
+
+    Method simSetMethod = joysim.getClass().getMethod(simSetMethodName, double.class);
+    Method joyGetMethod = joy.getClass().getMethod(joyGetMethodName);
+
+    simSetMethod.invoke(joysim, 0.35);
+    joysim.notifyNewData();
+    assertEquals(0.35, (Double) joyGetMethod.invoke(joy), 0.001);
+  }
+}

wpilibj/src/test/java/org/wpilib/event/BooleanEventTest.java

@@ -0,0 +1,495 @@
+// 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.event;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import java.util.concurrent.atomic.AtomicBoolean;
+import java.util.concurrent.atomic.AtomicInteger;
+import org.junit.jupiter.api.Test;
+
+class BooleanEventTest {
+  @Test
+  void testBinaryCompositions() {
+    var loop = new EventLoop();
+
+    var andCounter = new AtomicInteger(0);
+    var orCounter = new AtomicInteger(0);
+
+    assertEquals(0, andCounter.get());
+    assertEquals(0, orCounter.get());
+
+    new BooleanEvent(loop, () -> true).and(() -> false).ifHigh(andCounter::incrementAndGet);
+    new BooleanEvent(loop, () -> true).or(() -> false).ifHigh(orCounter::incrementAndGet);
+
+    loop.poll();
+
+    assertEquals(0, andCounter.get());
+    assertEquals(1, orCounter.get());
+  }
+
+  /**
+   * Tests that composed edge events only execute on edges (two rising edge events composed with
+   * and() should only execute when both signals are on the rising edge).
+   */
+  @Test
+  void testBinaryCompositionsWithEdgeDecorators() {
+    var loop = new EventLoop();
+    var bool1 = new AtomicBoolean(false);
+    var bool2 = new AtomicBoolean(false);
+    var bool3 = new AtomicBoolean(false);
+    var bool4 = new AtomicBoolean(false);
+    var counter = new AtomicInteger(0);
+
+    var event1 = new BooleanEvent(loop, bool1::get).rising();
+    var event2 = new BooleanEvent(loop, bool2::get).rising();
+    var event3 = new BooleanEvent(loop, bool3::get).rising();
+    var event4 = new BooleanEvent(loop, bool4::get).rising();
+    event1.and(event2).ifHigh(counter::incrementAndGet);
+    event3.or(event4).ifHigh(counter::incrementAndGet);
+    assertEquals(0, counter.get());
+
+    bool1.set(true);
+    bool2.set(true);
+    bool3.set(true);
+    bool4.set(true);
+    loop.poll(); // Both actions execute
+
+    assertEquals(2, counter.get());
+
+    loop.poll(); // Nothing should happen since nothing is on rising edge
+
+    assertEquals(2, counter.get());
+
+    bool1.set(false);
+    bool2.set(false);
+    bool3.set(false);
+    bool4.set(false);
+    loop.poll(); // Nothing should happen
+
+    assertEquals(2, counter.get());
+
+    bool1.set(true);
+    loop.poll(); // Nothing should happen since only Bool 1 is on rising edge
+
+    assertEquals(2, counter.get());
+
+    bool2.set(true);
+    loop.poll(); // Bool 2 is on rising edge, but Bool 1 isn't, nothing should happen
+
+    assertEquals(2, counter.get());
+
+    bool1.set(false);
+    bool2.set(false);
+    loop.poll(); // Nothing should happen
+
+    assertEquals(2, counter.get());
+
+    bool1.set(true);
+    bool2.set(true);
+    loop.poll(); // Bool 1 and 2 are on rising edge, increments counter once
+
+    assertEquals(3, counter.get());
+
+    bool3.set(true);
+    loop.poll(); // Bool 3 is on rising edge, increments counter once
+
+    assertEquals(4, counter.get());
+
+    loop.poll(); // Nothing should happen, Bool 3 isn't on rising edge
+
+    assertEquals(4, counter.get());
+
+    bool4.set(true);
+    loop.poll(); // Bool 4 is on rising edge, increments counter once
+
+    assertEquals(5, counter.get());
+
+    loop.poll(); // Nothing should happen, Bool 4 isn't on rising edge
+
+    assertEquals(5, counter.get());
+  }
+
+  @Test
+  void testBinaryCompositionLoopSemantics() {
+    var loop1 = new EventLoop();
+    var loop2 = new EventLoop();
+    var bool1 = new AtomicBoolean(true);
+    var bool2 = new AtomicBoolean(true);
+    var counter1 = new AtomicInteger(0);
+    var counter2 = new AtomicInteger(0);
+
+    new BooleanEvent(loop1, bool1::get)
+        .and(new BooleanEvent(loop2, bool2::get))
+        .ifHigh(counter1::incrementAndGet);
+
+    new BooleanEvent(loop2, bool2::get)
+        .and(new BooleanEvent(loop1, bool1::get))
+        .ifHigh(counter2::incrementAndGet);
+
+    assertEquals(0, counter1.get());
+    assertEquals(0, counter2.get());
+
+    loop1.poll(); // 1st event executes, Bool 1 and 2 are true, increments counter
+
+    assertEquals(1, counter1.get());
+    assertEquals(0, counter2.get());
+
+    loop2.poll(); // 2nd event executes, Bool 1 and 2 are true, increments counter
+
+    assertEquals(1, counter1.get());
+    assertEquals(1, counter2.get());
+
+    bool2.set(false);
+    loop1.poll(); // 1st event executes, Bool 2 is still true because loop 2 hasn't updated it,
+    // increments counter
+
+    assertEquals(2, counter1.get());
+    assertEquals(1, counter2.get());
+
+    loop2.poll(); // 2nd event executes, Bool 2 is now false because this loop updated it, does
+    // nothing
+
+    assertEquals(2, counter1.get());
+    assertEquals(1, counter2.get());
+
+    loop1.poll(); // All bools are updated at this point, nothing should happen
+
+    assertEquals(2, counter1.get());
+    assertEquals(1, counter2.get());
+
+    bool2.set(true);
+    loop2.poll(); // 2nd event executes, Bool 2 is true because this loop updated it, increments
+    // counter
+
+    assertEquals(2, counter1.get());
+    assertEquals(2, counter2.get());
+
+    loop1
+        .poll(); // 1st event executes, Bool 2 is true because loop 2 updated it, increments counter
+
+    assertEquals(3, counter1.get());
+    assertEquals(2, counter2.get());
+
+    bool1.set(false);
+    loop2.poll(); // 2nd event executes, Bool 1 is still true because loop 1 hasn't updated it,
+    // increments counter
+
+    assertEquals(3, counter1.get());
+    assertEquals(3, counter2.get());
+
+    loop1.poll(); // 1st event executes, Bool 1 is false because this loop updated it, does nothing
+
+    assertEquals(3, counter1.get());
+    assertEquals(3, counter2.get());
+
+    loop2.poll(); // All bools are updated at this point, nothing should happen
+
+    assertEquals(3, counter1.get());
+    assertEquals(3, counter2.get());
+  }
+
+  /** Tests the order of actions bound to an event loop. */
+  @Test
+  void testPollOrdering() {
+    var loop = new EventLoop();
+    var bool1 = new AtomicBoolean(true);
+    var bool2 = new AtomicBoolean(true);
+    var enableAssert = new AtomicBoolean(false);
+    var counter = new AtomicInteger(0);
+    // This event binds an action to the event loop first
+    new BooleanEvent(
+            loop,
+            () -> {
+              if (enableAssert.get()) {
+                counter.incrementAndGet();
+                assertEquals(1, counter.get() % 3);
+              }
+              return bool1.get();
+            })
+        // The composed event binds an action to the event loop third
+        .and(
+            // This event binds an action to the event loop second
+            new BooleanEvent(
+                loop,
+                () -> {
+                  if (enableAssert.get()) {
+                    counter.incrementAndGet();
+                    assertEquals(2, counter.get() % 3);
+                  }
+                  return bool2.get();
+                }))
+        // This binds an action to the event loop fourth
+        .ifHigh(
+            () -> {
+              if (enableAssert.get()) {
+                counter.incrementAndGet();
+                assertEquals(0, counter.get() % 3);
+              }
+            });
+    enableAssert.set(true);
+    loop.poll();
+    loop.poll();
+    loop.poll();
+    loop.poll();
+  }
+
+  @Test
+  void testEdgeDecorators() {
+    var loop = new EventLoop();
+    var bool = new AtomicBoolean(false);
+    var counter = new AtomicInteger(0);
+
+    new BooleanEvent(loop, bool::get).falling().ifHigh(counter::decrementAndGet);
+    new BooleanEvent(loop, bool::get).rising().ifHigh(counter::incrementAndGet);
+
+    assertEquals(0, counter.get());
+
+    bool.set(false);
+    loop.poll();
+
+    assertEquals(0, counter.get());
+
+    bool.set(true);
+    loop.poll();
+
+    assertEquals(1, counter.get());
+
+    bool.set(true);
+    loop.poll();
+
+    assertEquals(1, counter.get());
+
+    bool.set(false);
+    loop.poll();
+
+    assertEquals(0, counter.get());
+  }
+
+  /** Tests that binding actions to the same edge event will result in all actions executing. */
+  @Test
+  void testEdgeReuse() {
+    var loop = new EventLoop();
+    var bool = new AtomicBoolean(false);
+    var counter = new AtomicInteger(0);
+
+    var event = new BooleanEvent(loop, bool::get).rising();
+    event.ifHigh(counter::incrementAndGet);
+    event.ifHigh(counter::incrementAndGet);
+
+    assertEquals(0, counter.get());
+
+    loop.poll();
+
+    assertEquals(0, counter.get());
+
+    bool.set(true);
+    loop.poll();
+
+    assertEquals(2, counter.get());
+
+    loop.poll();
+
+    assertEquals(2, counter.get());
+
+    bool.set(false);
+    loop.poll();
+
+    assertEquals(2, counter.get());
+
+    bool.set(true);
+    loop.poll();
+
+    assertEquals(4, counter.get());
+  }
+
+  /** Tests that all actions execute on separate edge events constructed from the original event. */
+  @Test
+  void testEdgeReconstruct() {
+    var loop = new EventLoop();
+    var bool = new AtomicBoolean(false);
+    var counter = new AtomicInteger(0);
+
+    var event = new BooleanEvent(loop, bool::get);
+    event.rising().ifHigh(counter::incrementAndGet);
+    event.rising().ifHigh(counter::incrementAndGet);
+
+    assertEquals(0, counter.get());
+
+    loop.poll();
+
+    assertEquals(0, counter.get());
+
+    bool.set(true);
+    loop.poll();
+
+    assertEquals(2, counter.get());
+
+    loop.poll();
+
+    assertEquals(2, counter.get());
+
+    bool.set(false);
+    loop.poll();
+
+    assertEquals(2, counter.get());
+
+    bool.set(true);
+    loop.poll();
+
+    assertEquals(4, counter.get());
+  }
+
+  /**
+   * Tests that all actions bound to an event will still execute even if the signal is changed
+   * during the loop poll.
+   */
+  @Test
+  void testMidLoopBooleanChange() {
+    var loop = new EventLoop();
+    var bool = new AtomicBoolean(false);
+    var counter = new AtomicInteger(0);
+
+    var event = new BooleanEvent(loop, bool::get).rising();
+    event.ifHigh(
+        () -> {
+          bool.set(false);
+          counter.incrementAndGet();
+        });
+    event.ifHigh(counter::incrementAndGet);
+
+    assertEquals(0, counter.get());
+
+    loop.poll();
+
+    assertEquals(0, counter.get());
+
+    bool.set(true);
+    loop.poll();
+
+    assertEquals(2, counter.get());
+
+    loop.poll();
+
+    assertEquals(2, counter.get());
+
+    bool.set(false);
+    loop.poll();
+
+    assertEquals(2, counter.get());
+
+    bool.set(true);
+    loop.poll();
+
+    assertEquals(4, counter.get());
+  }
+
+  /**
+   * Tests that all actions bound to composed events will still execute even if the composed signal
+   * changes during the loop poll.
+   */
+  @Test
+  void testMidLoopBooleanChangeWithComposedEvents() {
+    var loop = new EventLoop();
+    var bool1 = new AtomicBoolean(false);
+    var bool2 = new AtomicBoolean(false);
+    var bool3 = new AtomicBoolean(false);
+    var bool4 = new AtomicBoolean(false);
+    var counter = new AtomicInteger(0);
+
+    var event1 = new BooleanEvent(loop, bool1::get);
+    var event2 = new BooleanEvent(loop, bool2::get);
+    var event3 = new BooleanEvent(loop, bool3::get);
+    var event4 = new BooleanEvent(loop, bool4::get);
+    event1.ifHigh(
+        () -> {
+          bool2.set(false);
+          bool3.set(false);
+          counter.incrementAndGet();
+        });
+    event3
+        .or(event4)
+        .ifHigh(
+            () -> {
+              bool1.set(false);
+              counter.incrementAndGet();
+            });
+    event1
+        .and(event2)
+        .ifHigh(
+            () -> {
+              bool4.set(false);
+              counter.incrementAndGet();
+            });
+
+    assertEquals(0, counter.get());
+
+    bool1.set(true);
+    bool2.set(true);
+    bool3.set(true);
+    bool4.set(true);
+    loop.poll(); // All three actions execute, incrementing the counter three times and setting all
+    // booleans to false
+
+    assertEquals(3, counter.get());
+
+    loop.poll(); // Nothing should happen since everything was set to false
+
+    assertEquals(3, counter.get());
+
+    bool1.set(true);
+    bool2.set(true);
+    loop.poll(); // Bool 1 and 2 are true, increments counter twice, Bool 2 gets set to false
+
+    assertEquals(5, counter.get());
+
+    bool1.set(false);
+    loop.poll(); // Nothing should happen
+
+    assertEquals(5, counter.get());
+
+    bool1.set(true);
+    bool3.set(true);
+    loop.poll(); // Bool 1 and 3 are true, increments counter twice, Bool 3 gets set to false
+
+    assertEquals(7, counter.get());
+
+    bool1.set(false);
+    bool4.set(true);
+    loop.poll(); // Bool 4 is true, increments counter once
+
+    assertEquals(8, counter.get());
+  }
+
+  @Test
+  void testNegation() {
+    var loop = new EventLoop();
+    var bool = new AtomicBoolean(false);
+    var counter = new AtomicInteger(0);
+
+    new BooleanEvent(loop, bool::get).negate().ifHigh(counter::incrementAndGet);
+
+    assertEquals(0, counter.get());
+
+    loop.poll();
+
+    assertEquals(1, counter.get());
+
+    bool.set(true);
+    loop.poll();
+
+    assertEquals(1, counter.get());
+
+    bool.set(false);
+    loop.poll();
+
+    assertEquals(2, counter.get());
+
+    bool.set(true);
+    loop.poll();
+
+    assertEquals(2, counter.get());
+  }
+}

wpilibj/src/test/java/org/wpilib/event/EventLoopTest.java

@@ -0,0 +1,92 @@
+// 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.event;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+import java.util.ConcurrentModificationException;
+import java.util.concurrent.atomic.AtomicBoolean;
+import java.util.concurrent.atomic.AtomicInteger;
+import org.junit.jupiter.api.Test;
+
+class EventLoopTest {
+  @Test
+  void testConditions() {
+    var counterTrue = new AtomicInteger(0);
+    var counterFalse = new AtomicInteger(0);
+    var loop = new EventLoop();
+    new BooleanEvent(loop, () -> true).ifHigh(counterTrue::incrementAndGet);
+    new BooleanEvent(loop, () -> false).ifHigh(counterFalse::incrementAndGet);
+
+    assertEquals(0, counterTrue.get());
+    assertEquals(0, counterFalse.get());
+
+    loop.poll();
+
+    assertEquals(1, counterTrue.get());
+    assertEquals(0, counterFalse.get());
+
+    loop.poll();
+
+    assertEquals(2, counterTrue.get());
+    assertEquals(0, counterFalse.get());
+  }
+
+  @Test
+  void testClear() {
+    var condition = new AtomicBoolean(false);
+    var counter = new AtomicInteger(0);
+    var loop = new EventLoop();
+
+    // first ensure binding works
+    new BooleanEvent(loop, condition::get).ifHigh(counter::incrementAndGet);
+
+    condition.set(false);
+    loop.poll();
+    assertEquals(0, counter.get());
+
+    condition.set(true);
+    loop.poll();
+    assertEquals(1, counter.get());
+
+    // clear bindings
+    loop.clear();
+
+    condition.set(true);
+    loop.poll();
+    // shouldn't change
+    assertEquals(1, counter.get());
+  }
+
+  @Test
+  void testConcurrentModification() {
+    var loop = new EventLoop();
+
+    loop.bind(
+        () -> {
+          assertThrows(
+              ConcurrentModificationException.class,
+              () -> {
+                loop.bind(() -> {});
+              });
+        });
+
+    loop.poll();
+
+    loop.clear();
+
+    loop.bind(
+        () -> {
+          assertThrows(
+              ConcurrentModificationException.class,
+              () -> {
+                loop.clear();
+              });
+        });
+
+    loop.poll();
+  }
+}

wpilibj/src/test/java/org/wpilib/event/NetworkBooleanEventTest.java

@@ -0,0 +1,47 @@
+// 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.event;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.networktables.NetworkTableInstance;
+import java.util.concurrent.atomic.AtomicInteger;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+class NetworkBooleanEventTest {
+  NetworkTableInstance m_inst;
+
+  @BeforeEach
+  void setup() {
+    m_inst = NetworkTableInstance.create();
+    m_inst.startLocal();
+  }
+
+  @AfterEach
+  void teardown() {
+    m_inst.close();
+  }
+
+  @Test
+  void testNetworkBooleanEvent() {
+    var loop = new EventLoop();
+    var counter = new AtomicInteger(0);
+
+    var pub = m_inst.getTable("TestTable").getBooleanTopic("Test").publish();
+
+    new NetworkBooleanEvent(loop, m_inst, "TestTable", "Test").ifHigh(counter::incrementAndGet);
+    pub.set(false);
+    loop.poll();
+    assertEquals(0, counter.get());
+    pub.set(true);
+    loop.poll();
+    assertEquals(1, counter.get());
+    pub.set(false);
+    loop.poll();
+    assertEquals(1, counter.get());
+  }
+}

wpilibj/src/test/java/org/wpilib/hal/JNITest.java

@@ -0,0 +1,25 @@
+// 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.hal;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.HALUtil;
+import edu.wpi.first.networktables.NetworkTablesJNI;
+import org.junit.jupiter.api.Test;
+
+class JNITest {
+  @Test
+  void jniNtcoreLinkTest() {
+    // Test to verify that the JNI test link works correctly.
+    NetworkTablesJNI.flush(NetworkTablesJNI.getDefaultInstance());
+  }
+
+  @Test
+  void jniHalLinkTest() {
+    HAL.initialize(500, 0);
+    // Test to verify that the JNI test link works correctly.
+    HALUtil.getHALRuntimeType();
+  }
+}

wpilibj/src/test/java/org/wpilib/hal/MatchInfoDataTest.java

@@ -0,0 +1,35 @@
+// 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.hal;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.hal.DriverStationJNI;
+import edu.wpi.first.hal.MatchInfoData;
+import edu.wpi.first.hal.simulation.DriverStationDataJNI;
+import edu.wpi.first.wpilibj.DriverStation.MatchType;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import org.junit.jupiter.api.Test;
+
+class MatchInfoDataTest {
+  @Test
+  void testSetMatchInfo() {
+    MatchType matchType = MatchType.Qualification;
+    DriverStationDataJNI.setMatchInfo("Event Name", "Game Message", 174, 191, matchType.ordinal());
+
+    DriverStationSim.notifyNewData();
+
+    MatchInfoData outMatchInfo = new MatchInfoData();
+    DriverStationJNI.getMatchInfo(outMatchInfo);
+
+    assertAll(
+        () -> assertEquals("Event Name", outMatchInfo.eventName),
+        () -> assertEquals(matchType.ordinal(), outMatchInfo.matchType),
+        () -> assertEquals(174, outMatchInfo.matchNumber),
+        () -> assertEquals(191, outMatchInfo.replayNumber),
+        () -> assertEquals("Game Message", outMatchInfo.gameSpecificMessage));
+  }
+}

wpilibj/src/test/java/org/wpilib/hardware/discrete/DigitalOutputTest.java

@@ -0,0 +1,45 @@
+// 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;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.DIOSim;
+import org.junit.jupiter.api.Test;
+
+class DigitalOutputTest {
+  @Test
+  void testDefaultFunctions() {
+    try (DigitalOutput output = new DigitalOutput(0)) {
+      assertFalse(output.isPulsing());
+    }
+  }
+
+  @Test
+  void testPwmFunctionsWithoutInitialization() {
+    HAL.initialize(500, 0);
+
+    try (DigitalOutput output = new DigitalOutput(0)) {
+      assertDoesNotThrow(() -> output.updateDutyCycle(0.6));
+      assertDoesNotThrow(output::disablePWM);
+    }
+  }
+
+  @Test
+  void testPwmFunctionsWithInitialization() {
+    HAL.initialize(500, 0);
+
+    try (DigitalOutput output = new DigitalOutput(0)) {
+      DIOSim sim = new DIOSim(output);
+      assertEquals(0, sim.getPulseLength());
+
+      output.enablePWM(0.5);
+      output.updateDutyCycle(0.6);
+    }
+  }
+}

wpilibj/src/test/java/org/wpilib/hardware/led/AddressableLEDBufferTest.java

@@ -0,0 +1,132 @@
+// 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;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.fail;
+import static org.junit.jupiter.params.provider.Arguments.arguments;
+
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+import java.util.stream.Stream;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+
+class AddressableLEDBufferTest {
+  @ParameterizedTest
+  @MethodSource("hsvToRgbProvider")
+  void hsvConvertTest(int h, int s, int v, int r, int g, int b) {
+    var buffer = new AddressableLEDBuffer(1);
+    buffer.setHSV(0, h, s, v);
+    assertAll(
+        () -> assertEquals((byte) r, buffer.m_buffer[0], "R value didn't match"),
+        () -> assertEquals((byte) g, buffer.m_buffer[1], "G value didn't match"),
+        () -> assertEquals((byte) b, buffer.m_buffer[2], "B value didn't match"));
+  }
+
+  static Stream<Arguments> hsvToRgbProvider() {
+    return Stream.of(
+        arguments(0, 0, 0, 0, 0, 0), // Black
+        arguments(0, 0, 255, 255, 255, 255), // White
+        arguments(0, 255, 255, 255, 0, 0), // Red
+        arguments(60, 255, 255, 0, 255, 0), // Lime
+        arguments(120, 255, 255, 0, 0, 255), // Blue
+        arguments(30, 255, 255, 255, 255, 0), // Yellow
+        arguments(90, 255, 255, 0, 255, 255), // Cyan
+        arguments(150, 255, 255, 255, 0, 255), // Magenta
+        arguments(0, 0, 191, 191, 191, 191), // Silver
+        arguments(0, 0, 128, 128, 128, 128), // Gray
+        arguments(0, 255, 128, 128, 0, 0), // Maroon
+        arguments(30, 255, 128, 128, 128, 0), // Olive
+        arguments(60, 255, 128, 0, 128, 0), // Green
+        arguments(150, 255, 128, 128, 0, 128), // Purple
+        arguments(90, 255, 128, 0, 128, 128), // Teal
+        arguments(120, 255, 128, 0, 0, 128) // Navy
+        );
+  }
+
+  @Test
+  void getColorTest() {
+    AddressableLEDBuffer buffer = new AddressableLEDBuffer(4);
+    final Color8Bit denimColor8Bit = new Color8Bit(Color.kDenim);
+    final Color8Bit firstBlueColor8Bit = new Color8Bit(Color.kFirstBlue);
+    final Color8Bit firstRedColor8Bit = new Color8Bit(Color.kFirstRed);
+
+    buffer.setLED(0, Color.kFirstBlue);
+    buffer.setLED(1, denimColor8Bit);
+    buffer.setLED(2, Color.kFirstRed);
+    buffer.setLED(3, Color.kFirstBlue);
+
+    assertEquals(Color.kFirstBlue, buffer.getLED(0));
+    assertEquals(Color.kDenim, buffer.getLED(1));
+    assertEquals(Color.kFirstRed, buffer.getLED(2));
+    assertEquals(Color.kFirstBlue, buffer.getLED(3));
+    assertEquals(firstBlueColor8Bit, buffer.getLED8Bit(0));
+    assertEquals(denimColor8Bit, buffer.getLED8Bit(1));
+    assertEquals(firstRedColor8Bit, buffer.getLED8Bit(2));
+    assertEquals(firstBlueColor8Bit, buffer.getLED8Bit(3));
+  }
+
+  @Test
+  void getRed() {
+    var buffer = new AddressableLEDBuffer(1);
+    buffer.setRGB(0, 127, 128, 129);
+    assertEquals(127, buffer.getRed(0));
+  }
+
+  @Test
+  void getGreen() {
+    var buffer = new AddressableLEDBuffer(1);
+    buffer.setRGB(0, 127, 128, 129);
+    assertEquals(128, buffer.getGreen(0));
+  }
+
+  @Test
+  void getBlue() {
+    var buffer = new AddressableLEDBuffer(1);
+    buffer.setRGB(0, 127, 128, 129);
+    assertEquals(129, buffer.getBlue(0));
+  }
+
+  @Test
+  void forEach() {
+    var buffer = new AddressableLEDBuffer(3);
+    buffer.setRGB(0, 1, 2, 3);
+    buffer.setRGB(1, 4, 5, 6);
+    buffer.setRGB(2, 7, 8, 9);
+
+    buffer.forEach(
+        (index, r, g, b) -> {
+          switch (index) {
+            case 0 ->
+                assertAll(
+                    () -> assertEquals(1, r, "red at index 0"),
+                    () -> assertEquals(2, g, "green at index 0"),
+                    () -> assertEquals(3, b, "blue at index 0"));
+            case 1 ->
+                assertAll(
+                    () -> assertEquals(4, r, "red at index 1"),
+                    () -> assertEquals(5, g, "green at index 1"),
+                    () -> assertEquals(6, b, "blue at index 1"));
+            case 2 ->
+                assertAll(
+                    () -> assertEquals(7, r, "red at index 2"),
+                    () -> assertEquals(8, g, "green at index 2"),
+                    () -> assertEquals(9, b, "blue at index 2"));
+            default -> fail("Unexpected index " + index);
+          }
+        });
+  }
+
+  @Test
+  void forEachOnEmptyBuffer() {
+    var buffer = new AddressableLEDBuffer(0);
+
+    buffer.forEach((i, r, g, b) -> fail("Iterator should not be called on an empty buffer"));
+  }
+}

wpilibj/src/test/java/org/wpilib/hardware/led/AddressableLEDBufferViewTest.java

@@ -0,0 +1,69 @@
+// 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;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.wpilibj.util.Color;
+import org.junit.jupiter.api.Test;
+
+class AddressableLEDBufferViewTest {
+  @Test
+  void singleLED() {
+    var buffer = new AddressableLEDBuffer(10);
+    var view = new AddressableLEDBufferView(buffer, 5, 5);
+    var color = Color.kAqua;
+    view.setLED(0, color);
+    assertEquals(color, buffer.getLED(5));
+    assertEquals(color, view.getLED(0));
+  }
+
+  @Test
+  void segment() {
+    var buffer = new AddressableLEDBuffer(10);
+    var view = new AddressableLEDBufferView(buffer, 2, 8);
+    view.setLED(0, Color.kAqua);
+    assertEquals(Color.kAqua, buffer.getLED(2));
+
+    view.setLED(6, Color.kAzure);
+    assertEquals(Color.kAzure, buffer.getLED(8));
+  }
+
+  @Test
+  void manualReversed() {
+    var buffer = new AddressableLEDBuffer(10);
+    var view = new AddressableLEDBufferView(buffer, 8, 2);
+
+    // LED 0 in the view should write to LED 8 on the real buffer
+    view.setLED(0, Color.kAqua);
+    assertEquals(Color.kAqua, buffer.getLED(8));
+
+    // .. and LED 6 in the view should write to LED 2 on the real buffer
+    view.setLED(6, Color.kAzure);
+    assertEquals(Color.kAzure, buffer.getLED(2));
+  }
+
+  @Test
+  void fullManualReversed() {
+    var buffer = new AddressableLEDBuffer(10);
+    var view = new AddressableLEDBufferView(buffer, 9, 0);
+    view.setLED(0, Color.kWhite);
+    assertEquals(Color.kWhite, buffer.getLED(9));
+
+    buffer.setLED(8, Color.kRed);
+    assertEquals(Color.kRed, view.getLED(1));
+  }
+
+  @Test
+  void reversed() {
+    var buffer = new AddressableLEDBuffer(10);
+    var view = new AddressableLEDBufferView(buffer, 0, 9).reversed();
+    view.setLED(0, Color.kWhite);
+    assertEquals(Color.kWhite, buffer.getLED(9));
+
+    view.setLED(9, Color.kRed);
+    assertEquals(Color.kRed, buffer.getLED(0));
+  }
+}

wpilibj/src/test/java/org/wpilib/hardware/led/LEDPatternTest.java

@@ -0,0 +1,997 @@
+// 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;
+
+import static edu.wpi.first.units.Units.Centimeters;
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.Microsecond;
+import static edu.wpi.first.units.Units.Microseconds;
+import static edu.wpi.first.units.Units.Percent;
+import static edu.wpi.first.units.Units.Seconds;
+import static edu.wpi.first.units.Units.Value;
+import static edu.wpi.first.wpilibj.LEDPattern.GradientType.kContinuous;
+import static edu.wpi.first.wpilibj.LEDPattern.GradientType.kDiscontinuous;
+import static edu.wpi.first.wpilibj.util.Color.kBlack;
+import static edu.wpi.first.wpilibj.util.Color.kBlue;
+import static edu.wpi.first.wpilibj.util.Color.kGreen;
+import static edu.wpi.first.wpilibj.util.Color.kLime;
+import static edu.wpi.first.wpilibj.util.Color.kMagenta;
+import static edu.wpi.first.wpilibj.util.Color.kMidnightBlue;
+import static edu.wpi.first.wpilibj.util.Color.kPurple;
+import static edu.wpi.first.wpilibj.util.Color.kRed;
+import static edu.wpi.first.wpilibj.util.Color.kWhite;
+import static edu.wpi.first.wpilibj.util.Color.kYellow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.fail;
+
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+import java.util.Map;
+import java.util.concurrent.atomic.AtomicBoolean;
+import java.util.concurrent.atomic.AtomicReference;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+class LEDPatternTest {
+  long m_mockTime;
+
+  // Applies a pattern of White, Yellow, Purple to LED triplets
+  LEDPattern m_whiteYellowPurple =
+      (reader, writer) -> {
+        for (int led = 0; led < reader.getLength(); led++) {
+          switch (led % 3) {
+            case 0:
+              writer.setLED(led, kWhite);
+              break;
+            case 1:
+              writer.setLED(led, kYellow);
+              break;
+            case 2:
+              writer.setLED(led, kPurple);
+              break;
+            default:
+              fail("Bad test setup");
+              break;
+          }
+        }
+      };
+
+  @BeforeEach
+  void setUp() {
+    m_mockTime = 0;
+    RobotController.setTimeSource(() -> m_mockTime);
+  }
+
+  @AfterEach
+  void tearDown() {
+    RobotController.setTimeSource(RobotController::getFPGATime);
+  }
+
+  @Test
+  void solidColor() {
+    LEDPattern pattern = LEDPattern.solid(kYellow);
+    AddressableLEDBuffer buffer = new AddressableLEDBuffer(99);
+    pattern.applyTo(buffer);
+
+    for (int i = 0; i < buffer.getLength(); i++) {
+      assertEquals(kYellow, buffer.getLED(i));
+    }
+  }
+
+  @Test
+  void gradient0SetsToBlack() {
+    LEDPattern pattern = LEDPattern.gradient(kContinuous);
+    AddressableLEDBuffer buffer = new AddressableLEDBuffer(99);
+    for (int i = 0; i < buffer.getLength(); i++) {
+      buffer.setRGB(i, 127, 128, 129);
+    }
+
+    pattern.applyTo(buffer);
+
+    for (int i = 0; i < buffer.getLength(); i++) {
+      assertEquals(kBlack, buffer.getLED(i));
+    }
+  }
+
+  @Test
+  void gradient1SetsToSolid() {
+    LEDPattern pattern = LEDPattern.gradient(kContinuous, kYellow);
+
+    AddressableLEDBuffer buffer = new AddressableLEDBuffer(99);
+    pattern.applyTo(buffer);
+
+    for (int i = 0; i < buffer.getLength(); i++) {
+      assertEquals(kYellow, buffer.getLED(i));
+    }
+  }
+
+  @Test
+  void continuousGradient2Colors() {
+    LEDPattern pattern = LEDPattern.gradient(kContinuous, kYellow, kPurple);
+
+    AddressableLEDBuffer buffer = new AddressableLEDBuffer(99);
+    pattern.applyTo(buffer);
+
+    assertColorEquals(kYellow, buffer.getLED(0));
+    assertColorEquals(Color.lerpRGB(kYellow, kPurple, 25 / 49.0), buffer.getLED(25));
+    assertColorEquals(kPurple, buffer.getLED(49));
+    assertColorEquals(Color.lerpRGB(kYellow, kPurple, 25 / 49.0), buffer.getLED(73));
+    assertColorEquals(kYellow, buffer.getLED(98));
+  }
+
+  @Test
+  void discontinuousGradient2Colors() {
+    LEDPattern pattern = LEDPattern.gradient(kDiscontinuous, kYellow, kPurple);
+
+    AddressableLEDBuffer buffer = new AddressableLEDBuffer(99);
+    pattern.applyTo(buffer);
+
+    assertColorEquals(kYellow, buffer.getLED(0));
+    assertColorEquals(Color.lerpRGB(kYellow, kPurple, 0.5), buffer.getLED(49));
+    assertColorEquals(kPurple, buffer.getLED(98));
+  }
+
+  @Test
+  void gradient3Colors() {
+    LEDPattern pattern = LEDPattern.gradient(kContinuous, kYellow, kPurple, kWhite);
+    AddressableLEDBuffer buffer = new AddressableLEDBuffer(99);
+    pattern.applyTo(buffer);
+
+    assertColorEquals(kYellow, buffer.getLED(0));
+    assertColorEquals(Color.lerpRGB(kYellow, kPurple, 25.0 / 33.0), buffer.getLED(25));
+    assertColorEquals(kPurple, buffer.getLED(33));
+    assertColorEquals(Color.lerpRGB(kPurple, kWhite, 25.0 / 33.0), buffer.getLED(58));
+    assertColorEquals(kWhite, buffer.getLED(66));
+    assertColorEquals(Color.lerpRGB(kWhite, kYellow, 25.0 / 33.0), buffer.getLED(91));
+    assertColorEquals(Color.lerpRGB(kWhite, kYellow, 32.0 / 33.0), buffer.getLED(98));
+  }
+
+  @Test
+  void discontinuousGradient3Colors() {
+    LEDPattern pattern = LEDPattern.gradient(kDiscontinuous, kYellow, kPurple, kWhite);
+    AddressableLEDBuffer buffer = new AddressableLEDBuffer(101);
+    pattern.applyTo(buffer);
+
+    assertColorEquals(kYellow, buffer.getLED(0));
+    assertColorEquals(Color.lerpRGB(kYellow, kPurple, 0.5), buffer.getLED(25));
+    assertColorEquals(kPurple, buffer.getLED(50));
+    assertColorEquals(Color.lerpRGB(kPurple, kWhite, 0.5), buffer.getLED(75));
+    assertColorEquals(kWhite, buffer.getLED(100));
+  }
+
+  @Test
+  void step0SetsToBlack() {
+    LEDPattern pattern = LEDPattern.steps(Map.of());
+
+    AddressableLEDBuffer buffer = new AddressableLEDBuffer(99);
+    for (int i = 0; i < buffer.getLength(); i++) {
+      buffer.setRGB(i, 127, 128, 129);
+    }
+
+    pattern.applyTo(buffer);
+    for (int i = 0; i < 99; i++) {
+      assertColorEquals(kBlack, buffer.getLED(i));
+    }
+  }
+
+  @Test
+  void step1SetsToSolid() {
+    LEDPattern pattern = LEDPattern.steps(Map.of(0.0, kYellow));
+
+    AddressableLEDBuffer buffer = new AddressableLEDBuffer(99);
+
+    pattern.applyTo(buffer);
+    for (int i = 0; i < 99; i++) {
+      assertColorEquals(kYellow, buffer.getLED(i));
+    }
+  }
+
+  @Test
+  void step1HalfSetsToHalfOffHalfColor() {
+    LEDPattern pattern = LEDPattern.steps(Map.of(0.50, kYellow));
+
+    AddressableLEDBuffer buffer = new AddressableLEDBuffer(99);
+    pattern.applyTo(buffer);
+
+    // [0, 48] should be black...
+    for (int i = 0; i < 49; i++) {
+      assertColorEquals(kBlack, buffer.getLED(i));
+    }
+    // ... and [49, <end>] should be the color that was set
+    for (int i = 49; i < buffer.getLength(); i++) {
+      assertColorEquals(kYellow, buffer.getLED(i));
+    }
+  }
+
+  @Test
+  void scrollForward() {
+    var buffer = new AddressableLEDBuffer(256);
+
+    LEDPattern base =
+        (reader, writer) -> {
+          for (int led = 0; led < reader.getLength(); led++) {
+            writer.setRGB(led, led % 256, led % 256, led % 256);
+          }
+        };
+
+    // scroll forwards 1/256th (1 LED) per microsecond - this makes mock time easier
+    var scroll = base.scrollAtRelativeSpeed(Value.per(Microsecond).of(1 / 256.0));
+
+    for (int time = 0; time < 500; time++) {
+      m_mockTime = time;
+      scroll.applyTo(buffer);
+
+      for (int led = 0; led < buffer.getLength(); led++) {
+        // Base: [(0, 0, 0) (1, 1, 1) (2, 2, 2) (3, 3, 3) (4, 4, 4) ... (255, 255, 255)]
+        // Value for every channel should DECREASE by 1 in each timestep, wrapping around 0 and 255
+
+        // t=0,   channel value = (0, 1, 2, ..., 254, 255)
+        // t=1,   channel value = (255, 0, 1, ..., 253, 254)
+        // t=2,   channel value = (254, 255, 0, ..., 252, 253)
+        // t=255, channel value = (1, 2, 3, ..., 255, 0)
+        // t=256, channel value = (0, 1, 2, ..., 254, 255)
+        int ch = Math.floorMod(led - time, 256);
+
+        assertEquals(new Color8Bit(ch, ch, ch), buffer.getLED8Bit(led));
+      }
+    }
+  }
+
+  @Test
+  void scrollBackward() {
+    var buffer = new AddressableLEDBuffer(256);
+
+    LEDPattern base =
+        (reader, writer) -> {
+          for (int led = 0; led < reader.getLength(); led++) {
+            writer.setRGB(led, led % 256, led % 256, led % 256);
+          }
+        };
+
+    // scroll backwards 1/256th (1 LED) per microsecond - this makes mock time easier
+    var scroll = base.scrollAtRelativeSpeed(Value.per(Microsecond).of(-1 / 256.0));
+
+    for (int time = 0; time < 500; time++) {
+      m_mockTime = time;
+      scroll.applyTo(buffer);
+
+      for (int led = 0; led < buffer.getLength(); led++) {
+        // Base: [(0, 0, 0) (1, 1, 1) (2, 2, 2) (3, 3, 3) (4, 4, 4) ... (255, 255, 255)]
+        // Value for every channel should INCREASE by 1 in each timestep, wrapping around 0 and 255
+
+        // t=0,   channel value = (0, 1, 2, ..., 254, 255)
+        // t=1,   channel value = (1, 2, 3, ..., 255, 0)
+        // t=2,   channel value = (2, 3, 4, ..., 0, 1)
+        // t=255, channel value = (255, 0, 1, ..., 253, 254)
+        // t=256, channel value = (0, 1, 2, ..., 254, 255)
+        int ch = Math.floorMod(led + time, 256);
+
+        assertEquals(new Color8Bit(ch, ch, ch), buffer.getLED8Bit(led));
+      }
+    }
+  }
+
+  @Test
+  void scrollAbsoluteSpeedForward() {
+    var buffer = new AddressableLEDBuffer(256);
+
+    LEDPattern base =
+        (reader, writer) -> {
+          for (int led = 0; led < reader.getLength(); led++) {
+            writer.setRGB(led, led % 256, led % 256, led % 256);
+          }
+        };
+
+    // scroll at 16 m/s, LED spacing = 2cm
+    // buffer is 256 LEDs, so total length = 512cm = 5.12m
+    // scrolling at 16 m/s yields a period of 0.32 seconds, or 0.00125 seconds per LED (800 LEDs/s)
+    var scroll = base.scrollAtAbsoluteSpeed(MetersPerSecond.of(16), Centimeters.of(2));
+
+    for (int time = 0; time < 500; time++) {
+      m_mockTime = time * 1_250; // 1.25ms per LED
+      scroll.applyTo(buffer);
+
+      for (int led = 0; led < buffer.getLength(); led++) {
+        // Base: [(0, 0, 0) (1, 1, 1) (2, 2, 2) (3, 3, 3) (4, 4, 4) ... (255, 255, 255)]
+        // Value for every channel should DECREASE by 1 in each timestep, wrapping around 0 and 255
+
+        // t=0,   channel value = (0, 1, 2, ..., 254, 255)
+        // t=1,   channel value = (255, 0, 1, ..., 253, 254)
+        // t=2,   channel value = (254, 255, 0, ..., 252, 253)
+        // t=255, channel value = (1, 2, 3, ..., 255, 0)
+        // t=256, channel value = (0, 1, 2, ..., 254, 255)
+        int ch = Math.floorMod(led - time, 256);
+
+        assertEquals(new Color8Bit(ch, ch, ch), buffer.getLED8Bit(led));
+      }
+    }
+  }
+
+  @Test
+  void scrollAbsoluteSpeedBackward() {
+    var buffer = new AddressableLEDBuffer(256);
+
+    LEDPattern base =
+        (reader, writer) -> {
+          for (int led = 0; led < reader.getLength(); led++) {
+            writer.setRGB(led, led % 256, led % 256, led % 256);
+          }
+        };
+
+    // scroll at 16 m/s, LED spacing = 2cm
+    // buffer is 256 LEDs, so total length = 512cm = 5.12m
+    // scrolling at 16 m/s yields a period of 0.32 seconds, or 0.00125 seconds per LED (800 LEDs/s)
+    var scroll = base.scrollAtAbsoluteSpeed(MetersPerSecond.of(-16), Centimeters.of(2));
+
+    for (int time = 0; time < 500; time++) {
+      m_mockTime = time * 1_250; // 1.25ms per LED
+      scroll.applyTo(buffer);
+
+      for (int led = 0; led < buffer.getLength(); led++) {
+        // Base: [(0, 0, 0) (1, 1, 1) (2, 2, 2) (3, 3, 3) (4, 4, 4) ... (255, 255, 255)]
+        // Value for every channel should DECREASE by 1 in each timestep, wrapping around 0 and 255
+
+        // t=0,   channel value = (0, 1, 2, ..., 254, 255)
+        // t=1,   channel value = (255, 0, 1, ..., 253, 254)
+        // t=2,   channel value = (254, 255, 0, ..., 252, 253)
+        // t=255, channel value = (1, 2, 3, ..., 255, 0)
+        // t=256, channel value = (0, 1, 2, ..., 254, 255)
+        int ch = Math.floorMod(led + time, 256);
+
+        assertEquals(new Color8Bit(ch, ch, ch), buffer.getLED8Bit(led));
+      }
+    }
+  }
+
+  @Test
+  void rainbowAtFullSize() {
+    var buffer = new AddressableLEDBuffer(180);
+
+    int saturation = 255;
+    int value = 255;
+    var pattern = LEDPattern.rainbow(saturation, value);
+
+    pattern.applyTo(buffer);
+
+    for (int led = 0; led < buffer.getLength(); led++) {
+      assertColorEquals(Color.fromHSV(led, saturation, value), buffer.getLED(led));
+    }
+  }
+
+  @Test
+  void rainbowAtHalfSize() {
+    var buffer = new AddressableLEDBuffer(90);
+
+    int saturation = 42;
+    int value = 87;
+    var pattern = LEDPattern.rainbow(saturation, value);
+
+    pattern.applyTo(buffer);
+
+    for (int led = 0; led < buffer.getLength(); led++) {
+      assertColorEquals(Color.fromHSV(led * 2, saturation, value), buffer.getLED(led));
+    }
+  }
+
+  @Test
+  void rainbowAtOneThirdSize() {
+    var buffer = new AddressableLEDBuffer(60);
+
+    int saturation = 191;
+    int value = 255;
+    var pattern = LEDPattern.rainbow(saturation, value);
+
+    pattern.applyTo(buffer);
+
+    for (int led = 0; led < buffer.getLength(); led++) {
+      assertColorEquals(Color.fromHSV(led * 3, saturation, value), buffer.getLED(led));
+    }
+  }
+
+  @Test
+  void rainbowAtDoubleSize() {
+    var buffer = new AddressableLEDBuffer(360);
+
+    int saturation = 212;
+    int value = 93;
+    var pattern = LEDPattern.rainbow(saturation, value);
+
+    pattern.applyTo(buffer);
+
+    for (int led = 0; led < buffer.getLength(); led++) {
+      assertColorEquals(Color.fromHSV(led / 2, saturation, value), buffer.getLED(led));
+    }
+  }
+
+  @Test
+  void rainbowOddSize() {
+    var buffer = new AddressableLEDBuffer(127);
+    double scale = 180.0 / buffer.getLength();
+
+    int saturation = 73;
+    int value = 128;
+    var pattern = LEDPattern.rainbow(saturation, value);
+
+    pattern.applyTo(buffer);
+
+    for (int led = 0; led < buffer.getLength(); led++) {
+      assertColorEquals(Color.fromHSV((int) (led * scale), saturation, value), buffer.getLED(led));
+    }
+  }
+
+  @Test
+  void reverseSolid() {
+    var buffer = new AddressableLEDBuffer(90);
+
+    var pattern = LEDPattern.solid(Color.kRosyBrown).reversed();
+    pattern.applyTo(buffer);
+
+    for (int led = 0; led < buffer.getLength(); led++) {
+      assertColorEquals(Color.kRosyBrown, buffer.getLED(led));
+    }
+  }
+
+  @Test
+  void reverseSteps() {
+    var buffer = new AddressableLEDBuffer(100);
+
+    var pattern = LEDPattern.steps(Map.of(0, kWhite, 0.5, kYellow)).reversed();
+    pattern.applyTo(buffer);
+
+    // colors should be swapped; yellow first, then white
+    for (int led = 0; led < buffer.getLength(); led++) {
+      if (led < 50) {
+        assertColorEquals(kYellow, buffer.getLED(led));
+      } else {
+        assertColorEquals(kWhite, buffer.getLED(led));
+      }
+    }
+  }
+
+  @Test
+  void offsetPositive() {
+    var buffer = new AddressableLEDBuffer(21);
+
+    // offset repeats PWY
+    var offset = m_whiteYellowPurple.offsetBy(1);
+    offset.applyTo(buffer);
+
+    for (int led = 0; led < buffer.getLength(); led++) {
+      Color color = buffer.getLED(led);
+      switch (led % 3) {
+        case 0:
+          assertColorEquals(kPurple, color);
+          break;
+        case 1:
+          assertColorEquals(kWhite, color);
+          break;
+        case 2:
+          assertColorEquals(kYellow, color);
+          break;
+        default:
+          fail("Bad test setup");
+          break;
+      }
+    }
+  }
+
+  @Test
+  void offsetNegative() {
+    var buffer = new AddressableLEDBuffer(21);
+
+    // offset repeats YPW
+    var offset = m_whiteYellowPurple.offsetBy(-1);
+    offset.applyTo(buffer);
+
+    for (int led = 0; led < buffer.getLength(); led++) {
+      Color color = buffer.getLED(led);
+      switch (led % 3) {
+        case 0:
+          assertColorEquals(kYellow, color);
+          break;
+        case 1:
+          assertColorEquals(kPurple, color);
+          break;
+        case 2:
+          assertColorEquals(kWhite, color);
+          break;
+        default:
+          fail("Bad test setup");
+          break;
+      }
+    }
+  }
+
+  @Test
+  void offsetZero() {
+    var buffer = new AddressableLEDBuffer(21);
+
+    // offset copies the base pattern, WYP
+    var offset = m_whiteYellowPurple.offsetBy(0);
+    offset.applyTo(buffer);
+
+    for (int led = 0; led < buffer.getLength(); led++) {
+      Color color = buffer.getLED(led);
+      switch (led % 3) {
+        case 0:
+          assertColorEquals(kWhite, color);
+          break;
+        case 1:
+          assertColorEquals(kYellow, color);
+          break;
+        case 2:
+          assertColorEquals(kPurple, color);
+          break;
+        default:
+          fail("Bad test setup");
+          break;
+      }
+    }
+  }
+
+  @Test
+  void blinkSymmetric() {
+    // on for 2 seconds, off for 2 seconds
+    var pattern = LEDPattern.solid(kWhite).blink(Seconds.of(2));
+
+    var buffer = new AddressableLEDBuffer(1);
+
+    for (int t = 0; t < 8; t++) {
+      m_mockTime = t * 1_000_000L; // time travel 1 second
+      pattern.applyTo(buffer);
+
+      Color color = buffer.getLED(0);
+      switch (t) {
+        case 0:
+        case 1:
+        case 4:
+        case 5:
+          assertColorEquals(kWhite, color);
+          break;
+        case 2:
+        case 3:
+        case 6:
+        case 7:
+          assertColorEquals(kBlack, color);
+          break;
+        default:
+          fail("Bad test setup");
+          break;
+      }
+    }
+  }
+
+  @Test
+  void blinkAsymmetric() {
+    // on for 3 seconds, off for 1 second
+    var pattern = LEDPattern.solid(kWhite).blink(Seconds.of(3), Seconds.of(1));
+
+    var buffer = new AddressableLEDBuffer(1);
+
+    for (int t = 0; t < 8; t++) {
+      m_mockTime = t * 1_000_000L; // time travel 1 second
+      pattern.applyTo(buffer);
+
+      Color color = buffer.getLED(0);
+      switch (t) {
+        case 0:
+        case 1:
+        case 2: // first period
+        case 4:
+        case 5:
+        case 6: // second period
+          assertColorEquals(kWhite, color);
+          break;
+        case 3:
+        case 7:
+          assertColorEquals(kBlack, color);
+          break;
+        default:
+          fail("Bad test setup");
+          break;
+      }
+    }
+  }
+
+  @Test
+  void blinkInSync() {
+    AtomicBoolean condition = new AtomicBoolean(false);
+    var pattern = LEDPattern.solid(kWhite).synchronizedBlink(condition::get);
+
+    var buffer = new AddressableLEDBuffer(1);
+
+    pattern.applyTo(buffer);
+    assertColorEquals(kBlack, buffer.getLED(0));
+
+    condition.set(true);
+    pattern.applyTo(buffer);
+    assertColorEquals(kWhite, buffer.getLED(0));
+
+    condition.set(false);
+    pattern.applyTo(buffer);
+    assertColorEquals(kBlack, buffer.getLED(0));
+  }
+
+  @Test
+  void breathe() {
+    final Color midGray = new Color(0.5, 0.5, 0.5);
+
+    var pattern = LEDPattern.solid(kWhite).breathe(Microseconds.of(4));
+
+    var buffer = new AddressableLEDBuffer(1);
+
+    {
+      m_mockTime = 0; // start
+      pattern.applyTo(buffer);
+      assertColorEquals(kWhite, buffer.getLED(0));
+    }
+
+    {
+      m_mockTime = 1; // midway (down)
+      pattern.applyTo(buffer);
+      assertColorEquals(midGray, buffer.getLED(0));
+    }
+
+    {
+      m_mockTime = 2; // bottom
+      pattern.applyTo(buffer);
+      assertColorEquals(kBlack, buffer.getLED(0));
+    }
+
+    {
+      m_mockTime = 3; // midway (up)
+      pattern.applyTo(buffer);
+      assertColorEquals(midGray, buffer.getLED(0));
+    }
+
+    {
+      m_mockTime = 4; // back to start
+      pattern.applyTo(buffer);
+      assertColorEquals(kWhite, buffer.getLED(0));
+    }
+  }
+
+  @Test
+  void overlaySolidOnSolid() {
+    var overlay = LEDPattern.solid(kYellow).overlayOn(LEDPattern.solid(kWhite));
+
+    var buffer = new AddressableLEDBuffer(1);
+    overlay.applyTo(buffer);
+
+    assertColorEquals(kYellow, buffer.getLED(0));
+  }
+
+  @Test
+  void overlayNearlyBlack() {
+    Color overlayColor = new Color(new Color8Bit(1, 0, 0));
+    var overlay = LEDPattern.solid(overlayColor).overlayOn(LEDPattern.solid(kWhite));
+
+    var buffer = new AddressableLEDBuffer(1);
+    overlay.applyTo(buffer);
+
+    assertColorEquals(overlayColor, buffer.getLED(0));
+  }
+
+  @Test
+  void overlayMixed() {
+    var overlay =
+        LEDPattern.steps(Map.of(0, kYellow, 0.5, kBlack)).overlayOn(LEDPattern.solid(kWhite));
+
+    var buffer = new AddressableLEDBuffer(2);
+    overlay.applyTo(buffer);
+
+    assertColorEquals(kYellow, buffer.getLED(0));
+    assertColorEquals(kWhite, buffer.getLED(1));
+  }
+
+  @Test
+  void blend() {
+    var pattern1 = LEDPattern.solid(kBlue);
+    var pattern2 = LEDPattern.solid(kRed);
+    var blend = pattern1.blend(pattern2);
+
+    var buffer = new AddressableLEDBuffer(1);
+    blend.applyTo(buffer);
+
+    // Individual RGB channels are averaged; #0000FF blended with #FF0000 yields #7F007F
+    assertColorEquals(new Color(127, 0, 127), buffer.getLED(0));
+  }
+
+  @Test
+  void binaryMask() {
+    Color color = new Color(123, 123, 123);
+    var base = LEDPattern.solid(color);
+    // first 50% mask on, last 50% mask off
+    var mask = LEDPattern.steps(Map.of(0, kWhite, 0.5, kBlack));
+    var masked = base.mask(mask);
+
+    var buffer = new AddressableLEDBuffer(10);
+    masked.applyTo(buffer);
+
+    for (int i = 0; i < 5; i++) {
+      assertColorEquals(color, buffer.getLED(i));
+    }
+
+    for (int i = 5; i < 10; i++) {
+      assertColorEquals(kBlack, buffer.getLED(i));
+    }
+  }
+
+  @Test
+  void channelwiseMask() {
+    Color baseColor = new Color(123, 123, 123);
+    Color halfGray = new Color(0.5, 0.5, 0.5);
+    var base = LEDPattern.solid(baseColor);
+
+    var mask =
+        LEDPattern.steps(Map.of(0, kRed, 0.2, kLime, 0.4, kBlue, 0.6, halfGray, 0.8, kWhite));
+
+    var masked = base.mask(mask);
+
+    var buffer = new AddressableLEDBuffer(5);
+    masked.applyTo(buffer);
+
+    assertColorEquals(new Color(123, 0, 0), buffer.getLED(0)); // red channel only
+    assertColorEquals(new Color(0, 123, 0), buffer.getLED(1)); // green channel only
+    assertColorEquals(new Color(0, 0, 123), buffer.getLED(2)); // blue channel only
+
+    // mask channels are all 0b00111111, base is 0b00111011,
+    // so the AND should give us the unmodified base color
+    assertColorEquals(baseColor, buffer.getLED(3));
+    assertColorEquals(baseColor, buffer.getLED(4)); // full color allowed
+  }
+
+  @Test
+  void progressMaskLayer() {
+    var progress = new AtomicReference<>(0.0);
+    var maskLayer = LEDPattern.progressMaskLayer(progress::get);
+    var buffer = new AddressableLEDBuffer(100);
+
+    for (double t = 0; t <= 1.0; t += 0.01) {
+      progress.set(t);
+      maskLayer.applyTo(buffer);
+
+      int lastMaskedLED = (int) (t * 100);
+      for (int i = 0; i < lastMaskedLED; i++) {
+        assertColorEquals(
+            kWhite,
+            buffer.getLED(i),
+            "Progress " + lastMaskedLED + "%, LED " + i + " should be WHITE");
+      }
+
+      for (int i = lastMaskedLED; i < 100; i++) {
+        assertColorEquals(
+            kBlack,
+            buffer.getLED(i),
+            "Progress " + lastMaskedLED + "% , LED " + i + " should be BLACK");
+      }
+    }
+  }
+
+  @Test
+  void zeroBrightness() {
+    var pattern = LEDPattern.solid(kRed).atBrightness(Percent.of(0));
+    var buffer = new AddressableLEDBuffer(1);
+    pattern.applyTo(buffer);
+
+    assertColorEquals(kBlack, buffer.getLED(0));
+  }
+
+  @Test
+  void sameBrightness() {
+    var pattern = LEDPattern.solid(kMagenta).atBrightness(Percent.of(100));
+    var buffer = new AddressableLEDBuffer(1);
+    pattern.applyTo(buffer);
+
+    assertColorEquals(kMagenta, buffer.getLED(0));
+  }
+
+  @Test
+  void higherBrightness() {
+    var pattern = LEDPattern.solid(kMagenta).atBrightness(Value.of(4 / 3.0));
+    var buffer = new AddressableLEDBuffer(1);
+    pattern.applyTo(buffer);
+
+    assertColorEquals(kMagenta, buffer.getLED(0));
+  }
+
+  @Test
+  void negativeBrightness() {
+    var pattern = LEDPattern.solid(kWhite).atBrightness(Percent.of(-1000));
+    var buffer = new AddressableLEDBuffer(1);
+    pattern.applyTo(buffer);
+
+    assertColorEquals(kBlack, buffer.getLED(0));
+  }
+
+  @Test
+  void clippingBrightness() {
+    var pattern = LEDPattern.solid(kMidnightBlue).atBrightness(Percent.of(10000));
+    var buffer = new AddressableLEDBuffer(1);
+    pattern.applyTo(buffer);
+
+    assertColorEquals(kWhite, buffer.getLED(0));
+  }
+
+  @Test
+  void reverseMask() {
+    var pattern =
+        LEDPattern.steps(Map.of(0, kRed, 0.25, kBlue, 0.5, kYellow, 0.75, kGreen))
+            .mask(LEDPattern.steps(Map.of(0, kWhite, 0.5, kBlack)))
+            .reversed();
+    var buffer = new AddressableLEDBuffer(8);
+
+    pattern.applyTo(buffer);
+
+    assertColorEquals(kRed, buffer.getLED(7));
+    assertColorEquals(kRed, buffer.getLED(6));
+    assertColorEquals(kBlue, buffer.getLED(5));
+    assertColorEquals(kBlue, buffer.getLED(4));
+    assertColorEquals(kBlack, buffer.getLED(3));
+    assertColorEquals(kBlack, buffer.getLED(2));
+    assertColorEquals(kBlack, buffer.getLED(1));
+    assertColorEquals(kBlack, buffer.getLED(0));
+  }
+
+  @Test
+  void offsetMask() {
+    var pattern =
+        LEDPattern.steps(Map.of(0, kRed, 0.25, kBlue, 0.5, kYellow, 0.75, kGreen))
+            .mask(LEDPattern.steps(Map.of(0, kWhite, 0.5, kBlack)))
+            .offsetBy(4);
+    var buffer = new AddressableLEDBuffer(8);
+
+    pattern.applyTo(buffer);
+
+    assertColorEquals(kBlack, buffer.getLED(0));
+    assertColorEquals(kBlack, buffer.getLED(1));
+    assertColorEquals(kBlack, buffer.getLED(2));
+    assertColorEquals(kBlack, buffer.getLED(3));
+    assertColorEquals(kRed, buffer.getLED(4));
+    assertColorEquals(kRed, buffer.getLED(5));
+    assertColorEquals(kBlue, buffer.getLED(6));
+    assertColorEquals(kBlue, buffer.getLED(7));
+  }
+
+  @Test
+  void relativeScrollingMask() {
+    // [red, red, blue, blue, yellow, yellow, green, green]
+    // under a mask of first 50% on, last 50% off
+    // [red, red, blue, blue, black, black, black, black]
+    // all scrolling at 1 LED per microsecond
+    var pattern =
+        LEDPattern.steps(Map.of(0, kRed, 0.25, kBlue, 0.5, kYellow, 0.75, kGreen))
+            .mask(LEDPattern.steps(Map.of(0, kWhite, 0.5, kBlack)))
+            .scrollAtRelativeSpeed(Percent.per(Microsecond).of(12.5));
+    var buffer = new AddressableLEDBuffer(8);
+
+    {
+      m_mockTime = 0; // start
+      pattern.applyTo(buffer);
+      assertColorEquals(kRed, buffer.getLED(0));
+      assertColorEquals(kRed, buffer.getLED(1));
+      assertColorEquals(kBlue, buffer.getLED(2));
+      assertColorEquals(kBlue, buffer.getLED(3));
+      assertColorEquals(kBlack, buffer.getLED(4));
+      assertColorEquals(kBlack, buffer.getLED(5));
+      assertColorEquals(kBlack, buffer.getLED(6));
+      assertColorEquals(kBlack, buffer.getLED(7));
+    }
+
+    {
+      m_mockTime = 1;
+      pattern.applyTo(buffer);
+      assertColorEquals(kBlack, buffer.getLED(0));
+      assertColorEquals(kRed, buffer.getLED(1));
+      assertColorEquals(kRed, buffer.getLED(2));
+      assertColorEquals(kBlue, buffer.getLED(3));
+      assertColorEquals(kBlue, buffer.getLED(4));
+      assertColorEquals(kBlack, buffer.getLED(5));
+      assertColorEquals(kBlack, buffer.getLED(6));
+      assertColorEquals(kBlack, buffer.getLED(7));
+    }
+
+    {
+      m_mockTime = 2;
+      pattern.applyTo(buffer);
+      assertColorEquals(kBlack, buffer.getLED(0));
+      assertColorEquals(kBlack, buffer.getLED(1));
+      assertColorEquals(kRed, buffer.getLED(2));
+      assertColorEquals(kRed, buffer.getLED(3));
+      assertColorEquals(kBlue, buffer.getLED(4));
+      assertColorEquals(kBlue, buffer.getLED(5));
+      assertColorEquals(kBlack, buffer.getLED(6));
+      assertColorEquals(kBlack, buffer.getLED(7));
+    }
+
+    {
+      m_mockTime = 3;
+      pattern.applyTo(buffer);
+      assertColorEquals(kBlack, buffer.getLED(0));
+      assertColorEquals(kBlack, buffer.getLED(1));
+      assertColorEquals(kBlack, buffer.getLED(2));
+      assertColorEquals(kRed, buffer.getLED(3));
+      assertColorEquals(kRed, buffer.getLED(4));
+      assertColorEquals(kBlue, buffer.getLED(5));
+      assertColorEquals(kBlue, buffer.getLED(6));
+      assertColorEquals(kBlack, buffer.getLED(7));
+    }
+  }
+
+  @Test
+  void absoluteScrollingMask() {
+    // [red, red, blue, blue, yellow, yellow, green, green]
+    // under a mask of first 50% on, last 50% off
+    // [red, red, blue, blue, black, black, black, black]
+    // all scrolling at 1 LED per microsecond
+    var pattern =
+        LEDPattern.steps(Map.of(0, kRed, 0.25, kBlue, 0.5, kYellow, 0.75, kGreen))
+            .mask(LEDPattern.steps(Map.of(0, kWhite, 0.5, kBlack)))
+            .scrollAtAbsoluteSpeed(Meters.per(Microsecond).of(1), Meters.one());
+    var buffer = new AddressableLEDBuffer(8);
+
+    {
+      m_mockTime = 0; // start
+      pattern.applyTo(buffer);
+      assertColorEquals(kRed, buffer.getLED(0));
+      assertColorEquals(kRed, buffer.getLED(1));
+      assertColorEquals(kBlue, buffer.getLED(2));
+      assertColorEquals(kBlue, buffer.getLED(3));
+      assertColorEquals(kBlack, buffer.getLED(4));
+      assertColorEquals(kBlack, buffer.getLED(5));
+      assertColorEquals(kBlack, buffer.getLED(6));
+      assertColorEquals(kBlack, buffer.getLED(7));
+    }
+
+    {
+      m_mockTime = 1;
+      pattern.applyTo(buffer);
+      assertColorEquals(kBlack, buffer.getLED(0));
+      assertColorEquals(kRed, buffer.getLED(1));
+      assertColorEquals(kRed, buffer.getLED(2));
+      assertColorEquals(kBlue, buffer.getLED(3));
+      assertColorEquals(kBlue, buffer.getLED(4));
+      assertColorEquals(kBlack, buffer.getLED(5));
+      assertColorEquals(kBlack, buffer.getLED(6));
+      assertColorEquals(kBlack, buffer.getLED(7));
+    }
+
+    {
+      m_mockTime = 2;
+      pattern.applyTo(buffer);
+      assertColorEquals(kBlack, buffer.getLED(0));
+      assertColorEquals(kBlack, buffer.getLED(1));
+      assertColorEquals(kRed, buffer.getLED(2));
+      assertColorEquals(kRed, buffer.getLED(3));
+      assertColorEquals(kBlue, buffer.getLED(4));
+      assertColorEquals(kBlue, buffer.getLED(5));
+      assertColorEquals(kBlack, buffer.getLED(6));
+      assertColorEquals(kBlack, buffer.getLED(7));
+    }
+
+    {
+      m_mockTime = 3;
+      pattern.applyTo(buffer);
+      assertColorEquals(kBlack, buffer.getLED(0));
+      assertColorEquals(kBlack, buffer.getLED(1));
+      assertColorEquals(kBlack, buffer.getLED(2));
+      assertColorEquals(kRed, buffer.getLED(3));
+      assertColorEquals(kRed, buffer.getLED(4));
+      assertColorEquals(kBlue, buffer.getLED(5));
+      assertColorEquals(kBlue, buffer.getLED(6));
+      assertColorEquals(kBlack, buffer.getLED(7));
+    }
+  }
+
+  void assertColorEquals(Color expected, Color actual) {
+    assertEquals(new Color8Bit(expected), new Color8Bit(actual));
+  }
+
+  void assertColorEquals(Color expected, Color actual, String message) {
+    assertEquals(new Color8Bit(expected), new Color8Bit(actual), message);
+  }
+}

wpilibj/src/test/java/org/wpilib/hardware/motor/MockMotorController.java

@@ -0,0 +1,41 @@
+// 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.motorcontrol;
+
+@SuppressWarnings("removal")
+public class MockMotorController implements MotorController {
+  private double m_speed;
+  private boolean m_isInverted;
+
+  @Override
+  public void set(double speed) {
+    m_speed = m_isInverted ? -speed : speed;
+  }
+
+  @Override
+  public double get() {
+    return m_speed;
+  }
+
+  @Override
+  public void setInverted(boolean isInverted) {
+    m_isInverted = isInverted;
+  }
+
+  @Override
+  public boolean getInverted() {
+    return m_isInverted;
+  }
+
+  @Override
+  public void disable() {
+    m_speed = 0;
+  }
+
+  @Override
+  public void stopMotor() {
+    disable();
+  }
+}

wpilibj/src/test/java/org/wpilib/hardware/motor/MockPWMMotorController.java

@@ -0,0 +1,34 @@
+// 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.motorcontrol;
+
+public class MockPWMMotorController {
+  private double m_speed;
+  private boolean m_isInverted;
+
+  public void set(double speed) {
+    m_speed = m_isInverted ? -speed : speed;
+  }
+
+  public double get() {
+    return m_speed;
+  }
+
+  public void setInverted(boolean isInverted) {
+    m_isInverted = isInverted;
+  }
+
+  public boolean getInverted() {
+    return m_isInverted;
+  }
+
+  public void disable() {
+    m_speed = 0;
+  }
+
+  public void stopMotor() {
+    disable();
+  }
+}

wpilibj/src/test/java/org/wpilib/hardware/motor/MotorControllerGroupTest.java

@@ -0,0 +1,102 @@
+// 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.motorcontrol;
+
+import static org.junit.jupiter.api.Assertions.assertArrayEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import java.util.Arrays;
+import java.util.stream.DoubleStream;
+import java.util.stream.IntStream;
+import java.util.stream.Stream;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+
+@SuppressWarnings("removal")
+class MotorControllerGroupTest {
+  private static Stream<Arguments> motorControllerArguments() {
+    return IntStream.of(1, 2, 3)
+        .mapToObj(
+            number -> {
+              MotorController[] motorControllers =
+                  Stream.generate(MockMotorController::new)
+                      .limit(number)
+                      .toArray(MotorController[]::new);
+              MotorControllerGroup group =
+                  new MotorControllerGroup(
+                      motorControllers[0],
+                      Arrays.copyOfRange(motorControllers, 1, motorControllers.length));
+              return Arguments.of(group, motorControllers);
+            });
+  }
+
+  @ParameterizedTest
+  @MethodSource("motorControllerArguments")
+  void setTest(final MotorControllerGroup group, final MotorController[] motorControllers) {
+    group.set(1.0);
+
+    assertArrayEquals(
+        DoubleStream.generate(() -> 1.0).limit(motorControllers.length).toArray(),
+        Arrays.stream(motorControllers).mapToDouble(MotorController::get).toArray(),
+        0.00005);
+  }
+
+  @ParameterizedTest
+  @MethodSource("motorControllerArguments")
+  void getInvertedTest(final MotorControllerGroup group, final MotorController[] motorControllers) {
+    group.setInverted(true);
+
+    assertTrue(group.getInverted());
+  }
+
+  @ParameterizedTest
+  @MethodSource("motorControllerArguments")
+  void setInvertedDoesNotModifyMotorControllersTest(
+      final MotorControllerGroup group, final MotorController[] motorControllers) {
+    group.setInverted(true);
+
+    assertArrayEquals(
+        Stream.generate(() -> false).limit(motorControllers.length).toArray(),
+        Arrays.stream(motorControllers).map(MotorController::getInverted).toArray());
+  }
+
+  @ParameterizedTest
+  @MethodSource("motorControllerArguments")
+  void setInvertedDoesInvertTest(
+      final MotorControllerGroup group, final MotorController[] motorControllers) {
+    group.setInverted(true);
+    group.set(1.0);
+
+    assertArrayEquals(
+        DoubleStream.generate(() -> -1.0).limit(motorControllers.length).toArray(),
+        Arrays.stream(motorControllers).mapToDouble(MotorController::get).toArray(),
+        0.00005);
+  }
+
+  @ParameterizedTest
+  @MethodSource("motorControllerArguments")
+  void disableTest(final MotorControllerGroup group, final MotorController[] motorControllers) {
+    group.set(1.0);
+    group.disable();
+
+    assertArrayEquals(
+        DoubleStream.generate(() -> 0.0).limit(motorControllers.length).toArray(),
+        Arrays.stream(motorControllers).mapToDouble(MotorController::get).toArray(),
+        0.00005);
+  }
+
+  @ParameterizedTest
+  @MethodSource("motorControllerArguments")
+  void stopMotorTest(final MotorControllerGroup group, final MotorController[] motorControllers) {
+    group.set(1.0);
+    group.stopMotor();
+
+    assertArrayEquals(
+        DoubleStream.generate(() -> 0.0).limit(motorControllers.length).toArray(),
+        Arrays.stream(motorControllers).mapToDouble(MotorController::get).toArray(),
+        0.00005);
+  }
+}

wpilibj/src/test/java/org/wpilib/hardware/pneumatic/DoubleSolenoidTestCTRE.java

@@ -0,0 +1,94 @@
+// 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;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+import edu.wpi.first.hal.util.AllocationException;
+import org.junit.jupiter.api.Test;
+
+class DoubleSolenoidTestCTRE {
+  @Test
+  void testValidInitialization() {
+    try (DoubleSolenoid solenoid = new DoubleSolenoid(0, 3, PneumaticsModuleType.CTREPCM, 2, 3)) {
+      solenoid.set(DoubleSolenoid.Value.kReverse);
+      assertEquals(DoubleSolenoid.Value.kReverse, solenoid.get());
+
+      solenoid.set(DoubleSolenoid.Value.kForward);
+      assertEquals(DoubleSolenoid.Value.kForward, solenoid.get());
+
+      solenoid.set(DoubleSolenoid.Value.kOff);
+      assertEquals(DoubleSolenoid.Value.kOff, solenoid.get());
+    }
+  }
+
+  @Test
+  void testThrowForwardPortAlreadyInitialized() {
+    try (
+    // Single solenoid that is reused for forward port
+    Solenoid solenoid = new Solenoid(0, 5, PneumaticsModuleType.CTREPCM, 2)) {
+      assertThrows(
+          AllocationException.class,
+          () -> new DoubleSolenoid(0, 5, PneumaticsModuleType.CTREPCM, 2, 3));
+    }
+  }
+
+  @Test
+  void testThrowReversePortAlreadyInitialized() {
+    try (
+    // Single solenoid that is reused for forward port
+    Solenoid solenoid = new Solenoid(0, 6, PneumaticsModuleType.CTREPCM, 3)) {
+      assertThrows(
+          AllocationException.class,
+          () -> new DoubleSolenoid(0, 6, PneumaticsModuleType.CTREPCM, 2, 3));
+    }
+  }
+
+  @Test
+  void testThrowBothPortsAlreadyInitialized() {
+    try (
+    // Single solenoid that is reused for forward port
+    Solenoid solenoid0 = new Solenoid(0, 6, PneumaticsModuleType.CTREPCM, 2);
+        Solenoid solenoid1 = new Solenoid(0, 6, PneumaticsModuleType.CTREPCM, 3)) {
+      assertThrows(
+          AllocationException.class,
+          () -> new DoubleSolenoid(0, 6, PneumaticsModuleType.CTREPCM, 2, 3));
+    }
+  }
+
+  @Test
+  void testToggle() {
+    try (DoubleSolenoid solenoid = new DoubleSolenoid(0, 4, PneumaticsModuleType.CTREPCM, 2, 3)) {
+      // Bootstrap it into reverse
+      solenoid.set(DoubleSolenoid.Value.kReverse);
+
+      solenoid.toggle();
+      assertEquals(DoubleSolenoid.Value.kForward, solenoid.get());
+
+      solenoid.toggle();
+      assertEquals(DoubleSolenoid.Value.kReverse, solenoid.get());
+
+      // Of shouldn't do anything on toggle
+      solenoid.set(DoubleSolenoid.Value.kOff);
+      solenoid.toggle();
+      assertEquals(DoubleSolenoid.Value.kOff, solenoid.get());
+    }
+  }
+
+  @Test
+  void testInvalidForwardPort() {
+    assertThrows(
+        IllegalArgumentException.class,
+        () -> new DoubleSolenoid(0, 0, PneumaticsModuleType.CTREPCM, 100, 1));
+  }
+
+  @Test
+  void testInvalidReversePort() {
+    assertThrows(
+        IllegalArgumentException.class,
+        () -> new DoubleSolenoid(0, 0, PneumaticsModuleType.CTREPCM, 0, 100));
+  }
+}

wpilibj/src/test/java/org/wpilib/hardware/pneumatic/DoubleSolenoidTestREV.java

@@ -0,0 +1,94 @@
+// 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;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+import edu.wpi.first.hal.util.AllocationException;
+import org.junit.jupiter.api.Test;
+
+class DoubleSolenoidTestREV {
+  @Test
+  void testValidInitialization() {
+    try (DoubleSolenoid solenoid = new DoubleSolenoid(0, 3, PneumaticsModuleType.REVPH, 2, 3)) {
+      solenoid.set(DoubleSolenoid.Value.kReverse);
+      assertEquals(DoubleSolenoid.Value.kReverse, solenoid.get());
+
+      solenoid.set(DoubleSolenoid.Value.kForward);
+      assertEquals(DoubleSolenoid.Value.kForward, solenoid.get());
+
+      solenoid.set(DoubleSolenoid.Value.kOff);
+      assertEquals(DoubleSolenoid.Value.kOff, solenoid.get());
+    }
+  }
+
+  @Test
+  void testThrowForwardPortAlreadyInitialized() {
+    try (
+    // Single solenoid that is reused for forward port
+    Solenoid solenoid = new Solenoid(0, 5, PneumaticsModuleType.REVPH, 2)) {
+      assertThrows(
+          AllocationException.class,
+          () -> new DoubleSolenoid(0, 5, PneumaticsModuleType.REVPH, 2, 3));
+    }
+  }
+
+  @Test
+  void testThrowReversePortAlreadyInitialized() {
+    try (
+    // Single solenoid that is reused for forward port
+    Solenoid solenoid = new Solenoid(0, 6, PneumaticsModuleType.REVPH, 3)) {
+      assertThrows(
+          AllocationException.class,
+          () -> new DoubleSolenoid(0, 6, PneumaticsModuleType.REVPH, 2, 3));
+    }
+  }
+
+  @Test
+  void testThrowBothPortsAlreadyInitialized() {
+    try (
+    // Single solenoid that is reused for forward port
+    Solenoid solenoid0 = new Solenoid(0, 6, PneumaticsModuleType.REVPH, 2);
+        Solenoid solenoid1 = new Solenoid(0, 6, PneumaticsModuleType.REVPH, 3)) {
+      assertThrows(
+          AllocationException.class,
+          () -> new DoubleSolenoid(0, 6, PneumaticsModuleType.REVPH, 2, 3));
+    }
+  }
+
+  @Test
+  void testToggle() {
+    try (DoubleSolenoid solenoid = new DoubleSolenoid(0, 4, PneumaticsModuleType.REVPH, 2, 3)) {
+      // Bootstrap it into reverse
+      solenoid.set(DoubleSolenoid.Value.kReverse);
+
+      solenoid.toggle();
+      assertEquals(DoubleSolenoid.Value.kForward, solenoid.get());
+
+      solenoid.toggle();
+      assertEquals(DoubleSolenoid.Value.kReverse, solenoid.get());
+
+      // Of shouldn't do anything on toggle
+      solenoid.set(DoubleSolenoid.Value.kOff);
+      solenoid.toggle();
+      assertEquals(DoubleSolenoid.Value.kOff, solenoid.get());
+    }
+  }
+
+  @Test
+  void testInvalidForwardPort() {
+    assertThrows(
+        IllegalArgumentException.class,
+        () -> new DoubleSolenoid(0, 1, PneumaticsModuleType.REVPH, 100, 1));
+  }
+
+  @Test
+  void testInvalidReversePort() {
+    assertThrows(
+        IllegalArgumentException.class,
+        () -> new DoubleSolenoid(0, 1, PneumaticsModuleType.REVPH, 0, 100));
+  }
+}

wpilibj/src/test/java/org/wpilib/hardware/pneumatic/SolenoidTestCTRE.java

@@ -0,0 +1,65 @@
+// 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;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.util.AllocationException;
+import org.junit.jupiter.api.Test;
+
+class SolenoidTestCTRE {
+  @Test
+  void testValidInitialization() {
+    try (Solenoid solenoid = new Solenoid(0, 3, PneumaticsModuleType.CTREPCM, 2)) {
+      assertEquals(2, solenoid.getChannel());
+
+      solenoid.set(true);
+      assertTrue(solenoid.get());
+
+      solenoid.set(false);
+      assertFalse(solenoid.get());
+    }
+  }
+
+  @Test
+  void testDoubleInitialization() {
+    try (Solenoid solenoid = new Solenoid(0, 3, PneumaticsModuleType.CTREPCM, 2)) {
+      assertThrows(
+          AllocationException.class, () -> new Solenoid(0, 3, PneumaticsModuleType.CTREPCM, 2));
+    }
+  }
+
+  @Test
+  void testDoubleInitializationFromDoubleSolenoid() {
+    try (DoubleSolenoid solenoid = new DoubleSolenoid(0, 3, PneumaticsModuleType.CTREPCM, 2, 3)) {
+      assertThrows(
+          AllocationException.class, () -> new Solenoid(0, 3, PneumaticsModuleType.CTREPCM, 2));
+    }
+  }
+
+  @Test
+  void testInvalidChannel() {
+    assertThrows(
+        IllegalArgumentException.class,
+        () -> new Solenoid(0, 3, PneumaticsModuleType.CTREPCM, 100));
+  }
+
+  @Test
+  void testToggle() {
+    try (Solenoid solenoid = new Solenoid(0, 3, PneumaticsModuleType.CTREPCM, 2)) {
+      solenoid.set(true);
+      assertTrue(solenoid.get());
+
+      solenoid.toggle();
+      assertFalse(solenoid.get());
+
+      solenoid.toggle();
+      assertTrue(solenoid.get());
+    }
+  }
+}

wpilibj/src/test/java/org/wpilib/hardware/pneumatic/SolenoidTestREV.java

@@ -0,0 +1,64 @@
+// 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;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.util.AllocationException;
+import org.junit.jupiter.api.Test;
+
+class SolenoidTestREV {
+  @Test
+  void testValidInitialization() {
+    try (Solenoid solenoid = new Solenoid(0, 3, PneumaticsModuleType.REVPH, 2)) {
+      assertEquals(2, solenoid.getChannel());
+
+      solenoid.set(true);
+      assertTrue(solenoid.get());
+
+      solenoid.set(false);
+      assertFalse(solenoid.get());
+    }
+  }
+
+  @Test
+  void testDoubleInitialization() {
+    try (Solenoid solenoid = new Solenoid(0, 3, PneumaticsModuleType.REVPH, 2)) {
+      assertThrows(
+          AllocationException.class, () -> new Solenoid(0, 3, PneumaticsModuleType.REVPH, 2));
+    }
+  }
+
+  @Test
+  void testDoubleInitializationFromDoubleSolenoid() {
+    try (DoubleSolenoid solenoid = new DoubleSolenoid(0, 3, PneumaticsModuleType.REVPH, 2, 3)) {
+      assertThrows(
+          AllocationException.class, () -> new Solenoid(0, 3, PneumaticsModuleType.REVPH, 2));
+    }
+  }
+
+  @Test
+  void testInvalidChannel() {
+    assertThrows(
+        IllegalArgumentException.class, () -> new Solenoid(0, 3, PneumaticsModuleType.REVPH, 100));
+  }
+
+  @Test
+  void testToggle() {
+    try (Solenoid solenoid = new Solenoid(0, 3, PneumaticsModuleType.REVPH, 2)) {
+      solenoid.set(true);
+      assertTrue(solenoid.get());
+
+      solenoid.toggle();
+      assertFalse(solenoid.get());
+
+      solenoid.toggle();
+      assertTrue(solenoid.get());
+    }
+  }
+}

wpilibj/src/test/java/org/wpilib/hardware/power/PowerDistributionTest.java

@@ -0,0 +1,29 @@
+// 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;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
+import edu.wpi.first.wpilibj.simulation.PDPSim;
+import org.junit.jupiter.api.Test;
+
+class PowerDistributionTest {
+  @Test
+  void testGetAllCurrents() {
+    HAL.initialize(500, 0);
+    PowerDistribution pdp = new PowerDistribution(0, 1, ModuleType.kRev);
+    PDPSim sim = new PDPSim(pdp);
+
+    for (int i = 0; i < pdp.getNumChannels(); i++) {
+      sim.setCurrent(i, 24 - i);
+    }
+    double[] currents = pdp.getAllCurrents();
+    for (int i = 0; i < pdp.getNumChannels(); i++) {
+      assertEquals(24 - i, currents[i]);
+    }
+  }
+}

wpilibj/src/test/java/org/wpilib/hardware/range/SharpIRTest.java

@@ -0,0 +1,27 @@
+// 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;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.wpilibj.simulation.SharpIRSim;
+import org.junit.jupiter.api.Test;
+
+class SharpIRTest {
+  @Test
+  void testSharpIR() {
+    try (SharpIR s = SharpIR.GP2Y0A02YK0F(1)) {
+      SharpIRSim sim = new SharpIRSim(s);
+
+      assertEquals(0.2, s.getRange());
+
+      sim.setRange(0.3);
+      assertEquals(0.3, s.getRange());
+
+      sim.setRange(3);
+      assertEquals(1.5, s.getRange());
+    }
+  }
+}

wpilibj/src/test/java/org/wpilib/hardware/rotation/AnalogPotentiometerTest.java

@@ -0,0 +1,104 @@
+// 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;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.AnalogInputSim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import org.junit.jupiter.api.Test;
+
+class AnalogPotentiometerTest {
+  @Test
+  void testInitializeWithAnalogInput() {
+    HAL.initialize(500, 0);
+
+    try (AnalogInput ai = new AnalogInput(0);
+        AnalogPotentiometer pot = new AnalogPotentiometer(ai)) {
+      AnalogInputSim sim = new AnalogInputSim(ai);
+
+      RoboRioSim.resetData();
+      sim.setVoltage(2.8);
+      assertEquals(2.8 / 3.3, pot.get());
+    }
+  }
+
+  @Test
+  void testInitializeWithAnalogInputAndScale() {
+    HAL.initialize(500, 0);
+
+    try (AnalogInput ai = new AnalogInput(0);
+        AnalogPotentiometer pot = new AnalogPotentiometer(ai, 270.0)) {
+      RoboRioSim.resetData();
+      AnalogInputSim sim = new AnalogInputSim(ai);
+
+      sim.setVoltage(3.3);
+      assertEquals(270.0, pot.get());
+
+      sim.setVoltage(2.5);
+      assertEquals((2.5 / 3.3) * 270.0, pot.get());
+
+      sim.setVoltage(0.0);
+      assertEquals(0.0, pot.get());
+    }
+  }
+
+  @Test
+  void testInitializeWithChannel() {
+    HAL.initialize(500, 0);
+
+    try (AnalogPotentiometer pot = new AnalogPotentiometer(1)) {
+      RoboRioSim.resetData();
+      AnalogInputSim sim = new AnalogInputSim(1);
+
+      sim.setVoltage(3.3);
+      assertEquals(1.0, pot.get());
+    }
+  }
+
+  @Test
+  void testInitializeWithChannelAndScale() {
+    HAL.initialize(500, 0);
+
+    try (AnalogPotentiometer pot = new AnalogPotentiometer(1, 180.0)) {
+      RoboRioSim.resetData();
+      AnalogInputSim sim = new AnalogInputSim(1);
+
+      sim.setVoltage(3.3);
+      assertEquals(180.0, pot.get());
+
+      sim.setVoltage(0.0);
+      assertEquals(0.0, pot.get());
+    }
+  }
+
+  @Test
+  void testWithModifiedBatteryVoltage() {
+    try (AnalogPotentiometer pot = new AnalogPotentiometer(1, 180.0, 90.0)) {
+      RoboRioSim.resetData();
+      AnalogInputSim sim = new AnalogInputSim(1);
+
+      // Test at 3.3v
+      sim.setVoltage(3.3);
+      assertEquals(270, pot.get());
+
+      sim.setVoltage(0.0);
+      assertEquals(90, pot.get());
+
+      // Simulate a lower battery voltage
+      RoboRioSim.setUserVoltage3V3(2.5);
+
+      sim.setVoltage(2.5);
+      assertEquals(270, pot.get());
+
+      sim.setVoltage(2.0);
+      assertEquals(234, pot.get());
+
+      sim.setVoltage(0.0);
+      assertEquals(90, pot.get());
+    }
+  }
+}

wpilibj/src/test/java/org/wpilib/opmode/TimedRobotTest.java

@@ -0,0 +1,702 @@
+// 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;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import java.util.concurrent.atomic.AtomicInteger;
+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;
+
+class TimedRobotTest {
+  static final double kPeriod = 0.02;
+
+  static class MockRobot extends TimedRobot {
+    public final AtomicInteger m_simulationInitCount = new AtomicInteger(0);
+    public final AtomicInteger m_disabledInitCount = new AtomicInteger(0);
+    public final AtomicInteger m_autonomousInitCount = new AtomicInteger(0);
+    public final AtomicInteger m_teleopInitCount = new AtomicInteger(0);
+    public final AtomicInteger m_testInitCount = new AtomicInteger(0);
+
+    public final AtomicInteger m_robotPeriodicCount = new AtomicInteger(0);
+    public final AtomicInteger m_simulationPeriodicCount = new AtomicInteger(0);
+    public final AtomicInteger m_disabledPeriodicCount = new AtomicInteger(0);
+    public final AtomicInteger m_autonomousPeriodicCount = new AtomicInteger(0);
+    public final AtomicInteger m_teleopPeriodicCount = new AtomicInteger(0);
+    public final AtomicInteger m_testPeriodicCount = new AtomicInteger(0);
+
+    public final AtomicInteger m_disabledExitCount = new AtomicInteger(0);
+    public final AtomicInteger m_autonomousExitCount = new AtomicInteger(0);
+    public final AtomicInteger m_teleopExitCount = new AtomicInteger(0);
+    public final AtomicInteger m_testExitCount = new AtomicInteger(0);
+
+    MockRobot() {
+      super(kPeriod);
+    }
+
+    @Override
+    public void simulationInit() {
+      m_simulationInitCount.addAndGet(1);
+    }
+
+    @Override
+    public void disabledInit() {
+      m_disabledInitCount.addAndGet(1);
+    }
+
+    @Override
+    public void autonomousInit() {
+      m_autonomousInitCount.addAndGet(1);
+    }
+
+    @Override
+    public void teleopInit() {
+      m_teleopInitCount.addAndGet(1);
+    }
+
+    @Override
+    public void testInit() {
+      m_testInitCount.addAndGet(1);
+    }
+
+    @Override
+    public void robotPeriodic() {
+      m_robotPeriodicCount.addAndGet(1);
+    }
+
+    @Override
+    public void simulationPeriodic() {
+      m_simulationPeriodicCount.addAndGet(1);
+    }
+
+    @Override
+    public void disabledPeriodic() {
+      m_disabledPeriodicCount.addAndGet(1);
+    }
+
+    @Override
+    public void autonomousPeriodic() {
+      m_autonomousPeriodicCount.addAndGet(1);
+    }
+
+    @Override
+    public void teleopPeriodic() {
+      m_teleopPeriodicCount.addAndGet(1);
+    }
+
+    @Override
+    public void testPeriodic() {
+      m_testPeriodicCount.addAndGet(1);
+    }
+
+    @Override
+    public void disabledExit() {
+      m_disabledExitCount.addAndGet(1);
+    }
+
+    @Override
+    public void autonomousExit() {
+      m_autonomousExitCount.addAndGet(1);
+    }
+
+    @Override
+    public void teleopExit() {
+      m_teleopExitCount.addAndGet(1);
+    }
+
+    @Override
+    public void testExit() {
+      m_testExitCount.addAndGet(1);
+    }
+  }
+
+  @BeforeEach
+  void setup() {
+    SimHooks.pauseTiming();
+    DriverStationSim.resetData();
+  }
+
+  @AfterEach
+  void cleanup() {
+    SimHooks.resumeTiming();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void disabledModeTest() {
+    MockRobot robot = new MockRobot();
+
+    Thread robotThread = new Thread(robot::startCompetition);
+    robotThread.start();
+
+    DriverStationSim.setEnabled(false);
+    DriverStationSim.notifyNewData();
+    SimHooks.stepTiming(0.0); // Wait for Notifiers
+
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(0, robot.m_robotPeriodicCount.get());
+    assertEquals(0, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    assertEquals(0, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
+    SimHooks.stepTiming(kPeriod);
+
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(1, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(1, robot.m_robotPeriodicCount.get());
+    assertEquals(1, robot.m_simulationPeriodicCount.get());
+    assertEquals(1, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    assertEquals(0, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
+    SimHooks.stepTiming(kPeriod);
+
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(1, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(2, robot.m_robotPeriodicCount.get());
+    assertEquals(2, robot.m_simulationPeriodicCount.get());
+    assertEquals(2, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    assertEquals(0, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
+    robot.endCompetition();
+    try {
+      robotThread.interrupt();
+      robotThread.join();
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    robot.close();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void autonomousModeTest() {
+    MockRobot robot = new MockRobot();
+
+    Thread robotThread = new Thread(robot::startCompetition);
+    robotThread.start();
+
+    DriverStationSim.setEnabled(true);
+    DriverStationSim.setAutonomous(true);
+    DriverStationSim.setTest(false);
+    DriverStationSim.notifyNewData();
+    SimHooks.stepTiming(0.0); // Wait for Notifiers
+
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(0, robot.m_robotPeriodicCount.get());
+    assertEquals(0, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    assertEquals(0, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
+    SimHooks.stepTiming(kPeriod);
+
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(1, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(1, robot.m_robotPeriodicCount.get());
+    assertEquals(1, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(1, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    assertEquals(0, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
+    SimHooks.stepTiming(kPeriod);
+
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(1, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(2, robot.m_robotPeriodicCount.get());
+    assertEquals(2, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(2, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    assertEquals(0, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
+    robot.endCompetition();
+    try {
+      robotThread.interrupt();
+      robotThread.join();
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    robot.close();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void teleopModeTest() {
+    MockRobot robot = new MockRobot();
+
+    Thread robotThread = new Thread(robot::startCompetition);
+    robotThread.start();
+
+    DriverStationSim.setEnabled(true);
+    DriverStationSim.setAutonomous(false);
+    DriverStationSim.setTest(false);
+    DriverStationSim.notifyNewData();
+    SimHooks.stepTiming(0.0); // Wait for Notifiers
+
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(0, robot.m_robotPeriodicCount.get());
+    assertEquals(0, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    assertEquals(0, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
+    SimHooks.stepTiming(kPeriod);
+
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(1, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(1, robot.m_robotPeriodicCount.get());
+    assertEquals(1, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(1, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    assertEquals(0, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
+    SimHooks.stepTiming(kPeriod);
+
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(1, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(2, robot.m_robotPeriodicCount.get());
+    assertEquals(2, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(2, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    assertEquals(0, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
+    robot.endCompetition();
+    try {
+      robotThread.interrupt();
+      robotThread.join();
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    robot.close();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void testModeTest() {
+    MockRobot robot = new MockRobot();
+
+    Thread robotThread = new Thread(robot::startCompetition);
+    robotThread.start();
+
+    DriverStationSim.setEnabled(true);
+    DriverStationSim.setAutonomous(false);
+    DriverStationSim.setTest(true);
+    DriverStationSim.notifyNewData();
+    SimHooks.stepTiming(0.0); // Wait for Notifiers
+
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(0, robot.m_robotPeriodicCount.get());
+    assertEquals(0, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    assertEquals(0, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
+    SimHooks.stepTiming(kPeriod);
+
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(1, robot.m_testInitCount.get());
+
+    assertEquals(1, robot.m_robotPeriodicCount.get());
+    assertEquals(1, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(1, robot.m_testPeriodicCount.get());
+
+    assertEquals(0, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
+    SimHooks.stepTiming(kPeriod);
+
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(1, robot.m_testInitCount.get());
+
+    assertEquals(2, robot.m_robotPeriodicCount.get());
+    assertEquals(2, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(2, robot.m_testPeriodicCount.get());
+
+    assertEquals(0, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
+    DriverStationSim.setEnabled(false);
+    DriverStationSim.setAutonomous(false);
+    DriverStationSim.setTest(false);
+    DriverStationSim.notifyNewData();
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(1, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(1, robot.m_testInitCount.get());
+
+    assertEquals(3, robot.m_robotPeriodicCount.get());
+    assertEquals(3, robot.m_simulationPeriodicCount.get());
+    assertEquals(1, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(2, robot.m_testPeriodicCount.get());
+
+    assertEquals(0, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(1, robot.m_testExitCount.get());
+
+    robot.endCompetition();
+    try {
+      robotThread.interrupt();
+      robotThread.join();
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    robot.close();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void modeChangeTest() {
+    MockRobot robot = new MockRobot();
+
+    Thread robotThread = new Thread(robot::startCompetition);
+    robotThread.start();
+
+    // Start in disabled
+    DriverStationSim.setEnabled(false);
+    DriverStationSim.setAutonomous(false);
+    DriverStationSim.setTest(false);
+    DriverStationSim.notifyNewData();
+    SimHooks.stepTiming(0.0); // Wait for Notifiers
+
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(0, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
+    SimHooks.stepTiming(kPeriod);
+
+    assertEquals(1, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(0, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
+    // Transition to autonomous
+    DriverStationSim.setEnabled(true);
+    DriverStationSim.setAutonomous(true);
+    DriverStationSim.setTest(false);
+    DriverStationSim.notifyNewData();
+
+    SimHooks.stepTiming(kPeriod);
+
+    assertEquals(1, robot.m_disabledInitCount.get());
+    assertEquals(1, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(1, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
+    // Transition to teleop
+    DriverStationSim.setEnabled(true);
+    DriverStationSim.setAutonomous(false);
+    DriverStationSim.setTest(false);
+    DriverStationSim.notifyNewData();
+
+    SimHooks.stepTiming(kPeriod);
+
+    assertEquals(1, robot.m_disabledInitCount.get());
+    assertEquals(1, robot.m_autonomousInitCount.get());
+    assertEquals(1, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(1, robot.m_disabledExitCount.get());
+    assertEquals(1, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
+    // Transition to test
+    DriverStationSim.setEnabled(true);
+    DriverStationSim.setAutonomous(false);
+    DriverStationSim.setTest(true);
+    DriverStationSim.notifyNewData();
+
+    SimHooks.stepTiming(kPeriod);
+
+    assertEquals(1, robot.m_disabledInitCount.get());
+    assertEquals(1, robot.m_autonomousInitCount.get());
+    assertEquals(1, robot.m_teleopInitCount.get());
+    assertEquals(1, robot.m_testInitCount.get());
+
+    assertEquals(1, robot.m_disabledExitCount.get());
+    assertEquals(1, robot.m_autonomousExitCount.get());
+    assertEquals(1, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
+    // Transition to disabled
+    DriverStationSim.setEnabled(false);
+    DriverStationSim.setAutonomous(false);
+    DriverStationSim.setTest(false);
+    DriverStationSim.notifyNewData();
+
+    SimHooks.stepTiming(kPeriod);
+
+    assertEquals(2, robot.m_disabledInitCount.get());
+    assertEquals(1, robot.m_autonomousInitCount.get());
+    assertEquals(1, robot.m_teleopInitCount.get());
+    assertEquals(1, robot.m_testInitCount.get());
+
+    assertEquals(1, robot.m_disabledExitCount.get());
+    assertEquals(1, robot.m_autonomousExitCount.get());
+    assertEquals(1, robot.m_teleopExitCount.get());
+    assertEquals(1, robot.m_testExitCount.get());
+
+    robot.endCompetition();
+    try {
+      robotThread.interrupt();
+      robotThread.join();
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    robot.close();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void addPeriodicTest() {
+    MockRobot robot = new MockRobot();
+
+    final AtomicInteger callbackCount = new AtomicInteger(0);
+    robot.addPeriodic(() -> callbackCount.addAndGet(1), kPeriod / 2.0);
+
+    Thread robotThread = new Thread(robot::startCompetition);
+    robotThread.start();
+
+    DriverStationSim.setEnabled(false);
+    DriverStationSim.notifyNewData();
+    SimHooks.stepTiming(0.0); // Wait for Notifiers
+
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, callbackCount.get());
+
+    SimHooks.stepTiming(kPeriod / 2.0);
+
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(1, callbackCount.get());
+
+    SimHooks.stepTiming(kPeriod / 2.0);
+
+    assertEquals(1, robot.m_disabledInitCount.get());
+    assertEquals(1, robot.m_disabledPeriodicCount.get());
+    assertEquals(2, callbackCount.get());
+
+    robot.endCompetition();
+    try {
+      robotThread.interrupt();
+      robotThread.join();
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    robot.close();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void addPeriodicWithOffsetTest() {
+    MockRobot robot = new MockRobot();
+
+    final AtomicInteger callbackCount = new AtomicInteger(0);
+    robot.addPeriodic(() -> callbackCount.addAndGet(1), kPeriod / 2.0, kPeriod / 4.0);
+
+    // Expirations in this test (ms)
+    //
+    // Let p be period in ms.
+    //
+    // Robot | Callback
+    // ================
+    //     p |    0.75p
+    //    2p |    1.25p
+
+    Thread robotThread = new Thread(robot::startCompetition);
+    robotThread.start();
+
+    DriverStationSim.setEnabled(false);
+    DriverStationSim.notifyNewData();
+    SimHooks.stepTiming(0.0); // Wait for Notifiers
+
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, callbackCount.get());
+
+    SimHooks.stepTiming(kPeriod * 3.0 / 8.0);
+
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, callbackCount.get());
+
+    SimHooks.stepTiming(kPeriod * 3.0 / 8.0);
+
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(1, callbackCount.get());
+
+    SimHooks.stepTiming(kPeriod / 4.0);
+
+    assertEquals(1, robot.m_disabledInitCount.get());
+    assertEquals(1, robot.m_disabledPeriodicCount.get());
+    assertEquals(1, callbackCount.get());
+
+    SimHooks.stepTiming(kPeriod / 4.0);
+
+    assertEquals(1, robot.m_disabledInitCount.get());
+    assertEquals(1, robot.m_disabledPeriodicCount.get());
+    assertEquals(2, callbackCount.get());
+
+    robot.endCompetition();
+    try {
+      robotThread.interrupt();
+      robotThread.join();
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    robot.close();
+  }
+}

wpilibj/src/test/java/org/wpilib/opmode/TimesliceRobotTest.java

@@ -0,0 +1,119 @@
+// 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;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import java.util.concurrent.atomic.AtomicInteger;
+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;
+
+class TimesliceRobotTest {
+  static class MockRobot extends TimesliceRobot {
+    public final AtomicInteger m_robotPeriodicCount = new AtomicInteger(0);
+
+    MockRobot() {
+      super(0.002, 0.005);
+    }
+
+    @Override
+    public void robotPeriodic() {
+      m_robotPeriodicCount.addAndGet(1);
+    }
+  }
+
+  @BeforeEach
+  void setup() {
+    SimHooks.pauseTiming();
+  }
+
+  @AfterEach
+  void cleanup() {
+    SimHooks.resumeTiming();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void scheduleTest() {
+    MockRobot robot = new MockRobot();
+
+    final AtomicInteger callbackCount1 = new AtomicInteger(0);
+    final AtomicInteger callbackCount2 = new AtomicInteger(0);
+
+    // Timeslice allocation table
+    //
+    // |       Name      | Offset (ms) | Allocation (ms)|
+    // |-----------------|-------------|----------------|
+    // | RobotPeriodic() |           0 |              2 |
+    // | Callback 1      |           2 |            0.5 |
+    // | Callback 2      |         2.5 |              1 |
+    robot.schedule(() -> callbackCount1.addAndGet(1), 0.0005);
+    robot.schedule(() -> callbackCount2.addAndGet(1), 0.001);
+
+    Thread robotThread = new Thread(robot::startCompetition);
+    robotThread.start();
+
+    DriverStationSim.setEnabled(false);
+    DriverStationSim.notifyNewData();
+    SimHooks.stepTiming(0.0); // Wait for Notifiers
+
+    // Functions scheduled with addPeriodic() are delayed by one period before
+    // their first run (5 ms for this test's callbacks here and 20 ms for
+    // robotPeriodic()).
+    SimHooks.stepTiming(0.005);
+
+    assertEquals(0, robot.m_robotPeriodicCount.get());
+    assertEquals(0, callbackCount1.get());
+    assertEquals(0, callbackCount2.get());
+
+    // Step to 1.5 ms
+    SimHooks.stepTiming(0.0015);
+    assertEquals(0, robot.m_robotPeriodicCount.get());
+    assertEquals(0, callbackCount1.get());
+    assertEquals(0, callbackCount2.get());
+
+    // Step to 2.25 ms
+    SimHooks.stepTiming(0.00075);
+    assertEquals(0, robot.m_robotPeriodicCount.get());
+    assertEquals(1, callbackCount1.get());
+    assertEquals(0, callbackCount2.get());
+
+    // Step to 2.75 ms
+    SimHooks.stepTiming(0.0005);
+    assertEquals(0, robot.m_robotPeriodicCount.get());
+    assertEquals(1, callbackCount1.get());
+    assertEquals(1, callbackCount2.get());
+
+    robot.endCompetition();
+    try {
+      robotThread.interrupt();
+      robotThread.join();
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    robot.close();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void scheduleOverrunTest() {
+    MockRobot robot = new MockRobot();
+
+    robot.schedule(() -> {}, 0.0005);
+    robot.schedule(() -> {}, 0.001);
+
+    // offset = 2 ms + 0.5 ms + 1 ms = 3.5 ms
+    // 3.5 ms + 3 ms allocation = 6.5 ms > max of 5 ms
+    assertThrows(IllegalStateException.class, () -> robot.schedule(() -> {}, 0.003));
+
+    robot.endCompetition();
+    robot.close();
+  }
+}

wpilibj/src/test/java/org/wpilib/simulation/ADXL345SimTest.java

@@ -0,0 +1,38 @@
+// 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.simulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.ADXL345_I2C;
+import edu.wpi.first.wpilibj.I2C;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.EnumSource;
+
+class ADXL345SimTest {
+  @ParameterizedTest
+  @EnumSource(ADXL345_I2C.Range.class)
+  void testInitI2C(ADXL345_I2C.Range range) {
+    HAL.initialize(500, 0);
+
+    try (ADXL345_I2C accel = new ADXL345_I2C(I2C.Port.kPort0, range)) {
+      ADXL345Sim sim = new ADXL345Sim(accel);
+
+      sim.setX(1.91);
+      sim.setY(-3.405);
+      sim.setZ(2.29);
+
+      assertEquals(1.91, accel.getX());
+      assertEquals(-3.405, accel.getY());
+      assertEquals(2.29, accel.getZ());
+
+      ADXL345_I2C.AllAxes allAccel = accel.getAccelerations();
+      assertEquals(1.91, allAccel.XAxis);
+      assertEquals(-3.405, allAccel.YAxis);
+      assertEquals(2.29, allAccel.ZAxis);
+    }
+  }
+}

wpilibj/src/test/java/org/wpilib/simulation/AddressableLEDSimTest.java

@@ -0,0 +1,104 @@
+// 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.simulation;
+
+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.HAL;
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.simulation.testutils.BooleanCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.BufferCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.IntCallback;
+import java.util.Arrays;
+import org.junit.jupiter.api.Test;
+
+class AddressableLEDSimTest {
+  @Test
+  void testInitialization() {
+    HAL.initialize(500, 0);
+    AddressableLEDSim sim = new AddressableLEDSim(0);
+    assertFalse(sim.getInitialized());
+
+    BooleanCallback initializedCallback = new BooleanCallback();
+
+    try (CallbackStore cb = sim.registerInitializedCallback(initializedCallback, false);
+        AddressableLED led = new AddressableLED(0)) {
+      assertTrue(sim.getInitialized());
+      assertTrue(initializedCallback.wasTriggered());
+      assertTrue(initializedCallback.getSetValue());
+    }
+  }
+
+  @Test
+  void testLength() {
+    AddressableLEDSim sim = new AddressableLEDSim(0);
+    IntCallback callback = new IntCallback();
+    try (CallbackStore cb = sim.registerLengthCallback(callback, false);
+        AddressableLED led = new AddressableLED(0)) {
+      assertEquals(0, sim.getLength()); // Defaults to 0 leds
+
+      AddressableLEDBuffer ledData = new AddressableLEDBuffer(50);
+      led.setLength(ledData.getLength());
+      led.setData(ledData);
+
+      assertEquals(50, sim.getLength());
+      assertTrue(callback.wasTriggered());
+      assertEquals(50, callback.getSetValue());
+    }
+  }
+
+  @Test
+  void testSetData() {
+    AddressableLEDSim sim = new AddressableLEDSim(0);
+    BufferCallback callback = new BufferCallback();
+
+    try (AddressableLED led = new AddressableLED(0);
+        CallbackStore cb = AddressableLEDSim.registerDataCallback(callback)) {
+      assertEquals(0, sim.getLength()); // Defaults to 0 leds
+
+      AddressableLEDBuffer ledData = new AddressableLEDBuffer(3);
+      led.setLength(ledData.getLength());
+
+      ledData.setRGB(0, 255, 0, 0);
+      ledData.setRGB(1, 0, 255, 0);
+      ledData.setRGB(2, 0, 0, 255);
+      led.setData(ledData);
+
+      byte[] data = sim.getData();
+      System.out.println(Arrays.toString(data));
+      assertEquals(9, data.length);
+
+      assertEquals((byte) 255, data[0]);
+      assertEquals((byte) 0, data[1]);
+      assertEquals((byte) 0, data[2]);
+
+      assertEquals((byte) 0, data[3]);
+      assertEquals((byte) 255, data[4]);
+      assertEquals((byte) 0, data[5]);
+
+      assertEquals((byte) 0, data[6]);
+      assertEquals((byte) 0, data[7]);
+      assertEquals((byte) 255, data[8]);
+
+      assertTrue(callback.wasTriggered());
+      data = callback.getSetValue();
+
+      assertEquals((byte) 255, data[0]);
+      assertEquals((byte) 0, data[1]);
+      assertEquals((byte) 0, data[2]);
+
+      assertEquals((byte) 0, data[3]);
+      assertEquals((byte) 255, data[4]);
+      assertEquals((byte) 0, data[5]);
+
+      assertEquals((byte) 0, data[6]);
+      assertEquals((byte) 0, data[7]);
+      assertEquals((byte) 255, data[8]);
+    }
+  }
+}

wpilibj/src/test/java/org/wpilib/simulation/AnalogEncoderSimTest.java

@@ -0,0 +1,25 @@
+// 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.simulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.wpilibj.AnalogEncoder;
+import edu.wpi.first.wpilibj.AnalogInput;
+import org.junit.jupiter.api.Test;
+
+class AnalogEncoderSimTest {
+  @Test
+  void testBasic() {
+    try (var analogInput = new AnalogInput(0);
+        var analogEncoder = new AnalogEncoder(analogInput, 360, 0)) {
+      var encoderSim = new AnalogEncoderSim(analogEncoder);
+
+      encoderSim.set(180);
+      assertEquals(analogEncoder.get(), 180, 1E-8);
+      assertEquals(encoderSim.get(), 180, 1E-8);
+    }
+  }
+}

wpilibj/src/test/java/org/wpilib/simulation/AnalogInputSimTest.java

@@ -0,0 +1,64 @@
+// 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.simulation;
+
+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.AnalogInput;
+import edu.wpi.first.wpilibj.simulation.testutils.BooleanCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.DoubleCallback;
+import org.junit.jupiter.api.Test;
+
+class AnalogInputSimTest {
+  @Test
+  void setInitializeTest() {
+    HAL.initialize(500, 0);
+
+    AnalogInputSim sim = new AnalogInputSim(5);
+    BooleanCallback callback = new BooleanCallback();
+
+    try (CallbackStore cb = sim.registerInitializedCallback(callback, false);
+        AnalogInput input = new AnalogInput(5)) {
+      assertTrue(callback.wasTriggered());
+      assertTrue(callback.getSetValue());
+    }
+  }
+
+  @Test
+  void testSetVoltage() {
+    HAL.initialize(500, 0);
+
+    AnalogInputSim sim = new AnalogInputSim(5);
+    DoubleCallback callback = new DoubleCallback();
+
+    try (CallbackStore cb = sim.registerVoltageCallback(callback, false);
+        AnalogInput input = new AnalogInput(5)) {
+      // Bootstrap the voltage to be non-zero
+      sim.setVoltage(1.0);
+
+      for (double i = 0; i < 5.0; i += 0.1) {
+        callback.reset();
+
+        sim.setVoltage(0);
+        assertEquals(input.getVoltage(), 0, 0.001);
+
+        callback.reset();
+        sim.setVoltage(i);
+        assertEquals(input.getVoltage(), i, 0.001);
+      }
+    }
+  }
+
+  @Test
+  void testSetOverSampleBits() {
+    HAL.initialize(500, 0);
+    try (AnalogInput input = new AnalogInput(5)) {
+      input.setOversampleBits(3504);
+      assertEquals(3504, input.getOversampleBits());
+    }
+  }
+}

wpilibj/src/test/java/org/wpilib/simulation/CTREPCMSimTest.java

@@ -0,0 +1,168 @@
+// 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.simulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertNull;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.CompressorConfigType;
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.PneumaticsControlModule;
+import edu.wpi.first.wpilibj.PneumaticsModuleType;
+import edu.wpi.first.wpilibj.simulation.testutils.BooleanCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.DoubleCallback;
+import org.junit.jupiter.api.Test;
+
+class CTREPCMSimTest {
+  @Test
+  void testInitialization() {
+    HAL.initialize(500, 0);
+
+    CTREPCMSim sim = new CTREPCMSim(0);
+    sim.resetData();
+    assertFalse(sim.getInitialized());
+
+    BooleanCallback callback = new BooleanCallback();
+
+    try (CallbackStore cb = sim.registerInitializedCallback(callback, false);
+        PneumaticsControlModule pcm = new PneumaticsControlModule(0)) {
+      assertTrue(sim.getInitialized());
+    }
+    assertFalse(sim.getInitialized());
+  }
+
+  @Test
+  void solenoidOutputTest() {
+    HAL.initialize(500, 0);
+
+    try (PneumaticsControlModule pcm = new PneumaticsControlModule(0);
+        DoubleSolenoid doubleSolenoid = new DoubleSolenoid(0, PneumaticsModuleType.CTREPCM, 3, 4)) {
+      CTREPCMSim sim = new CTREPCMSim(0);
+      sim.resetData();
+
+      BooleanCallback callback3 = new BooleanCallback();
+      BooleanCallback callback4 = new BooleanCallback();
+
+      try (CallbackStore cb3 = sim.registerSolenoidOutputCallback(3, callback3, false);
+          CallbackStore cb4 = sim.registerSolenoidOutputCallback(4, callback4, false)) {
+        // Reverse
+        callback3.reset();
+        callback4.reset();
+        doubleSolenoid.set(DoubleSolenoid.Value.kReverse);
+        assertFalse(callback3.wasTriggered());
+        assertNull(callback3.getSetValue());
+        assertTrue(callback4.wasTriggered());
+        assertTrue(callback4.getSetValue());
+        assertFalse(sim.getSolenoidOutput(3));
+        assertTrue(sim.getSolenoidOutput(4));
+        assertEquals(0x10, pcm.getSolenoids());
+
+        // Forward
+        callback3.reset();
+        callback4.reset();
+        doubleSolenoid.set(DoubleSolenoid.Value.kForward);
+        assertTrue(callback3.wasTriggered());
+        assertTrue(callback3.getSetValue());
+        assertTrue(callback4.wasTriggered());
+        assertFalse(callback4.getSetValue());
+        assertTrue(sim.getSolenoidOutput(3));
+        assertFalse(sim.getSolenoidOutput(4));
+        assertEquals(0x8, pcm.getSolenoids());
+
+        // Off
+        callback3.reset();
+        callback4.reset();
+        doubleSolenoid.set(DoubleSolenoid.Value.kOff);
+        assertTrue(callback3.wasTriggered());
+        assertFalse(callback3.getSetValue());
+        assertFalse(callback4.wasTriggered());
+        assertNull(callback4.getSetValue());
+        assertFalse(sim.getSolenoidOutput(3));
+        assertFalse(sim.getSolenoidOutput(4));
+        assertEquals(0x0, pcm.getSolenoids());
+      }
+    }
+  }
+
+  @Test
+  void setCompressorOnTest() {
+    HAL.initialize(500, 0);
+
+    CTREPCMSim sim = new CTREPCMSim(0);
+    BooleanCallback callback = new BooleanCallback();
+
+    try (PneumaticsControlModule pcm = new PneumaticsControlModule(0);
+        CallbackStore cb = sim.registerCompressorOnCallback(callback, false)) {
+      assertFalse(pcm.getCompressor());
+      assertFalse(sim.getCompressorOn());
+      sim.setCompressorOn(true);
+      assertTrue(pcm.getCompressor());
+      assertTrue(sim.getCompressorOn());
+      assertTrue(callback.wasTriggered());
+      assertTrue(callback.getSetValue());
+    }
+  }
+
+  @Test
+  void setEnableDigital() {
+    HAL.initialize(500, 0);
+
+    CTREPCMSim sim = new CTREPCMSim(0);
+    BooleanCallback callback = new BooleanCallback();
+
+    try (PneumaticsControlModule pcm = new PneumaticsControlModule(0);
+        CallbackStore cb = sim.registerClosedLoopEnabledCallback(callback, false)) {
+      pcm.disableCompressor();
+      assertEquals(pcm.getCompressorConfigType(), CompressorConfigType.Disabled);
+
+      pcm.enableCompressorDigital();
+      assertTrue(sim.getClosedLoopEnabled());
+      assertEquals(pcm.getCompressorConfigType(), CompressorConfigType.Digital);
+      assertTrue(callback.wasTriggered());
+      assertTrue(callback.getSetValue());
+    }
+  }
+
+  @Test
+  void setPressureSwitchEnabledTest() {
+    HAL.initialize(500, 0);
+
+    CTREPCMSim sim = new CTREPCMSim(0);
+    BooleanCallback callback = new BooleanCallback();
+
+    try (PneumaticsControlModule pcm = new PneumaticsControlModule(0);
+        CallbackStore cb = sim.registerPressureSwitchCallback(callback, false)) {
+      assertFalse(pcm.getPressureSwitch());
+
+      sim.setPressureSwitch(true);
+      assertTrue(sim.getPressureSwitch());
+      assertTrue(pcm.getPressureSwitch());
+      assertTrue(callback.wasTriggered());
+      assertTrue(callback.getSetValue());
+    }
+  }
+
+  @Test
+  void setCompressorCurrentTest() {
+    HAL.initialize(500, 0);
+
+    CTREPCMSim sim = new CTREPCMSim(0);
+    DoubleCallback callback = new DoubleCallback();
+
+    try (PneumaticsControlModule pcm = new PneumaticsControlModule(0);
+        CallbackStore cb = sim.registerCompressorCurrentCallback(callback, false)) {
+      assertFalse(pcm.getPressureSwitch());
+
+      sim.setCompressorCurrent(35.04);
+      assertEquals(35.04, sim.getCompressorCurrent());
+      assertEquals(35.04, pcm.getCompressorCurrent());
+      assertTrue(callback.wasTriggered());
+      assertEquals(35.04, callback.getSetValue());
+    }
+  }
+}

wpilibj/src/test/java/org/wpilib/simulation/DCMotorSimTest.java

@@ -0,0 +1,92 @@
+// 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.simulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX;
+import org.junit.jupiter.api.Test;
+
+class DCMotorSimTest {
+  @Test
+  void testVoltageSteadyState() {
+    RoboRioSim.resetData();
+
+    DCMotor gearbox = DCMotor.getNEO(1);
+    LinearSystem<N2, N1, N2> plant = LinearSystemId.createDCMotorSystem(gearbox, 0.0005, 1);
+    DCMotorSim sim = new DCMotorSim(plant, gearbox);
+
+    try (var motor = new PWMVictorSPX(0);
+        var encoder = new Encoder(0, 1)) {
+      var encoderSim = new EncoderSim(encoder);
+      encoderSim.resetData();
+
+      for (int i = 0; i < 100; i++) {
+        motor.setVoltage(12);
+
+        // ------ SimulationPeriodic() happens after user code -------
+        RoboRioSim.setVInVoltage(
+            BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDraw()));
+        sim.setInputVoltage(motor.get() * RobotController.getBatteryVoltage());
+        sim.update(0.020);
+        encoderSim.setRate(sim.getAngularVelocity());
+      }
+
+      assertEquals(gearbox.Kv * 12, encoder.getRate(), 0.1);
+
+      for (int i = 0; i < 100; i++) {
+        motor.setVoltage(0);
+
+        // ------ SimulationPeriodic() happens after user code -------
+        RoboRioSim.setVInVoltage(
+            BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDraw()));
+        sim.setInputVoltage(motor.get() * RobotController.getBatteryVoltage());
+        sim.update(0.020);
+        encoderSim.setRate(sim.getAngularVelocity());
+      }
+
+      assertEquals(0, encoder.getRate(), 0.1);
+    }
+  }
+
+  @Test
+  void testPositionFeedbackControl() {
+    RoboRioSim.resetData();
+
+    DCMotor gearbox = DCMotor.getNEO(1);
+    LinearSystem<N2, N1, N2> plant = LinearSystemId.createDCMotorSystem(gearbox, 0.0005, 1);
+    DCMotorSim sim = new DCMotorSim(plant, gearbox);
+
+    try (var motor = new PWMVictorSPX(0);
+        var encoder = new Encoder(0, 1);
+        var controller = new PIDController(0.04, 0.0, 0.001)) {
+      var encoderSim = new EncoderSim(encoder);
+      encoderSim.resetData();
+
+      for (int i = 0; i < 140; i++) {
+        motor.set(controller.calculate(encoder.getDistance(), 750));
+
+        // ------ SimulationPeriodic() happens after user code -------
+        RoboRioSim.setVInVoltage(
+            BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDraw()));
+        sim.setInputVoltage(motor.get() * RobotController.getBatteryVoltage());
+        sim.update(0.020);
+        encoderSim.setDistance(sim.getAngularPosition());
+        encoderSim.setRate(sim.getAngularVelocity());
+      }
+
+      assertEquals(750, encoder.getDistance(), 1.0);
+      assertEquals(0, encoder.getRate(), 0.1);
+    }
+  }
+}

wpilibj/src/test/java/org/wpilib/simulation/DIOSimTest.java

@@ -0,0 +1,85 @@
+// 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.simulation;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DigitalOutput;
+import edu.wpi.first.wpilibj.simulation.testutils.BooleanCallback;
+import org.junit.jupiter.api.Test;
+
+class DIOSimTest {
+  @Test
+  void testInitialization() {
+    DIOSim sim = new DIOSim(2);
+    assertFalse(sim.getInitialized());
+
+    BooleanCallback initializedCallback = new BooleanCallback();
+    BooleanCallback isInputCallback = new BooleanCallback();
+
+    try (CallbackStore initializeCb = sim.registerInitializedCallback(initializedCallback, false);
+        CallbackStore inputCb = sim.registerIsInputCallback(isInputCallback, false);
+        DigitalOutput output = new DigitalOutput(2)) {
+      assertTrue(sim.getInitialized());
+      assertTrue(initializedCallback.wasTriggered());
+      assertTrue(initializedCallback.getSetValue());
+
+      assertFalse(sim.getIsInput());
+      assertTrue(isInputCallback.wasTriggered());
+      assertFalse(isInputCallback.getSetValue());
+
+      initializedCallback.reset();
+      sim.setInitialized(false);
+      assertTrue(initializedCallback.wasTriggered());
+      assertFalse(initializedCallback.getSetValue());
+    }
+  }
+
+  @Test
+  void testInput() {
+    HAL.initialize(500, 0);
+
+    try (DigitalInput input = new DigitalInput(0)) {
+      DIOSim sim = new DIOSim(input);
+      assertTrue(sim.getIsInput());
+
+      BooleanCallback valueCallback = new BooleanCallback();
+
+      try (CallbackStore cb = sim.registerValueCallback(valueCallback, false)) {
+        assertTrue(input.get());
+        assertTrue(sim.getValue());
+
+        assertFalse(valueCallback.wasTriggered());
+        sim.setValue(false);
+        assertTrue(valueCallback.wasTriggered());
+        assertFalse(valueCallback.getSetValue());
+      }
+    }
+  }
+
+  @Test
+  void testOutput() {
+    HAL.initialize(500, 0);
+    try (DigitalOutput output = new DigitalOutput(0)) {
+      DIOSim sim = new DIOSim(output);
+      assertFalse(sim.getIsInput());
+
+      BooleanCallback valueCallback = new BooleanCallback();
+
+      try (CallbackStore cb = sim.registerValueCallback(valueCallback, false)) {
+        assertTrue(output.get());
+        assertTrue(sim.getValue());
+
+        assertFalse(valueCallback.wasTriggered());
+        output.set(false);
+        assertTrue(valueCallback.wasTriggered());
+        assertFalse(valueCallback.getSetValue());
+      }
+    }
+  }
+}

wpilibj/src/test/java/org/wpilib/simulation/DifferentialDrivetrainSimTest.java

@@ -0,0 +1,152 @@
+// 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.simulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.controller.LTVUnicycleController;
+import edu.wpi.first.math.controller.LinearPlantInversionFeedforward;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N7;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import edu.wpi.first.math.trajectory.constraint.DifferentialDriveKinematicsConstraint;
+import edu.wpi.first.math.util.Units;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class DifferentialDrivetrainSimTest {
+  @Test
+  void testConvergence() {
+    var motor = DCMotor.getNEO(2);
+    var plant =
+        LinearSystemId.createDrivetrainVelocitySystem(
+            motor, 50, Units.inchesToMeters(2), Units.inchesToMeters(12), 0.5, 1.0);
+
+    var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24));
+    var sim =
+        new DifferentialDrivetrainSim(
+            plant,
+            motor,
+            1,
+            kinematics.trackwidth,
+            Units.inchesToMeters(2),
+            VecBuilder.fill(0, 0, 0.0001, 0.1, 0.1, 0.005, 0.005));
+
+    var feedforward = new LinearPlantInversionFeedforward<>(plant, 0.020);
+    var feedback = new LTVUnicycleController(0.020);
+    feedforward.reset(VecBuilder.fill(0, 0));
+
+    // ground truth
+    Matrix<N7, N1> groundTruthX = new Vector<>(Nat.N7());
+
+    var traj =
+        TrajectoryGenerator.generateTrajectory(
+            Pose2d.kZero,
+            List.of(),
+            new Pose2d(2, 2, Rotation2d.kZero),
+            new TrajectoryConfig(1, 1)
+                .addConstraint(new DifferentialDriveKinematicsConstraint(kinematics, 1)));
+
+    for (double t = 0; t < traj.getTotalTime(); t += 0.020) {
+      var state = traj.sample(t);
+      var feedbackOut = feedback.calculate(sim.getPose(), state);
+
+      var wheelSpeeds = kinematics.toWheelSpeeds(feedbackOut);
+
+      var voltages = feedforward.calculate(VecBuilder.fill(wheelSpeeds.left, wheelSpeeds.right));
+
+      // Sim periodic code
+      sim.setInputs(voltages.get(0, 0), voltages.get(1, 0));
+      sim.update(0.020);
+
+      // Update our ground truth
+      groundTruthX = NumericalIntegration.rkdp(sim::getDynamics, groundTruthX, voltages, 0.020);
+    }
+
+    // 2 inch tolerance is OK since our ground truth is an approximation of the
+    // ODE solution using RKDP anyway
+    assertEquals(
+        groundTruthX.get(DifferentialDrivetrainSim.State.kX.value, 0),
+        sim.getState(DifferentialDrivetrainSim.State.kX),
+        0.05);
+    assertEquals(
+        groundTruthX.get(DifferentialDrivetrainSim.State.kY.value, 0),
+        sim.getState(DifferentialDrivetrainSim.State.kY),
+        0.05);
+    assertEquals(
+        groundTruthX.get(DifferentialDrivetrainSim.State.kHeading.value, 0),
+        sim.getState(DifferentialDrivetrainSim.State.kHeading),
+        0.01);
+  }
+
+  @Test
+  void testCurrent() {
+    var motor = DCMotor.getNEO(2);
+    var plant =
+        LinearSystemId.createDrivetrainVelocitySystem(
+            motor, 50, Units.inchesToMeters(2), Units.inchesToMeters(12), 0.5, 1.0);
+    var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24));
+    var sim =
+        new DifferentialDrivetrainSim(
+            plant, motor, 1, kinematics.trackwidth, Units.inchesToMeters(2), null);
+
+    sim.setInputs(-12, -12);
+    for (int i = 0; i < 10; i++) {
+      sim.update(0.020);
+    }
+    assertTrue(sim.getCurrentDraw() > 0);
+
+    sim.setInputs(12, 12);
+    for (int i = 0; i < 20; i++) {
+      sim.update(0.020);
+    }
+    assertTrue(sim.getCurrentDraw() > 0);
+
+    sim.setInputs(-12, 12);
+    for (int i = 0; i < 30; i++) {
+      sim.update(0.020);
+    }
+    assertTrue(sim.getCurrentDraw() > 0);
+  }
+
+  @Test
+  void testModelStability() {
+    var motor = DCMotor.getNEO(2);
+    var plant =
+        LinearSystemId.createDrivetrainVelocitySystem(
+            motor, 50, Units.inchesToMeters(2), Units.inchesToMeters(12), 2.0, 5.0);
+
+    var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24));
+    var sim =
+        new DifferentialDrivetrainSim(
+            plant,
+            motor,
+            5,
+            kinematics.trackwidth,
+            Units.inchesToMeters(2),
+            VecBuilder.fill(0, 0, 0.0001, 0.1, 0.1, 0.005, 0.005));
+
+    sim.setInputs(2, 4);
+
+    // 10 seconds should be enough time to verify stability
+    for (int i = 0; i < 500; i++) {
+      sim.update(0.020);
+    }
+
+    assertTrue(Math.abs(sim.getPose().getTranslation().getNorm()) < 100);
+  }
+}

wpilibj/src/test/java/org/wpilib/simulation/DigitalPWMSimTest.java

@@ -0,0 +1,58 @@
+// 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.simulation;
+
+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.HAL;
+import edu.wpi.first.wpilibj.DigitalOutput;
+import edu.wpi.first.wpilibj.simulation.testutils.BooleanCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.DoubleCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.IntCallback;
+import org.junit.jupiter.api.Test;
+
+class DigitalPWMSimTest {
+  @Test
+  void testInitialization() {
+    try (DigitalOutput output = new DigitalOutput(0)) {
+      DigitalPWMSim sim = new DigitalPWMSim(output);
+      assertFalse(sim.getInitialized());
+
+      BooleanCallback initializeCallback = new BooleanCallback();
+      DoubleCallback dutyCycleCallback = new DoubleCallback();
+      try (CallbackStore initCb = sim.registerInitializedCallback(initializeCallback, false);
+          CallbackStore dutyCycleCb = sim.registerDutyCycleCallback(dutyCycleCallback, false)) {
+        final double kTestDutyCycle = 0.191;
+        output.enablePWM(kTestDutyCycle);
+
+        assertTrue(sim.getInitialized());
+        assertTrue(initializeCallback.wasTriggered());
+        assertTrue(initializeCallback.getSetValue());
+
+        assertEquals(kTestDutyCycle, sim.getDutyCycle());
+        assertTrue(dutyCycleCallback.wasTriggered());
+        assertEquals(kTestDutyCycle, dutyCycleCallback.getSetValue());
+      }
+    }
+  }
+
+  @Test
+  void setPinTest() {
+    HAL.initialize(500, 0);
+
+    try (DigitalOutput output = new DigitalOutput(0)) {
+      DigitalPWMSim sim = new DigitalPWMSim(output);
+      IntCallback callback = new IntCallback();
+      try (CallbackStore cb = sim.registerPinCallback(callback, false)) {
+        sim.setPin(191);
+        assertEquals(191, sim.getPin());
+        assertTrue(callback.wasTriggered());
+        assertEquals(191, callback.getSetValue());
+      }
+    }
+  }
+}

wpilibj/src/test/java/org/wpilib/simulation/DriverStationSimTest.java

@@ -0,0 +1,282 @@
+// 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.simulation;
+
+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.DriverStation;
+import edu.wpi.first.wpilibj.simulation.testutils.BooleanCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.DoubleCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.EnumCallback;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.EnumSource;
+
+class DriverStationSimTest {
+  @Test
+  void testEnabled() {
+    HAL.initialize(500, 0);
+    DriverStationSim.resetData();
+
+    assertFalse(DriverStation.isEnabled());
+    BooleanCallback callback = new BooleanCallback();
+    try (CallbackStore cb = DriverStationSim.registerEnabledCallback(callback, false)) {
+      DriverStationSim.setEnabled(true);
+      DriverStationSim.notifyNewData();
+      assertTrue(DriverStationSim.getEnabled());
+      assertTrue(DriverStation.isEnabled());
+      assertTrue(callback.wasTriggered());
+      assertTrue(callback.getSetValue());
+    }
+  }
+
+  @Test
+  void testAutonomous() {
+    HAL.initialize(500, 0);
+    DriverStationSim.resetData();
+
+    assertFalse(DriverStation.isAutonomous());
+    BooleanCallback callback = new BooleanCallback();
+    try (CallbackStore cb = DriverStationSim.registerAutonomousCallback(callback, false)) {
+      DriverStationSim.setAutonomous(true);
+      DriverStationSim.notifyNewData();
+      assertTrue(DriverStationSim.getAutonomous());
+      assertTrue(DriverStation.isAutonomous());
+      assertTrue(callback.wasTriggered());
+      assertTrue(callback.getSetValue());
+    }
+  }
+
+  @Test
+  void testTest() {
+    HAL.initialize(500, 0);
+    DriverStationSim.resetData();
+
+    assertFalse(DriverStation.isTest());
+    BooleanCallback callback = new BooleanCallback();
+    try (CallbackStore cb = DriverStationSim.registerTestCallback(callback, false)) {
+      DriverStationSim.setTest(true);
+      DriverStationSim.notifyNewData();
+      assertTrue(DriverStationSim.getTest());
+      assertTrue(DriverStation.isTest());
+      assertTrue(callback.wasTriggered());
+      assertTrue(callback.getSetValue());
+    }
+  }
+
+  @Test
+  void testEstop() {
+    HAL.initialize(500, 0);
+    DriverStationSim.resetData();
+
+    assertFalse(DriverStation.isEStopped());
+    BooleanCallback callback = new BooleanCallback();
+    try (CallbackStore cb = DriverStationSim.registerEStopCallback(callback, false)) {
+      DriverStationSim.setEStop(true);
+      DriverStationSim.notifyNewData();
+      assertTrue(DriverStationSim.getEStop());
+      assertTrue(DriverStation.isEStopped());
+      assertTrue(callback.wasTriggered());
+      assertTrue(callback.getSetValue());
+    }
+  }
+
+  @Test
+  void testFmsAttached() {
+    HAL.initialize(500, 0);
+    DriverStationSim.resetData();
+
+    assertFalse(DriverStation.isFMSAttached());
+    BooleanCallback callback = new BooleanCallback();
+    try (CallbackStore cb = DriverStationSim.registerFmsAttachedCallback(callback, false)) {
+      DriverStationSim.setFmsAttached(true);
+      DriverStationSim.notifyNewData();
+      assertTrue(DriverStationSim.getFmsAttached());
+      assertTrue(DriverStation.isFMSAttached());
+      assertTrue(callback.wasTriggered());
+      assertTrue(callback.getSetValue());
+    }
+  }
+
+  @Test
+  void testDsAttached() {
+    HAL.initialize(500, 0);
+    DriverStationSim.resetData();
+    DriverStation.refreshData();
+
+    assertFalse(DriverStationSim.getDsAttached());
+    assertFalse(DriverStation.isDSAttached());
+    DriverStationSim.notifyNewData();
+    assertTrue(DriverStationSim.getDsAttached());
+    assertTrue(DriverStation.isDSAttached());
+
+    BooleanCallback callback = new BooleanCallback();
+    try (CallbackStore cb = DriverStationSim.registerDsAttachedCallback(callback, false)) {
+      DriverStationSim.setDsAttached(false);
+      DriverStation.refreshData();
+      assertFalse(DriverStationSim.getDsAttached());
+      assertFalse(DriverStation.isDSAttached());
+      assertTrue(callback.wasTriggered());
+      assertFalse(callback.getSetValue());
+    }
+  }
+
+  @Test
+  void testAllianceStationId() {
+    HAL.initialize(500, 0);
+    DriverStationSim.resetData();
+
+    EnumCallback callback = new EnumCallback();
+
+    AllianceStationID allianceStation = AllianceStationID.Blue2;
+    DriverStationSim.setAllianceStationId(allianceStation);
+
+    try (CallbackStore cb = DriverStationSim.registerAllianceStationIdCallback(callback, false)) {
+      // Unknown
+      allianceStation = AllianceStationID.Unknown;
+      DriverStationSim.setAllianceStationId(allianceStation);
+      DriverStationSim.notifyNewData();
+      assertEquals(allianceStation, DriverStationSim.getAllianceStationId());
+      assertFalse(DriverStation.getAlliance().isPresent());
+      assertFalse(DriverStation.getLocation().isPresent());
+      assertTrue(callback.wasTriggered());
+      assertEquals(allianceStation.ordinal(), callback.getSetValue());
+
+      // B1
+      allianceStation = AllianceStationID.Blue1;
+      DriverStationSim.setAllianceStationId(allianceStation);
+      DriverStationSim.notifyNewData();
+      assertEquals(allianceStation, DriverStationSim.getAllianceStationId());
+      assertEquals(DriverStation.Alliance.Blue, DriverStation.getAlliance().get());
+      assertEquals(1, DriverStation.getLocation().getAsInt());
+      assertTrue(callback.wasTriggered());
+      assertEquals(allianceStation.ordinal(), callback.getSetValue());
+
+      // B2
+      allianceStation = AllianceStationID.Blue2;
+      DriverStationSim.setAllianceStationId(allianceStation);
+      DriverStationSim.notifyNewData();
+      assertEquals(allianceStation, DriverStationSim.getAllianceStationId());
+      assertEquals(DriverStation.Alliance.Blue, DriverStation.getAlliance().get());
+      assertEquals(2, DriverStation.getLocation().getAsInt());
+      assertTrue(callback.wasTriggered());
+      assertEquals(allianceStation.ordinal(), callback.getSetValue());
+
+      // B3
+      allianceStation = AllianceStationID.Blue3;
+      DriverStationSim.setAllianceStationId(allianceStation);
+      DriverStationSim.notifyNewData();
+      assertEquals(allianceStation, DriverStationSim.getAllianceStationId());
+      assertEquals(DriverStation.Alliance.Blue, DriverStation.getAlliance().get());
+      assertEquals(3, DriverStation.getLocation().getAsInt());
+      assertTrue(callback.wasTriggered());
+      assertEquals(allianceStation.ordinal(), callback.getSetValue());
+
+      // R1
+      allianceStation = AllianceStationID.Red1;
+      DriverStationSim.setAllianceStationId(allianceStation);
+      DriverStationSim.notifyNewData();
+      assertEquals(allianceStation, DriverStationSim.getAllianceStationId());
+      assertEquals(DriverStation.Alliance.Red, DriverStation.getAlliance().get());
+      assertEquals(1, DriverStation.getLocation().getAsInt());
+      assertTrue(callback.wasTriggered());
+      assertEquals(allianceStation.ordinal(), callback.getSetValue());
+
+      // R2
+      allianceStation = AllianceStationID.Red2;
+      DriverStationSim.setAllianceStationId(allianceStation);
+      DriverStationSim.notifyNewData();
+      assertEquals(allianceStation, DriverStationSim.getAllianceStationId());
+      assertEquals(DriverStation.Alliance.Red, DriverStation.getAlliance().get());
+      assertEquals(2, DriverStation.getLocation().getAsInt());
+      assertTrue(callback.wasTriggered());
+      assertEquals(allianceStation.ordinal(), callback.getSetValue());
+
+      // R3
+      allianceStation = AllianceStationID.Red3;
+      DriverStationSim.setAllianceStationId(allianceStation);
+      DriverStationSim.notifyNewData();
+      assertEquals(allianceStation, DriverStationSim.getAllianceStationId());
+      assertEquals(DriverStation.Alliance.Red, DriverStation.getAlliance().get());
+      assertEquals(3, DriverStation.getLocation().getAsInt());
+      assertTrue(callback.wasTriggered());
+      assertEquals(allianceStation.ordinal(), callback.getSetValue());
+    }
+  }
+
+  @ParameterizedTest
+  @EnumSource(DriverStation.MatchType.class)
+  void testMatchType(DriverStation.MatchType matchType) {
+    HAL.initialize(500, 0);
+    DriverStationSim.resetData();
+
+    DriverStationSim.setMatchType(matchType);
+    DriverStationSim.notifyNewData();
+    assertEquals(matchType, DriverStation.getMatchType());
+  }
+
+  @Test
+  void testReplayNumber() {
+    HAL.initialize(500, 0);
+    DriverStationSim.resetData();
+
+    DriverStationSim.setReplayNumber(4);
+    DriverStationSim.notifyNewData();
+    assertEquals(4, DriverStation.getReplayNumber());
+  }
+
+  @Test
+  void testMatchNumber() {
+    HAL.initialize(500, 0);
+    DriverStationSim.resetData();
+
+    DriverStationSim.setMatchNumber(3);
+    DriverStationSim.notifyNewData();
+    assertEquals(3, DriverStation.getMatchNumber());
+  }
+
+  @Test
+  void testMatchTime() {
+    HAL.initialize(500, 0);
+    DriverStationSim.resetData();
+
+    DoubleCallback callback = new DoubleCallback();
+    try (CallbackStore cb = DriverStationSim.registerMatchTimeCallback(callback, false)) {
+      final double testTime = 19.174;
+      DriverStationSim.setMatchTime(testTime);
+      DriverStationSim.notifyNewData();
+      assertEquals(testTime, DriverStationSim.getMatchTime());
+      assertEquals(testTime, DriverStation.getMatchTime());
+      assertTrue(callback.wasTriggered());
+      assertEquals(testTime, callback.getSetValue());
+    }
+  }
+
+  @Test
+  void testSetGameSpecificMessage() {
+    HAL.initialize(500, 0);
+    DriverStationSim.resetData();
+
+    final String message = "Hello World!";
+    DriverStationSim.setGameSpecificMessage(message);
+    DriverStationSim.notifyNewData();
+    assertEquals(message, DriverStation.getGameSpecificMessage());
+  }
+
+  @Test
+  void testSetEventName() {
+    HAL.initialize(500, 0);
+    DriverStationSim.resetData();
+
+    final String message = "The Best Event";
+    DriverStationSim.setEventName(message);
+    DriverStationSim.notifyNewData();
+    assertEquals(message, DriverStation.getEventName());
+  }
+}

wpilibj/src/test/java/org/wpilib/simulation/DutyCycleEncoderSimTest.java

@@ -0,0 +1,39 @@
+// 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.simulation;
+
+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.HAL;
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
+import org.junit.jupiter.api.Test;
+
+class DutyCycleEncoderSimTest {
+  @Test
+  void setTest() {
+    try (DutyCycleEncoder encoder = new DutyCycleEncoder(0, 5.67, 0)) {
+      DutyCycleEncoderSim sim = new DutyCycleEncoderSim(encoder);
+
+      sim.set(5.67);
+      assertEquals(5.67, encoder.get());
+    }
+  }
+
+  @Test
+  void setIsConnectedTest() {
+    HAL.initialize(500, 0);
+
+    try (DutyCycleEncoder encoder = new DutyCycleEncoder(0)) {
+      DutyCycleEncoderSim sim = new DutyCycleEncoderSim(encoder);
+
+      sim.setConnected(true);
+      assertTrue(encoder.isConnected());
+      sim.setConnected(false);
+      assertFalse(encoder.isConnected());
+    }
+  }
+}

wpilibj/src/test/java/org/wpilib/simulation/DutyCycleSimTest.java

@@ -0,0 +1,66 @@
+// 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.simulation;
+
+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.HAL;
+import edu.wpi.first.wpilibj.DutyCycle;
+import edu.wpi.first.wpilibj.simulation.testutils.BooleanCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.DoubleCallback;
+import org.junit.jupiter.api.Test;
+
+class DutyCycleSimTest {
+  @Test
+  void testInitialization() {
+    DutyCycleSim sim = DutyCycleSim.createForChannel(2);
+    assertFalse(sim.getInitialized());
+
+    BooleanCallback callback = new BooleanCallback();
+
+    try (CallbackStore cb = sim.registerInitializedCallback(callback, false);
+        DutyCycle dc = new DutyCycle(2)) {
+      assertTrue(sim.getInitialized());
+      assertTrue(callback.wasTriggered());
+      assertTrue(callback.getSetValue());
+    }
+  }
+
+  @Test
+  void setFrequencyTest() {
+    HAL.initialize(500, 0);
+
+    try (DutyCycle dc = new DutyCycle(2)) {
+      DoubleCallback callback = new DoubleCallback();
+      DutyCycleSim sim = new DutyCycleSim(dc);
+      try (CallbackStore cb = sim.registerFrequencyCallback(callback, false)) {
+        sim.setFrequency(191);
+        assertEquals(191, sim.getFrequency());
+        assertEquals(191, dc.getFrequency());
+        assertTrue(callback.wasTriggered());
+        assertEquals(191, callback.getSetValue());
+      }
+    }
+  }
+
+  @Test
+  void setOutputTest() {
+    HAL.initialize(500, 0);
+
+    try (DutyCycle dc = new DutyCycle(2)) {
+      DoubleCallback callback = new DoubleCallback();
+      DutyCycleSim sim = new DutyCycleSim(dc);
+      try (CallbackStore cb = sim.registerOutputCallback(callback, false)) {
+        sim.setOutput(229.174);
+        assertEquals(229.174, sim.getOutput());
+        assertEquals(229.174, dc.getOutput());
+        assertTrue(callback.wasTriggered());
+        assertEquals(229.174, callback.getSetValue());
+      }
+    }
+  }
+}

wpilibj/src/test/java/org/wpilib/simulation/ElevatorSimTest.java

@@ -0,0 +1,127 @@
+// 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.simulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX;
+import org.junit.jupiter.api.Test;
+
+class ElevatorSimTest {
+  @Test
+  void testStateSpaceSimWithElevator() {
+    RoboRioSim.resetData();
+
+    @SuppressWarnings("resource")
+    var controller = new PIDController(10, 0, 0);
+
+    var sim =
+        new ElevatorSim(
+            DCMotor.getVex775Pro(4),
+            14.67,
+            8,
+            0.75 * 25.4 / 1000.0,
+            0.0,
+            3.0,
+            true,
+            0.0,
+            0.01,
+            0.0);
+
+    try (var motor = new PWMVictorSPX(0);
+        var encoder = new Encoder(0, 1)) {
+      var encoderSim = new EncoderSim(encoder);
+
+      for (int i = 0; i < 100; i++) {
+        controller.setSetpoint(2.0);
+
+        double nextVoltage = controller.calculate(encoderSim.getDistance());
+
+        double currentBatteryVoltage = RobotController.getBatteryVoltage();
+        motor.set(nextVoltage / currentBatteryVoltage);
+
+        // ------ SimulationPeriodic() happens after user code -------
+
+        var u = VecBuilder.fill(motor.get() * currentBatteryVoltage);
+        sim.setInput(u);
+        sim.update(0.020);
+        var y = sim.getOutput();
+        encoderSim.setDistance(y.get(0, 0));
+      }
+
+      assertEquals(controller.getSetpoint(), sim.getPosition(), 0.2);
+    }
+  }
+
+  @Test
+  void testInitialState() {
+    double startingHeightMeters = 0.5;
+    var sim =
+        new ElevatorSim(
+            DCMotor.getKrakenX60(2), 20, 8.0, 0.1, 0.0, 1.0, true, startingHeightMeters, 0.01, 0.0);
+
+    assertEquals(startingHeightMeters, sim.getPosition());
+    assertEquals(0, sim.getVelocity());
+  }
+
+  @Test
+  void testMinMax() {
+    var sim =
+        new ElevatorSim(
+            DCMotor.getVex775Pro(4),
+            14.67,
+            8.0,
+            0.75 * 25.4 / 1000.0,
+            0.0,
+            1.0,
+            true,
+            0.0,
+            0.01,
+            0.0);
+
+    for (int i = 0; i < 100; i++) {
+      sim.setInput(VecBuilder.fill(0));
+      sim.update(0.020);
+      var height = sim.getPosition();
+      assertTrue(height >= -0.05);
+    }
+
+    for (int i = 0; i < 100; i++) {
+      sim.setInput(VecBuilder.fill(12.0));
+      sim.update(0.020);
+      var height = sim.getPosition();
+      assertTrue(height <= 1.05);
+    }
+  }
+
+  @Test
+  void testStability() {
+    var sim =
+        new ElevatorSim(
+            DCMotor.getVex775Pro(4), 100, 4, Units.inchesToMeters(0.5), 0, 10, false, 0.0);
+
+    sim.setState(VecBuilder.fill(0, 0));
+    sim.setInput(12);
+    for (int i = 0; i < 50; ++i) {
+      sim.update(0.02);
+    }
+
+    var system =
+        LinearSystemId.createElevatorSystem(
+            DCMotor.getVex775Pro(4), 4, Units.inchesToMeters(0.5), 100);
+    assertEquals(
+        system.calculateX(VecBuilder.fill(0, 0), VecBuilder.fill(12), 0.02 * 50.0).get(0, 0),
+        sim.getPosition(),
+        0.01);
+  }
+}

wpilibj/src/test/java/org/wpilib/simulation/EncoderSimTest.java

@@ -0,0 +1,111 @@
+// 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.simulation;
+
+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.Encoder;
+import edu.wpi.first.wpilibj.simulation.testutils.DoubleCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.IntCallback;
+import org.junit.jupiter.api.Test;
+
+class EncoderSimTest {
+  private static final double DEFAULT_DISTANCE_PER_PULSE = 0.0005;
+
+  @Test
+  void testRate() {
+    HAL.initialize(500, 0);
+
+    try (Encoder encoder = new Encoder(0, 1)) {
+      EncoderSim sim = new EncoderSim(encoder);
+      sim.resetData();
+
+      encoder.setDistancePerPulse(DEFAULT_DISTANCE_PER_PULSE);
+
+      sim.setRate(1.91);
+      assertEquals(1.91, sim.getRate());
+    }
+  }
+
+  @Test
+  void testCount() {
+    HAL.initialize(500, 0);
+
+    try (Encoder encoder = new Encoder(0, 1)) {
+      EncoderSim sim = new EncoderSim(encoder);
+      sim.resetData();
+
+      encoder.setDistancePerPulse(DEFAULT_DISTANCE_PER_PULSE);
+
+      IntCallback callback = new IntCallback();
+      try (CallbackStore cb = sim.registerCountCallback(callback, false)) {
+        sim.setCount(3504);
+        assertEquals(3504, sim.getCount());
+
+        assertTrue(callback.wasTriggered());
+        assertEquals(3504, encoder.get());
+        assertEquals(3504, callback.getSetValue());
+      }
+    }
+  }
+
+  @Test
+  void testDistance() {
+    HAL.initialize(500, 0);
+
+    try (Encoder encoder = new Encoder(0, 1)) {
+      EncoderSim sim = new EncoderSim(encoder);
+      sim.resetData();
+
+      encoder.setDistancePerPulse(DEFAULT_DISTANCE_PER_PULSE);
+
+      sim.setDistance(229.174);
+      assertEquals(229.174, sim.getDistance());
+      assertEquals(229.174, encoder.getDistance());
+    }
+  }
+
+  @SuppressWarnings("deprecation") // Encoder.getPeriod()
+  @Test
+  void testPeriod() {
+    HAL.initialize(500, 0);
+
+    try (Encoder encoder = new Encoder(0, 1)) {
+      EncoderSim sim = new EncoderSim(encoder);
+      sim.resetData();
+
+      encoder.setDistancePerPulse(DEFAULT_DISTANCE_PER_PULSE);
+
+      DoubleCallback callback = new DoubleCallback();
+      try (CallbackStore cb = sim.registerPeriodCallback(callback, false)) {
+        sim.setPeriod(123.456);
+        assertEquals(123.456, sim.getPeriod());
+        assertEquals(123.456, encoder.getPeriod());
+        assertEquals(DEFAULT_DISTANCE_PER_PULSE / 123.456, encoder.getRate());
+      }
+    }
+  }
+
+  @Test
+  void testDistancePerPulse() {
+    HAL.initialize(500, 0);
+
+    try (Encoder encoder = new Encoder(0, 1)) {
+      EncoderSim sim = new EncoderSim(encoder);
+      sim.resetData();
+
+      DoubleCallback callback = new DoubleCallback();
+      try (CallbackStore cb = sim.registerDistancePerPulseCallback(callback, false)) {
+        sim.setDistancePerPulse(0.03405);
+        assertEquals(0.03405, sim.getDistancePerPulse());
+        assertEquals(0.03405, encoder.getDistancePerPulse());
+        assertTrue(callback.wasTriggered());
+        assertEquals(0.03405, callback.getSetValue());
+      }
+    }
+  }
+}

wpilibj/src/test/java/org/wpilib/simulation/PWMMotorControllerSimTest.java

@@ -0,0 +1,31 @@
+// 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.simulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import org.junit.jupiter.api.Test;
+
+class PWMMotorControllerSimTest {
+  @Test
+  void testMotor() {
+    HAL.initialize(500, 0);
+
+    try (Spark spark = new Spark(0)) {
+      PWMMotorControllerSim sim = new PWMMotorControllerSim(spark);
+
+      spark.set(0);
+      assertEquals(0, sim.getSpeed());
+
+      spark.set(0.354);
+      assertEquals(0.354, sim.getSpeed());
+
+      spark.set(-0.785);
+      assertEquals(-0.785, sim.getSpeed());
+    }
+  }
+}

wpilibj/src/test/java/org/wpilib/simulation/PWMSimTest.java

@@ -0,0 +1,72 @@
+// 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.simulation;
+
+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.HAL;
+import edu.wpi.first.wpilibj.PWM;
+import edu.wpi.first.wpilibj.simulation.testutils.BooleanCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.IntCallback;
+import org.junit.jupiter.api.Test;
+
+class PWMSimTest {
+  @Test
+  void testInitialize() {
+    HAL.initialize(500, 0);
+
+    PWMSim sim = new PWMSim(0);
+    sim.resetData();
+    assertFalse(sim.getInitialized());
+
+    BooleanCallback callback = new BooleanCallback();
+
+    try (CallbackStore cb = sim.registerInitializedCallback(callback, false);
+        PWM pwm = new PWM(0)) {
+      assertTrue(sim.getInitialized());
+    }
+  }
+
+  @Test
+  void testSetPulseTime() {
+    HAL.initialize(500, 0);
+
+    PWMSim sim = new PWMSim(0);
+    sim.resetData();
+    assertFalse(sim.getInitialized());
+
+    IntCallback callback = new IntCallback();
+
+    try (CallbackStore cb = sim.registerPulseMicrosecondCallback(callback, false);
+        PWM pwm = new PWM(0)) {
+      sim.setPulseMicrosecond(2290);
+      assertEquals(2290, sim.getPulseMicrosecond());
+      assertEquals(2290, pwm.getPulseTimeMicroseconds());
+      assertTrue(callback.wasTriggered());
+      assertEquals(2290, callback.getSetValue());
+    }
+  }
+
+  @Test
+  void testSetOutputPeriod() {
+    HAL.initialize(500, 0);
+
+    PWMSim sim = new PWMSim(0);
+    sim.resetData();
+    assertFalse(sim.getInitialized());
+
+    IntCallback callback = new IntCallback();
+
+    try (CallbackStore cb = sim.registerOutputPeriodCallback(callback, false);
+        PWM pwm = new PWM(0)) {
+      sim.setOutputPeriod(3504);
+      assertEquals(3504, sim.getOutputPeriod());
+      assertTrue(callback.wasTriggered());
+      assertEquals(3504, callback.getSetValue());
+    }
+  }
+}

wpilibj/src/test/java/org/wpilib/simulation/REVPHSimTest.java

@@ -0,0 +1,209 @@
+// 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.simulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertNull;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.CompressorConfigType;
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.PneumaticHub;
+import edu.wpi.first.wpilibj.PneumaticsModuleType;
+import edu.wpi.first.wpilibj.simulation.testutils.BooleanCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.DoubleCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.EnumCallback;
+import org.junit.jupiter.api.Test;
+
+class REVPHSimTest {
+  @Test
+  void testInitialization() {
+    HAL.initialize(500, 0);
+
+    REVPHSim sim = new REVPHSim(1);
+    sim.resetData();
+    assertFalse(sim.getInitialized());
+
+    BooleanCallback callback = new BooleanCallback();
+
+    try (CallbackStore cb = sim.registerInitializedCallback(callback, false);
+        PneumaticHub ph = new PneumaticHub(1)) {
+      assertTrue(sim.getInitialized());
+    }
+    assertFalse(sim.getInitialized());
+  }
+
+  @Test
+  void solenoidOutputTest() {
+    HAL.initialize(500, 0);
+
+    try (PneumaticHub ph = new PneumaticHub(1);
+        DoubleSolenoid doubleSolenoid = new DoubleSolenoid(1, PneumaticsModuleType.REVPH, 3, 4)) {
+      REVPHSim sim = new REVPHSim(ph);
+      sim.resetData();
+
+      BooleanCallback callback3 = new BooleanCallback();
+      BooleanCallback callback4 = new BooleanCallback();
+
+      try (CallbackStore cb3 = sim.registerSolenoidOutputCallback(3, callback3, false);
+          CallbackStore cb4 = sim.registerSolenoidOutputCallback(4, callback4, false)) {
+        // Reverse
+        callback3.reset();
+        callback4.reset();
+        doubleSolenoid.set(DoubleSolenoid.Value.kReverse);
+        assertFalse(callback3.wasTriggered());
+        assertNull(callback3.getSetValue());
+        assertTrue(callback4.wasTriggered());
+        assertTrue(callback4.getSetValue());
+        assertFalse(sim.getSolenoidOutput(3));
+        assertTrue(sim.getSolenoidOutput(4));
+        assertEquals(0x10, ph.getSolenoids());
+
+        // Forward
+        callback3.reset();
+        callback4.reset();
+        doubleSolenoid.set(DoubleSolenoid.Value.kForward);
+        assertTrue(callback3.wasTriggered());
+        assertTrue(callback3.getSetValue());
+        assertTrue(callback4.wasTriggered());
+        assertFalse(callback4.getSetValue());
+        assertTrue(sim.getSolenoidOutput(3));
+        assertFalse(sim.getSolenoidOutput(4));
+        assertEquals(0x8, ph.getSolenoids());
+
+        // Off
+        callback3.reset();
+        callback4.reset();
+        doubleSolenoid.set(DoubleSolenoid.Value.kOff);
+        assertTrue(callback3.wasTriggered());
+        assertFalse(callback3.getSetValue());
+        assertFalse(callback4.wasTriggered());
+        assertNull(callback4.getSetValue());
+        assertFalse(sim.getSolenoidOutput(3));
+        assertFalse(sim.getSolenoidOutput(4));
+        assertEquals(0x0, ph.getSolenoids());
+      }
+    }
+  }
+
+  @Test
+  void setCompressorOnTest() {
+    HAL.initialize(500, 0);
+
+    REVPHSim sim = new REVPHSim(1);
+    BooleanCallback callback = new BooleanCallback();
+
+    try (PneumaticHub ph = new PneumaticHub(1);
+        CallbackStore cb = sim.registerCompressorOnCallback(callback, false)) {
+      assertFalse(ph.getCompressor());
+      assertFalse(sim.getCompressorOn());
+      sim.setCompressorOn(true);
+      assertTrue(ph.getCompressor());
+      assertTrue(sim.getCompressorOn());
+      assertTrue(callback.wasTriggered());
+      assertTrue(callback.getSetValue());
+    }
+  }
+
+  @Test
+  void setEnableDigital() {
+    HAL.initialize(500, 0);
+
+    REVPHSim sim = new REVPHSim(1);
+    EnumCallback callback = new EnumCallback();
+
+    try (PneumaticHub ph = new PneumaticHub(1);
+        CallbackStore cb = sim.registerCompressorConfigTypeCallback(callback, false)) {
+      ph.disableCompressor();
+      assertEquals(ph.getCompressorConfigType(), CompressorConfigType.Disabled);
+
+      ph.enableCompressorDigital();
+      assertEquals(sim.getCompressorConfigType(), CompressorConfigType.Digital.getValue());
+      assertEquals(ph.getCompressorConfigType(), CompressorConfigType.Digital);
+      assertTrue(callback.wasTriggered());
+      assertEquals(callback.getSetValue(), CompressorConfigType.Digital.getValue());
+    }
+  }
+
+  @Test
+  void setEnableAnalog() {
+    HAL.initialize(500, 0);
+
+    REVPHSim sim = new REVPHSim(1);
+    EnumCallback callback = new EnumCallback();
+
+    try (PneumaticHub ph = new PneumaticHub(1);
+        CallbackStore cb = sim.registerCompressorConfigTypeCallback(callback, false)) {
+      ph.disableCompressor();
+      assertEquals(ph.getCompressorConfigType(), CompressorConfigType.Disabled);
+
+      ph.enableCompressorAnalog(1, 2);
+      assertEquals(sim.getCompressorConfigType(), CompressorConfigType.Analog.getValue());
+      assertEquals(ph.getCompressorConfigType(), CompressorConfigType.Analog);
+      assertTrue(callback.wasTriggered());
+      assertEquals(callback.getSetValue(), CompressorConfigType.Analog.getValue());
+    }
+  }
+
+  @Test
+  void setEnableHybrid() {
+    HAL.initialize(500, 0);
+
+    REVPHSim sim = new REVPHSim(1);
+    EnumCallback callback = new EnumCallback();
+
+    try (PneumaticHub ph = new PneumaticHub(1);
+        CallbackStore cb = sim.registerCompressorConfigTypeCallback(callback, false)) {
+      ph.disableCompressor();
+      assertEquals(ph.getCompressorConfigType(), CompressorConfigType.Disabled);
+
+      ph.enableCompressorHybrid(1, 2);
+      assertEquals(sim.getCompressorConfigType(), CompressorConfigType.Hybrid.getValue());
+      assertEquals(ph.getCompressorConfigType(), CompressorConfigType.Hybrid);
+      assertTrue(callback.wasTriggered());
+      assertEquals(callback.getSetValue(), CompressorConfigType.Hybrid.getValue());
+    }
+  }
+
+  @Test
+  void setPressureSwitchEnabledTest() {
+    HAL.initialize(500, 0);
+
+    REVPHSim sim = new REVPHSim(1);
+    BooleanCallback callback = new BooleanCallback();
+
+    try (PneumaticHub ph = new PneumaticHub(1);
+        CallbackStore cb = sim.registerPressureSwitchCallback(callback, false)) {
+      assertFalse(ph.getPressureSwitch());
+
+      sim.setPressureSwitch(true);
+      assertTrue(sim.getPressureSwitch());
+      assertTrue(ph.getPressureSwitch());
+      assertTrue(callback.wasTriggered());
+      assertTrue(callback.getSetValue());
+    }
+  }
+
+  @Test
+  void setCompressorCurrentTest() {
+    HAL.initialize(500, 0);
+
+    REVPHSim sim = new REVPHSim(1);
+    DoubleCallback callback = new DoubleCallback();
+
+    try (PneumaticHub ph = new PneumaticHub(1);
+        CallbackStore cb = sim.registerCompressorCurrentCallback(callback, false)) {
+      assertFalse(ph.getPressureSwitch());
+
+      sim.setCompressorCurrent(35.04);
+      assertEquals(35.04, sim.getCompressorCurrent());
+      assertEquals(35.04, ph.getCompressorCurrent());
+      assertTrue(callback.wasTriggered());
+      assertEquals(35.04, callback.getSetValue());
+    }
+  }
+}

wpilibj/src/test/java/org/wpilib/simulation/RoboRioSimTest.java

@@ -0,0 +1,165 @@
+// 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.simulation;
+
+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.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.simulation.testutils.BooleanCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.DoubleCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.IntCallback;
+import org.junit.jupiter.api.Test;
+
+class RoboRioSimTest {
+  @Test
+  void testSetVin() {
+    RoboRioSim.resetData();
+
+    DoubleCallback voltageCallback = new DoubleCallback();
+    try (CallbackStore voltageCb = RoboRioSim.registerVInVoltageCallback(voltageCallback, false)) {
+      final double kTestVoltage = 1.91;
+
+      RoboRioSim.setVInVoltage(kTestVoltage);
+      assertTrue(voltageCallback.wasTriggered());
+      assertEquals(kTestVoltage, voltageCallback.getSetValue());
+      assertEquals(kTestVoltage, RoboRioSim.getVInVoltage());
+      assertEquals(kTestVoltage, RobotController.getInputVoltage());
+    }
+  }
+
+  @Test
+  void testSetBrownout() {
+    RoboRioSim.resetData();
+
+    DoubleCallback voltageCallback = new DoubleCallback();
+    try (CallbackStore voltageCb =
+        RoboRioSim.registerBrownoutVoltageCallback(voltageCallback, false)) {
+      final double kTestVoltage = 1.91;
+
+      RoboRioSim.setBrownoutVoltage(kTestVoltage);
+      assertTrue(voltageCallback.wasTriggered());
+      assertEquals(kTestVoltage, voltageCallback.getSetValue());
+      assertEquals(kTestVoltage, RoboRioSim.getBrownoutVoltage());
+      assertEquals(kTestVoltage, RobotController.getBrownoutVoltage());
+    }
+  }
+
+  @Test
+  void test3V3() {
+    RoboRioSim.resetData();
+
+    DoubleCallback voltageCallback = new DoubleCallback();
+    DoubleCallback currentCallback = new DoubleCallback();
+    BooleanCallback activeCallback = new BooleanCallback();
+    IntCallback faultCallback = new IntCallback();
+    try (CallbackStore voltageCb =
+            RoboRioSim.registerUserVoltage3V3Callback(voltageCallback, false);
+        CallbackStore currentCb =
+            RoboRioSim.registerUserCurrent3V3Callback(currentCallback, false);
+        CallbackStore activeCb = RoboRioSim.registerUserActive3V3Callback(activeCallback, false);
+        CallbackStore faultsCb = RoboRioSim.registerUserFaults3V3Callback(faultCallback, false)) {
+      final double kTestVoltage = 22.9;
+      final double kTestCurrent = 174;
+      final int kTestFaults = 229;
+
+      RoboRioSim.setUserVoltage3V3(kTestVoltage);
+      assertTrue(voltageCallback.wasTriggered());
+      assertEquals(kTestVoltage, voltageCallback.getSetValue());
+      assertEquals(kTestVoltage, RoboRioSim.getUserVoltage3V3());
+      assertEquals(kTestVoltage, RobotController.getVoltage3V3());
+
+      RoboRioSim.setUserCurrent3V3(kTestCurrent);
+      assertTrue(currentCallback.wasTriggered());
+      assertEquals(kTestCurrent, currentCallback.getSetValue());
+      assertEquals(kTestCurrent, RoboRioSim.getUserCurrent3V3());
+      assertEquals(kTestCurrent, RobotController.getCurrent3V3());
+
+      RoboRioSim.setUserActive3V3(false);
+      assertTrue(activeCallback.wasTriggered());
+      assertFalse(activeCallback.getSetValue());
+      assertFalse(RoboRioSim.getUserActive3V3());
+      assertFalse(RobotController.getEnabled3V3());
+
+      RoboRioSim.setUserFaults3V3(kTestFaults);
+      assertTrue(faultCallback.wasTriggered());
+      assertEquals(kTestFaults, faultCallback.getSetValue());
+      assertEquals(kTestFaults, RoboRioSim.getUserFaults3V3());
+      assertEquals(kTestFaults, RobotController.getFaultCount3V3());
+    }
+  }
+
+  @Test
+  void testCPUTemp() {
+    RoboRioSim.resetData();
+
+    DoubleCallback callback = new DoubleCallback();
+
+    try (CallbackStore cb = RoboRioSim.registerCPUTempCallback(callback, false)) {
+      final double kCPUTemp = 100.0;
+
+      RoboRioSim.setCPUTemp(kCPUTemp);
+      assertTrue(callback.wasTriggered());
+      assertEquals(kCPUTemp, callback.getSetValue());
+      assertEquals(kCPUTemp, RoboRioSim.getCPUTemp());
+      assertEquals(kCPUTemp, RobotController.getCPUTemp());
+    }
+  }
+
+  @Test
+  void testTeamNumber() {
+    RoboRioSim.resetData();
+
+    IntCallback callback = new IntCallback();
+
+    try (CallbackStore cb = RoboRioSim.registerTeamNumberCallback(callback, false)) {
+      final int kTeamNumber = 9999;
+
+      RoboRioSim.setTeamNumber(kTeamNumber);
+      assertTrue(callback.wasTriggered());
+      assertEquals(kTeamNumber, callback.getSetValue());
+      assertEquals(kTeamNumber, RoboRioSim.getTeamNumber());
+      assertEquals(kTeamNumber, RobotController.getTeamNumber());
+    }
+  }
+
+  @Test
+  void testSerialNumber() {
+    RoboRioSim.resetData();
+
+    final String kSerialNumber = "Hello";
+
+    RoboRioSim.setSerialNumber(kSerialNumber);
+    assertEquals(kSerialNumber, RoboRioSim.getSerialNumber());
+    assertEquals(kSerialNumber, RobotController.getSerialNumber());
+
+    // Make sure it truncates at 8 characters properly
+    final String kSerialNumberOverflow = "SerialNumber";
+    final String kSerialNumberTruncated = kSerialNumberOverflow.substring(0, 8);
+    RoboRioSim.setSerialNumber(kSerialNumberOverflow);
+    assertEquals(kSerialNumberTruncated, RoboRioSim.getSerialNumber());
+    assertEquals(kSerialNumberTruncated, RobotController.getSerialNumber());
+  }
+
+  @Test
+  void testComments() {
+    RoboRioSim.resetData();
+
+    final String kComments = "Hello! These are comments in the roboRIO web interface!";
+
+    RoboRioSim.setComments(kComments);
+    assertEquals(kComments, RoboRioSim.getComments());
+    assertEquals(kComments, RobotController.getComments());
+
+    final String kCommentsOverflow =
+        "Hello! These are comments in the roboRIO web interface!"
+            + " This comment exceeds 64 characters!";
+    final String kCommentsTruncated = kCommentsOverflow.substring(0, 64);
+    RoboRioSim.setComments(kCommentsOverflow);
+    assertEquals(kCommentsTruncated, RoboRioSim.getComments());
+    assertEquals(kCommentsTruncated, RobotController.getComments());
+  }
+}

wpilibj/src/test/java/org/wpilib/simulation/SimDeviceSimTest.java

@@ -0,0 +1,154 @@
+// 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.simulation;
+
+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.SimBoolean;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDevice.Direction;
+import edu.wpi.first.hal.SimValue;
+import java.util.concurrent.atomic.AtomicInteger;
+import org.junit.jupiter.api.Test;
+
+class SimDeviceSimTest {
+  @Test
+  void testBasic() {
+    try (SimDevice dev = SimDevice.create("test")) {
+      SimBoolean devBool = dev.createBoolean("bool", Direction.kBidir, false);
+
+      SimDeviceSim sim = new SimDeviceSim("test");
+      SimBoolean simBool = sim.getBoolean("bool");
+
+      assertFalse(simBool.get());
+      simBool.set(true);
+      assertTrue(devBool.get());
+
+      assertEquals(dev.getName(), "test");
+    }
+  }
+
+  @Test
+  void testDeviceCreatedCallback() {
+    AtomicInteger callback1Counter = new AtomicInteger(0);
+    AtomicInteger callback2Counter = new AtomicInteger(0);
+
+    try (SimDevice otherdev = SimDevice.create("testnotDC");
+        SimDevice dev1 = SimDevice.create("testDC1")) {
+      try (CallbackStore callback1 =
+              SimDeviceSim.registerDeviceCreatedCallback(
+                  "testDC", (name, handle) -> callback1Counter.addAndGet(1), false);
+          CallbackStore callback2 =
+              SimDeviceSim.registerDeviceCreatedCallback(
+                  "testDC", (name, handle) -> callback2Counter.addAndGet(1), true)) {
+        assertEquals(0, callback1Counter.get(), "Callback 1 called early");
+        assertEquals(
+            1,
+            callback2Counter.get(),
+            "Callback 2 called early or not initialized with existing devices");
+
+        SimDevice dev2 = SimDevice.create("testDC2");
+        dev2.close();
+
+        assertEquals(
+            1, callback1Counter.get(), "Callback 1 called either more than once or not at all");
+        assertEquals(2, callback2Counter.get(), "Callback 2 called either more or less than twice");
+      }
+
+      SimDevice dev3 = SimDevice.create("testDC3");
+      dev3.close();
+
+      assertEquals(1, callback1Counter.get(), "Callback 1 called after closure");
+      assertEquals(2, callback2Counter.get(), "Callback 2 called after closure");
+    }
+  }
+
+  @Test
+  void testDeviceFreedCallback() {
+    AtomicInteger counter = new AtomicInteger(0);
+
+    SimDevice dev1 = SimDevice.create("testDF1");
+    try (CallbackStore callback =
+        SimDeviceSim.registerDeviceFreedCallback(
+            "testDF", (name, handle) -> counter.addAndGet(1), false)) {
+      assertEquals(0, counter.get(), "Callback called early");
+      dev1.close();
+      assertEquals(1, counter.get(), "Callback called either more than once or not at all");
+    }
+
+    SimDevice dev2 = SimDevice.create("testDF2");
+    dev2.close();
+
+    assertEquals(1, counter.get(), "Callback called after closure");
+  }
+
+  @Test
+  void testValueCreatedCallback() {
+    AtomicInteger callback1Counter = new AtomicInteger(0);
+    AtomicInteger callback2Counter = new AtomicInteger(0);
+
+    try (SimDevice dev1 = SimDevice.create("testVM1")) {
+      dev1.createBoolean("v1", Direction.kBidir, false);
+      SimDeviceSim sim = new SimDeviceSim("testVM1");
+      try (CallbackStore callback1 =
+              sim.registerValueCreatedCallback(
+                  (name, handle, readonly, value) -> callback1Counter.addAndGet(1), false);
+          CallbackStore callback2 =
+              sim.registerValueCreatedCallback(
+                  (name, handle, readonly, value) -> callback2Counter.addAndGet(1), true)) {
+        assertEquals(0, callback1Counter.get(), "Callback 1 called early");
+        assertEquals(
+            1,
+            callback2Counter.get(),
+            "Callback 2 called early or not initialized with existing devices");
+
+        dev1.createDouble("v2", Direction.kBidir, 0);
+
+        assertEquals(
+            1, callback1Counter.get(), "Callback 1 called either more than once or not at all");
+        assertEquals(2, callback2Counter.get(), "Callback 2 called either more or less than twice");
+      }
+      dev1.createBoolean("v3", Direction.kBidir, false);
+
+      assertEquals(1, callback1Counter.get(), "Callback 1 called after closure");
+      assertEquals(2, callback2Counter.get(), "Callback 2 called after closure");
+    }
+  }
+
+  @Test
+  void testValueChangedCallback() {
+    AtomicInteger callback1Counter = new AtomicInteger(0);
+    AtomicInteger callback2Counter = new AtomicInteger(0);
+
+    try (SimDevice dev1 = SimDevice.create("testVM1")) {
+      SimBoolean val = dev1.createBoolean("v1", Direction.kBidir, false);
+      SimDeviceSim sim = new SimDeviceSim("testVM1");
+      SimValue simVal = sim.getValue("v1");
+      try (CallbackStore callback1 =
+              sim.registerValueChangedCallback(
+                  simVal, (name, handle, readonly, value) -> callback1Counter.addAndGet(1), false);
+          CallbackStore callback2 =
+              sim.registerValueChangedCallback(
+                  simVal, (name, handle, readonly, value) -> callback2Counter.addAndGet(1), true)) {
+        assertEquals(0, callback1Counter.get(), "Callback 1 called early");
+        assertEquals(
+            1,
+            callback2Counter.get(),
+            "Callback 2 called early or not initialized with existing devices");
+
+        val.set(true);
+
+        assertEquals(
+            1, callback1Counter.get(), "Callback 1 called either more than once or not at all");
+        assertEquals(2, callback2Counter.get(), "Callback 2 called either more or less than twice");
+      }
+      val.set(false);
+      assertEquals(1, callback1Counter.get(), "Callback 1 called after closure");
+      assertEquals(2, callback2Counter.get(), "Callback 2 called after closure");
+    }
+  }
+}

wpilibj/src/test/java/org/wpilib/simulation/SingleJointedArmSimTest.java

@@ -0,0 +1,57 @@
+// 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.simulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
+import org.junit.jupiter.api.Test;
+
+class SingleJointedArmSimTest {
+  @Test
+  void testArmDisabled() {
+    SingleJointedArmSim sim =
+        new SingleJointedArmSim(
+            DCMotor.getVex775Pro(2),
+            300,
+            3.0,
+            Units.inchesToMeters(30.0),
+            -Math.PI,
+            0.0,
+            true,
+            Math.PI / 2.0);
+
+    // Reset Arm angle to 0
+    sim.setState(VecBuilder.fill(0.0, 0.0));
+
+    for (int i = 0; i < 12 / 0.02; i++) {
+      sim.setInput(0.0);
+      sim.update(0.020);
+    }
+
+    // the arm should swing down
+    assertEquals(-Math.PI / 2.0, sim.getAngle(), 0.1);
+  }
+
+  @Test
+  void testInitialState() {
+    double startingAngleRads = Math.PI / 4.0;
+    SingleJointedArmSim sim =
+        new SingleJointedArmSim(
+            DCMotor.getKrakenX60(2),
+            125,
+            3.0,
+            Units.inchesToMeters(30.0),
+            0,
+            Math.PI / 2.0,
+            true,
+            startingAngleRads);
+
+    assertEquals(startingAngleRads, sim.getAngle());
+    assertEquals(0, sim.getVelocity());
+  }
+}

wpilibj/src/test/java/org/wpilib/simulation/testutils/BooleanCallback.java

@@ -0,0 +1,19 @@
+// 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.simulation.testutils;
+
+import edu.wpi.first.hal.HALValue;
+
+public class BooleanCallback extends CallbackHelperBase<Boolean> {
+  @Override
+  public void callback(String name, HALValue value) {
+    if (value.getType() != HALValue.kBoolean) {
+      throw new IllegalArgumentException("Wrong callback for type " + value.getType());
+    }
+
+    m_wasTriggered = true;
+    m_setValue = value.getBoolean();
+  }
+}

wpilibj/src/test/java/org/wpilib/simulation/testutils/BufferCallback.java

@@ -0,0 +1,27 @@
+// 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.simulation.testutils;
+
+import edu.wpi.first.hal.simulation.ConstBufferCallback;
+import java.util.Arrays;
+
+public class BufferCallback implements ConstBufferCallback {
+  private boolean m_wasTriggered;
+  private byte[] m_setValue;
+
+  @Override
+  public void callback(String name, byte[] buffer, int count) {
+    m_wasTriggered = true;
+    m_setValue = Arrays.copyOf(buffer, buffer.length);
+  }
+
+  public boolean wasTriggered() {
+    return m_wasTriggered;
+  }
+
+  public byte[] getSetValue() {
+    return Arrays.copyOf(m_setValue, m_setValue.length);
+  }
+}

wpilibj/src/test/java/org/wpilib/simulation/testutils/CallbackHelperBase.java

@@ -0,0 +1,25 @@
+// 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.simulation.testutils;
+
+import edu.wpi.first.hal.simulation.NotifyCallback;
+
+public abstract class CallbackHelperBase<T> implements NotifyCallback {
+  protected boolean m_wasTriggered;
+  protected T m_setValue;
+
+  public final boolean wasTriggered() {
+    return m_wasTriggered;
+  }
+
+  public final T getSetValue() {
+    return m_setValue;
+  }
+
+  public final void reset() {
+    m_wasTriggered = false;
+    m_setValue = null;
+  }
+}

wpilibj/src/test/java/org/wpilib/simulation/testutils/DoubleCallback.java

@@ -0,0 +1,19 @@
+// 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.simulation.testutils;
+
+import edu.wpi.first.hal.HALValue;
+
+public class DoubleCallback extends CallbackHelperBase<Double> {
+  @Override
+  public void callback(String name, HALValue value) {
+    if (value.getType() != HALValue.kDouble) {
+      throw new IllegalArgumentException("Wrong callback for type " + value.getType());
+    }
+
+    m_wasTriggered = true;
+    m_setValue = value.getDouble();
+  }
+}

wpilibj/src/test/java/org/wpilib/simulation/testutils/EnumCallback.java

@@ -0,0 +1,19 @@
+// 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.simulation.testutils;
+
+import edu.wpi.first.hal.HALValue;
+
+public class EnumCallback extends CallbackHelperBase<Long> {
+  @Override
+  public void callback(String name, HALValue value) {
+    if (value.getType() != HALValue.kEnum) {
+      throw new IllegalArgumentException("Wrong callback for type " + value.getType());
+    }
+
+    m_wasTriggered = true;
+    m_setValue = value.getLong();
+  }
+}

wpilibj/src/test/java/org/wpilib/simulation/testutils/IntCallback.java

@@ -0,0 +1,19 @@
+// 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.simulation.testutils;
+
+import edu.wpi.first.hal.HALValue;
+
+public class IntCallback extends CallbackHelperBase<Integer> {
+  @Override
+  public void callback(String name, HALValue value) {
+    if (value.getType() != HALValue.kInt) {
+      throw new IllegalArgumentException("Wrong callback for type " + value.getType());
+    }
+
+    m_wasTriggered = true;
+    m_setValue = (int) value.getLong();
+  }
+}

wpilibj/src/test/java/org/wpilib/smartdashboard/Mechanism2dTest.java

@@ -0,0 +1,99 @@
+// 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.smartdashboard;
+
+import static org.junit.jupiter.api.Assertions.assertArrayEquals;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+class Mechanism2dTest {
+  private NetworkTableInstance m_inst;
+
+  @BeforeEach
+  void setUp() {
+    m_inst = NetworkTableInstance.create();
+    SmartDashboard.setNetworkTableInstance(m_inst);
+  }
+
+  @Test
+  void testCanvas() {
+    try (var mechanism = new Mechanism2d(5, 10);
+        var dimsEntry = m_inst.getEntry("/SmartDashboard/mechanism/dims");
+        var colorEntry = m_inst.getEntry("/SmartDashboard/mechanism/backgroundColor")) {
+      SmartDashboard.putData("mechanism", mechanism);
+      SmartDashboard.updateValues();
+      assertArrayEquals(new double[] {5, 10}, dimsEntry.getDoubleArray(new double[0]));
+      assertEquals("#000020", colorEntry.getString(""));
+      mechanism.setBackgroundColor(new Color8Bit(255, 255, 255));
+      SmartDashboard.updateValues();
+      assertEquals("#FFFFFF", colorEntry.getString(""));
+    }
+  }
+
+  @Test
+  void testRoot() {
+    try (var mechanism = new Mechanism2d(5, 10);
+        var xEntry = m_inst.getEntry("/SmartDashboard/mechanism/root/x");
+        var yEntry = m_inst.getEntry("/SmartDashboard/mechanism/root/y")) {
+      final var root = mechanism.getRoot("root", 1, 2);
+      SmartDashboard.putData("mechanism", mechanism);
+      SmartDashboard.updateValues();
+      assertEquals(1, xEntry.getDouble(0));
+      assertEquals(2, yEntry.getDouble(0));
+      root.setPosition(2, 4);
+      SmartDashboard.updateValues();
+      assertEquals(2, xEntry.getDouble(0));
+      assertEquals(4, yEntry.getDouble(0));
+    }
+  }
+
+  @Test
+  void testLigament() {
+    try (var mechanism = new Mechanism2d(5, 10);
+        var angleEntry = m_inst.getEntry("/SmartDashboard/mechanism/root/ligament/angle");
+        var colorEntry = m_inst.getEntry("/SmartDashboard/mechanism/root/ligament/color");
+        var lengthEntry = m_inst.getEntry("/SmartDashboard/mechanism/root/ligament/length");
+        var weightEntry = m_inst.getEntry("/SmartDashboard/mechanism/root/ligament/weight")) {
+      var root = mechanism.getRoot("root", 1, 2);
+      var ligament =
+          root.append(new MechanismLigament2d("ligament", 3, 90, 1, new Color8Bit(255, 255, 255)));
+      SmartDashboard.putData("mechanism", mechanism);
+      SmartDashboard.updateValues();
+      assertEquals(ligament.getAngle(), angleEntry.getDouble(0));
+      assertEquals(ligament.getColor().toHexString(), colorEntry.getString(""));
+      assertEquals(ligament.getLength(), lengthEntry.getDouble(0));
+      assertEquals(ligament.getLineWeight(), weightEntry.getDouble(0));
+      ligament.setAngle(45);
+      ligament.setColor(new Color8Bit(0, 0, 0));
+      ligament.setLength(2);
+      ligament.setLineWeight(4);
+      SmartDashboard.updateValues();
+      assertEquals(ligament.getAngle(), angleEntry.getDouble(0));
+      assertEquals(ligament.getColor().toHexString(), colorEntry.getString(""));
+      assertEquals(ligament.getLength(), lengthEntry.getDouble(0));
+      assertEquals(ligament.getLineWeight(), weightEntry.getDouble(0));
+      angleEntry.setDouble(22.5);
+      colorEntry.setString("#FF00FF");
+      lengthEntry.setDouble(4);
+      weightEntry.setDouble(6);
+      SmartDashboard.updateValues();
+      assertEquals(ligament.getAngle(), angleEntry.getDouble(0));
+      assertEquals(ligament.getColor().toHexString(), colorEntry.getString(""));
+      assertEquals(ligament.getLength(), lengthEntry.getDouble(0));
+      assertEquals(ligament.getLineWeight(), weightEntry.getDouble(0));
+    }
+  }
+
+  @AfterEach
+  void tearDown() {
+    m_inst.close();
+    SmartDashboard.setNetworkTableInstance(NetworkTableInstance.getDefault());
+  }
+}

wpilibj/src/test/java/org/wpilib/smartdashboard/SendableChooserTest.java

@@ -0,0 +1,94 @@
+// 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.smartdashboard;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNull;
+
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.simulation.SendableChooserSim;
+import java.util.concurrent.atomic.AtomicInteger;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.ValueSource;
+
+class SendableChooserTest {
+  private NetworkTableInstance m_inst;
+
+  @BeforeEach
+  void setUp() {
+    m_inst = NetworkTableInstance.create();
+    SmartDashboard.setNetworkTableInstance(m_inst);
+  }
+
+  @ValueSource(ints = {0, 1, 2, 3})
+  @ParameterizedTest
+  void returnsSelected(int toSelect) {
+    try (var chooser = new SendableChooser<Integer>();
+        var chooserSim =
+            new SendableChooserSim(
+                m_inst, "/SmartDashboard/returnsSelectedChooser" + toSelect + "/")) {
+      for (int i = 1; i <= 3; i++) {
+        chooser.addOption(String.valueOf(i), i);
+      }
+      chooser.setDefaultOption(String.valueOf(0), 0);
+
+      SmartDashboard.putData("returnsSelectedChooser" + toSelect, chooser);
+      SmartDashboard.updateValues();
+      chooserSim.setSelected(String.valueOf(toSelect));
+      SmartDashboard.updateValues();
+      assertEquals(toSelect, chooser.getSelected());
+    }
+  }
+
+  @Test
+  void defaultIsReturnedOnNoSelect() {
+    try (var chooser = new SendableChooser<Integer>()) {
+      for (int i = 1; i <= 3; i++) {
+        chooser.addOption(String.valueOf(i), i);
+      }
+      chooser.setDefaultOption(String.valueOf(0), 0);
+
+      assertEquals(0, chooser.getSelected());
+    }
+  }
+
+  @Test
+  void nullIsReturnedOnNoSelectAndNoDefault() {
+    try (var chooser = new SendableChooser<Integer>()) {
+      for (int i = 1; i <= 3; i++) {
+        chooser.addOption(String.valueOf(i), i);
+      }
+
+      assertNull(chooser.getSelected());
+    }
+  }
+
+  @Test
+  void testChangeListener() {
+    try (var chooser = new SendableChooser<Integer>();
+        var chooserSim = new SendableChooserSim(m_inst, "/SmartDashboard/changeListenerChooser/")) {
+      for (int i = 1; i <= 3; i++) {
+        chooser.addOption(String.valueOf(i), i);
+      }
+      AtomicInteger currentVal = new AtomicInteger();
+      chooser.onChange(currentVal::set);
+
+      SmartDashboard.putData("changeListenerChooser", chooser);
+      SmartDashboard.updateValues();
+      chooserSim.setSelected("3");
+      SmartDashboard.updateValues();
+      assertEquals(3, currentVal.get());
+    }
+  }
+
+  @AfterEach
+  void tearDown() {
+    m_inst.close();
+    SmartDashboard.setNetworkTableInstance(NetworkTableInstance.getDefault());
+  }
+}

wpilibj/src/test/java/org/wpilib/smartdashboard/SmartDashboardTest.java

@@ -0,0 +1,125 @@
+// 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.smartdashboard;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.UtilityClassTest;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+class SmartDashboardTest extends UtilityClassTest<SmartDashboard> {
+  private NetworkTableInstance m_inst;
+  private NetworkTable m_table;
+
+  SmartDashboardTest() {
+    super(SmartDashboard.class);
+  }
+
+  @BeforeEach
+  void beforeEach() {
+    m_inst = NetworkTableInstance.create();
+    m_table = m_inst.getTable("SmartDashboard");
+    SmartDashboard.setNetworkTableInstance(m_inst);
+  }
+
+  @AfterEach
+  void afterEach() {
+    m_inst.close();
+  }
+
+  @Test
+  void getBadValueTest() {
+    assertEquals("Expected", SmartDashboard.getString("KEY_SHOULD_NOT_BE_FOUND", "Expected"));
+  }
+
+  @Test
+  void putStringTest() {
+    final String key = "putString";
+    final String value = "thisIsAValue";
+
+    SmartDashboard.putString(key, value);
+
+    assertEquals(value, m_table.getEntry(key).getString(""));
+  }
+
+  @Test
+  void getStringTest() {
+    final String key = "getString";
+    final String value = "thisIsAValue";
+
+    m_table.getEntry(key).setString(value);
+
+    assertEquals(value, SmartDashboard.getString(key, ""));
+  }
+
+  @Test
+  void putNumberTest() {
+    final String key = "PutNumber";
+    final int value = 2147483647;
+
+    SmartDashboard.putNumber(key, value);
+
+    assertEquals(value, m_table.getEntry(key).getNumber(0).intValue());
+  }
+
+  @Test
+  void getNumberTest() {
+    final String key = "GetNumber";
+    final int value = 2147483647;
+
+    m_table.getEntry(key).setNumber(value);
+
+    assertEquals(value, SmartDashboard.getNumber(key, 0), 0.01);
+  }
+
+  @Test
+  void putBooleanTest() {
+    final String key = "PutBoolean";
+    final boolean value = true;
+
+    SmartDashboard.putBoolean(key, value);
+
+    assertEquals(value, m_table.getEntry(key).getBoolean(!value));
+  }
+
+  @Test
+  void getBooleanTest() {
+    final String key = "GetBoolean";
+    final boolean value = true;
+
+    m_table.getEntry(key).setBoolean(value);
+
+    assertEquals(value, SmartDashboard.getBoolean(key, !value));
+  }
+
+  @Test
+  void testReplaceString() {
+    final String key = "testReplaceString";
+    final String valueNew = "newValue";
+
+    m_table.getEntry(key).setString("oldValue");
+    SmartDashboard.putString(key, valueNew);
+
+    assertEquals(valueNew, m_table.getEntry(key).getString(""));
+  }
+
+  @Test
+  void putStringNullKeyTest() {
+    assertThrows(
+        NullPointerException.class, () -> SmartDashboard.putString(null, "This should not work"));
+  }
+
+  @Test
+  void putStringNullValueTest() {
+    assertThrows(
+        NullPointerException.class,
+        () -> SmartDashboard.putString("KEY_SHOULD_NOT_BE_STORED", null));
+  }
+}

wpilibj/src/test/java/org/wpilib/system/NotifierTest.java

@@ -0,0 +1,63 @@
+// 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;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import java.util.concurrent.atomic.AtomicInteger;
+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;
+
+/** Tests to see if the Notifier is working properly. */
+class NotifierTest {
+  @BeforeEach
+  void setup() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+    SimHooks.restartTiming();
+  }
+
+  @AfterEach
+  void cleanup() {
+    SimHooks.resumeTiming();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void testStartPeriodicAndStop() {
+    AtomicInteger counter = new AtomicInteger();
+    Notifier notifier = new Notifier(counter::getAndIncrement);
+    notifier.startPeriodic(1.0);
+
+    SimHooks.stepTiming(10);
+
+    notifier.stop();
+    assertEquals(10, counter.get());
+
+    SimHooks.stepTiming(3.0);
+
+    assertEquals(10, counter.get());
+
+    notifier.close();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void testStartSingle() {
+    AtomicInteger counter = new AtomicInteger();
+    Notifier notifier = new Notifier(counter::getAndIncrement);
+    notifier.startSingle(1.0);
+
+    SimHooks.stepTiming(10.5);
+
+    assertEquals(1, counter.get());
+
+    notifier.close();
+  }
+}

wpilibj/src/test/java/org/wpilib/system/RobotControllerTest.java

@@ -0,0 +1,12 @@
+// 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;
+
+@SuppressWarnings("PMD.TestClassWithoutTestCases")
+class RobotControllerTest extends UtilityClassTest<RobotController> {
+  RobotControllerTest() {
+    super(RobotController.class);
+  }
+}

wpilibj/src/test/java/org/wpilib/system/SensorUtilTest.java

@@ -0,0 +1,32 @@
+// 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;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+import org.junit.jupiter.api.Test;
+
+class SensorUtilTest {
+  @Test
+  void checkAnalogInputChannel() {
+    assertThrows(IllegalArgumentException.class, () -> SensorUtil.checkAnalogInputChannel(100));
+  }
+
+  @Test
+  void testInvalidDigitalChannel() {
+    assertThrows(IllegalArgumentException.class, () -> SensorUtil.checkDigitalChannel(100));
+  }
+
+  @Test
+  void testInvalidPwmChannel() {
+    assertThrows(IllegalArgumentException.class, () -> SensorUtil.checkPWMChannel(100));
+  }
+
+  @Test
+  void testgetDefaultCtrePcmModule() {
+    assertEquals(0, SensorUtil.getDefaultCTREPCMModule());
+  }
+}

wpilibj/src/test/java/org/wpilib/system/TimerTest.java

@@ -0,0 +1,136 @@
+// 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;
+
+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.HAL;
+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;
+
+class TimerTest {
+  @BeforeEach
+  void setup() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+    SimHooks.restartTiming();
+  }
+
+  @AfterEach
+  void cleanup() {
+    SimHooks.resumeTiming();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void startStopTest() {
+    var timer = new Timer();
+
+    // Verify timer is initialized as stopped
+    assertEquals(timer.get(), 0.0);
+    assertFalse(timer.isRunning());
+    SimHooks.stepTiming(0.5);
+    assertEquals(timer.get(), 0.0);
+    assertFalse(timer.isRunning());
+
+    // Verify timer increments after it's started
+    timer.start();
+    SimHooks.stepTiming(0.5);
+    assertEquals(timer.get(), 0.5, 1e-9);
+    assertTrue(timer.isRunning());
+
+    // Verify timer stops incrementing after it's stopped
+    timer.stop();
+    SimHooks.stepTiming(0.5);
+    assertEquals(timer.get(), 0.5, 1e-9);
+    assertFalse(timer.isRunning());
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void resetTest() {
+    var timer = new Timer();
+    timer.start();
+
+    // Advance timer to 500 ms
+    assertEquals(timer.get(), 0.0);
+    SimHooks.stepTiming(0.5);
+    assertEquals(timer.get(), 0.5, 1e-9);
+
+    // Verify timer reports 0 ms after reset
+    timer.reset();
+    assertEquals(timer.get(), 0.0);
+
+    // Verify timer continues incrementing
+    SimHooks.stepTiming(0.5);
+    assertEquals(timer.get(), 0.5, 1e-9);
+
+    // Verify timer doesn't start incrementing after reset if it was stopped
+    timer.stop();
+    timer.reset();
+    SimHooks.stepTiming(0.5);
+    assertEquals(timer.get(), 0.0);
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void hasElapsedTest() {
+    var timer = new Timer();
+
+    // Verify 0 ms has elapsed since timer hasn't started
+    assertTrue(timer.hasElapsed(0.0));
+
+    // Verify timer doesn't report elapsed time when stopped
+    SimHooks.stepTiming(0.5);
+    assertFalse(timer.hasElapsed(0.4));
+
+    timer.start();
+
+    // Verify timer reports >= 400 ms has elapsed after multiple calls
+    SimHooks.stepTiming(0.5);
+    assertTrue(timer.hasElapsed(0.4));
+    assertTrue(timer.hasElapsed(0.4));
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void advanceIfElapsedTest() {
+    var timer = new Timer();
+
+    // Verify 0 ms has elapsed since timer hasn't started
+    assertTrue(timer.advanceIfElapsed(0.0));
+
+    // Verify timer doesn't report elapsed time when stopped
+    SimHooks.stepTiming(0.5);
+    assertFalse(timer.advanceIfElapsed(0.4));
+
+    timer.start();
+
+    // Verify timer reports >= 400 ms has elapsed for only first call
+    SimHooks.stepTiming(0.5);
+    assertTrue(timer.advanceIfElapsed(0.4));
+    assertFalse(timer.advanceIfElapsed(0.4));
+
+    // Verify timer reports >= 400 ms has elapsed for two calls
+    SimHooks.stepTiming(1.0);
+    assertTrue(timer.advanceIfElapsed(0.4));
+    assertTrue(timer.advanceIfElapsed(0.4));
+    assertFalse(timer.advanceIfElapsed(0.4));
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void getFPGATimestampTest() {
+    double start = Timer.getFPGATimestamp();
+    SimHooks.stepTiming(0.5);
+    double end = Timer.getFPGATimestamp();
+    assertEquals(start + 0.5, end, 1e-9);
+  }
+}

wpilibj/src/test/java/org/wpilib/system/WatchdogTest.java

@@ -0,0 +1,174 @@
+// 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;
+
+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.HAL;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import java.util.concurrent.atomic.AtomicInteger;
+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;
+
+class WatchdogTest {
+  @BeforeEach
+  void setup() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+  }
+
+  @AfterEach
+  void cleanup() {
+    SimHooks.resumeTiming();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void enableDisableTest() {
+    final AtomicInteger watchdogCounter = new AtomicInteger(0);
+
+    try (Watchdog watchdog = new Watchdog(0.4, () -> watchdogCounter.addAndGet(1))) {
+      // Run 1
+      watchdog.enable();
+      SimHooks.stepTiming(0.2);
+      watchdog.disable();
+
+      assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
+
+      // Run 2
+      watchdogCounter.set(0);
+      watchdog.enable();
+      SimHooks.stepTiming(0.4);
+      watchdog.disable();
+
+      assertEquals(
+          1, watchdogCounter.get(), "Watchdog either didn't trigger or triggered more than once");
+
+      // Run 3
+      watchdogCounter.set(0);
+      watchdog.enable();
+      SimHooks.stepTiming(1.0);
+      watchdog.disable();
+
+      assertEquals(
+          1, watchdogCounter.get(), "Watchdog either didn't trigger or triggered more than once");
+    }
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void resetTest() {
+    final AtomicInteger watchdogCounter = new AtomicInteger(0);
+
+    try (Watchdog watchdog = new Watchdog(0.4, () -> watchdogCounter.addAndGet(1))) {
+      watchdog.enable();
+      SimHooks.stepTiming(0.2);
+      watchdog.reset();
+      SimHooks.stepTiming(0.2);
+      watchdog.disable();
+
+      assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
+    }
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void setTimeoutTest() {
+    final AtomicInteger watchdogCounter = new AtomicInteger(0);
+
+    try (Watchdog watchdog = new Watchdog(1.0, () -> watchdogCounter.addAndGet(1))) {
+      watchdog.enable();
+      SimHooks.stepTiming(0.2);
+      watchdog.setTimeout(0.2);
+
+      assertEquals(0.2, watchdog.getTimeout());
+      assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
+
+      SimHooks.stepTiming(0.3);
+      watchdog.disable();
+
+      assertEquals(
+          1, watchdogCounter.get(), "Watchdog either didn't trigger or triggered more than once");
+    }
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void isExpiredTest() {
+    try (Watchdog watchdog = new Watchdog(0.2, () -> {})) {
+      assertFalse(watchdog.isExpired());
+      watchdog.enable();
+
+      assertFalse(watchdog.isExpired());
+      SimHooks.stepTiming(0.3);
+      assertTrue(watchdog.isExpired());
+
+      watchdog.disable();
+      assertTrue(watchdog.isExpired());
+
+      watchdog.reset();
+      assertFalse(watchdog.isExpired());
+    }
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void epochsTest() {
+    final AtomicInteger watchdogCounter = new AtomicInteger(0);
+
+    try (Watchdog watchdog = new Watchdog(0.4, () -> watchdogCounter.addAndGet(1))) {
+      // Run 1
+      watchdog.enable();
+      watchdog.addEpoch("Epoch 1");
+      SimHooks.stepTiming(0.1);
+      watchdog.addEpoch("Epoch 2");
+      SimHooks.stepTiming(0.1);
+      watchdog.addEpoch("Epoch 3");
+      watchdog.disable();
+
+      assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
+
+      // Run 2
+      watchdog.enable();
+      watchdog.addEpoch("Epoch 1");
+      SimHooks.stepTiming(0.2);
+      watchdog.reset();
+      SimHooks.stepTiming(0.2);
+      watchdog.addEpoch("Epoch 2");
+      watchdog.disable();
+
+      assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
+    }
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void multiWatchdogTest() {
+    final AtomicInteger watchdogCounter1 = new AtomicInteger(0);
+    final AtomicInteger watchdogCounter2 = new AtomicInteger(0);
+
+    try (Watchdog watchdog1 = new Watchdog(0.2, () -> watchdogCounter1.addAndGet(1));
+        Watchdog watchdog2 = new Watchdog(0.6, () -> watchdogCounter2.addAndGet(1))) {
+      watchdog2.enable();
+      SimHooks.stepTiming(0.25);
+      assertEquals(0, watchdogCounter1.get(), "Watchdog triggered early");
+      assertEquals(0, watchdogCounter2.get(), "Watchdog triggered early");
+
+      // Sleep enough such that only the watchdog enabled later times out first
+      watchdog1.enable();
+      SimHooks.stepTiming(0.25);
+      watchdog1.disable();
+      watchdog2.disable();
+
+      assertEquals(
+          1, watchdogCounter1.get(), "Watchdog either didn't trigger or triggered more than once");
+      assertEquals(0, watchdogCounter2.get(), "Watchdog triggered early");
+    }
+  }
+}

wpilibj/src/test/java/org/wpilib/util/AlertTest.java

@@ -0,0 +1,243 @@
+// 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;
+
+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.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.StringArraySubscriber;
+import edu.wpi.first.wpilibj.Alert.AlertType;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.TestInfo;
+import org.junit.jupiter.api.parallel.ResourceLock;
+
+class AlertTest {
+  private NetworkTableInstance m_inst;
+  private String m_groupName;
+
+  @BeforeEach
+  void setup(TestInfo info) {
+    m_groupName = "AlertTest_" + info.getDisplayName();
+
+    m_inst = NetworkTableInstance.create();
+    SmartDashboard.setNetworkTableInstance(m_inst);
+  }
+
+  @AfterEach
+  void checkClean() {
+    update();
+    assertEquals(0, getActiveAlerts(AlertType.kError).length);
+    assertEquals(0, getActiveAlerts(AlertType.kWarning).length);
+    assertEquals(0, getActiveAlerts(AlertType.kInfo).length);
+
+    m_inst.close();
+    SmartDashboard.setNetworkTableInstance(NetworkTableInstance.getDefault());
+  }
+
+  private String getSubtableName(Alert.AlertType type) {
+    switch (type) {
+      case kError:
+        return "errors";
+      case kWarning:
+        return "warnings";
+      case kInfo:
+        return "infos";
+      default:
+        return "unknown";
+    }
+  }
+
+  private StringArraySubscriber getSubscriberForType(Alert.AlertType type) {
+    return m_inst
+        .getStringArrayTopic("/SmartDashboard/" + m_groupName + "/" + getSubtableName(type))
+        .subscribe(new String[] {});
+  }
+
+  private String[] getActiveAlerts(AlertType type) {
+    update();
+    try (var sub = getSubscriberForType(type)) {
+      return sub.get();
+    }
+  }
+
+  private void update() {
+    SmartDashboard.updateValues();
+  }
+
+  private boolean isAlertActive(String text, Alert.AlertType type) {
+    return Arrays.asList(getActiveAlerts(type)).contains(text);
+  }
+
+  private void assertState(Alert.AlertType type, List<String> state) {
+    assertEquals(state, Arrays.asList(getActiveAlerts(type)));
+  }
+
+  private Alert makeAlert(String text, Alert.AlertType type) {
+    return new Alert(m_groupName, text, type);
+  }
+
+  @Test
+  void setUnsetSingle() {
+    try (var one = makeAlert("one", AlertType.kInfo)) {
+      assertFalse(isAlertActive("one", AlertType.kInfo));
+      one.set(true);
+      assertTrue(isAlertActive("one", AlertType.kInfo));
+      one.set(false);
+      assertFalse(isAlertActive("one", AlertType.kInfo));
+    }
+  }
+
+  @Test
+  void setUnsetMultiple() {
+    try (var one = makeAlert("one", AlertType.kError);
+        var two = makeAlert("two", AlertType.kInfo)) {
+      assertFalse(isAlertActive("one", AlertType.kError));
+      assertFalse(isAlertActive("two", AlertType.kInfo));
+      one.set(true);
+      assertTrue(isAlertActive("one", AlertType.kError));
+      assertFalse(isAlertActive("two", AlertType.kInfo));
+      one.set(true);
+      two.set(true);
+      assertTrue(isAlertActive("one", AlertType.kError));
+      assertTrue(isAlertActive("two", AlertType.kInfo));
+      one.set(false);
+      assertFalse(isAlertActive("one", AlertType.kError));
+      assertTrue(isAlertActive("two", AlertType.kInfo));
+    }
+  }
+
+  @Test
+  void setIsIdempotent() {
+    try (var a = makeAlert("A", AlertType.kInfo);
+        var b = makeAlert("B", AlertType.kInfo);
+        var c = makeAlert("C", AlertType.kInfo)) {
+      a.set(true);
+      b.set(true);
+      c.set(true);
+
+      var startState = List.of(getActiveAlerts(AlertType.kInfo));
+
+      b.set(true);
+      assertState(AlertType.kInfo, startState);
+
+      a.set(true);
+      assertState(AlertType.kInfo, startState);
+    }
+  }
+
+  @Test
+  void closeUnsetsAlert() {
+    try (var alert = makeAlert("alert", AlertType.kWarning)) {
+      alert.set(true);
+      assertTrue(isAlertActive("alert", AlertType.kWarning));
+    }
+    assertFalse(isAlertActive("alert", AlertType.kWarning));
+  }
+
+  @Test
+  void setTextWhileUnset() {
+    try (var alert = makeAlert("BEFORE", AlertType.kInfo)) {
+      assertEquals("BEFORE", alert.getText());
+      alert.set(true);
+      assertTrue(isAlertActive("BEFORE", AlertType.kInfo));
+      alert.set(false);
+      assertFalse(isAlertActive("BEFORE", AlertType.kInfo));
+      alert.setText("AFTER");
+      assertEquals("AFTER", alert.getText());
+      alert.set(true);
+      assertFalse(isAlertActive("BEFORE", AlertType.kInfo));
+      assertTrue(isAlertActive("AFTER", AlertType.kInfo));
+    }
+  }
+
+  @Test
+  void setTextWhileSet() {
+    try (var alert = makeAlert("BEFORE", AlertType.kInfo)) {
+      assertEquals("BEFORE", alert.getText());
+      alert.set(true);
+      assertTrue(isAlertActive("BEFORE", AlertType.kInfo));
+      alert.setText("AFTER");
+      assertEquals("AFTER", alert.getText());
+      assertFalse(isAlertActive("BEFORE", AlertType.kInfo));
+      assertTrue(isAlertActive("AFTER", AlertType.kInfo));
+    }
+  }
+
+  @ResourceLock("timing")
+  @Test
+  void setTextDoesNotAffectFirstOrderSort() {
+    SimHooks.pauseTiming();
+    try (var a = makeAlert("A", AlertType.kInfo);
+        var b = makeAlert("B", AlertType.kInfo);
+        var c = makeAlert("C", AlertType.kInfo)) {
+      a.set(true);
+      SimHooks.stepTiming(1);
+      b.set(true);
+      SimHooks.stepTiming(1);
+      c.set(true);
+
+      var expectedEndState = new ArrayList<>(List.of(getActiveAlerts(AlertType.kInfo)));
+      expectedEndState.replaceAll(s -> "B".equals(s) ? "AFTER" : s);
+      b.setText("AFTER");
+
+      assertState(AlertType.kInfo, expectedEndState);
+    } finally {
+      SimHooks.resumeTiming();
+    }
+  }
+
+  @ResourceLock("timing")
+  @Test
+  void sortOrder() {
+    SimHooks.pauseTiming();
+    try (var a = makeAlert("A", AlertType.kInfo);
+        var b = makeAlert("B", AlertType.kInfo);
+        var c = makeAlert("C", AlertType.kInfo)) {
+      a.set(true);
+      assertState(AlertType.kInfo, List.of("A"));
+      SimHooks.stepTiming(1);
+      b.set(true);
+      assertState(AlertType.kInfo, List.of("B", "A"));
+      SimHooks.stepTiming(1);
+      c.set(true);
+      assertState(AlertType.kInfo, List.of("C", "B", "A"));
+
+      SimHooks.stepTiming(1);
+      c.set(false);
+      assertState(AlertType.kInfo, List.of("B", "A"));
+
+      SimHooks.stepTiming(1);
+      c.set(true);
+      assertState(AlertType.kInfo, List.of("C", "B", "A"));
+
+      SimHooks.stepTiming(1);
+      a.set(false);
+      assertState(AlertType.kInfo, List.of("C", "B"));
+
+      SimHooks.stepTiming(1);
+      b.set(false);
+      assertState(AlertType.kInfo, List.of("C"));
+
+      SimHooks.stepTiming(1);
+      b.set(true);
+      assertState(AlertType.kInfo, List.of("B", "C"));
+
+      SimHooks.stepTiming(1);
+      a.set(true);
+      assertState(AlertType.kInfo, List.of("A", "B", "C"));
+    } finally {
+      SimHooks.resumeTiming();
+    }
+  }
+}

wpilibj/src/test/java/org/wpilib/util/Color8BitTest.java

@@ -0,0 +1,65 @@
+// 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.util;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+import org.junit.jupiter.api.Test;
+
+class Color8BitTest {
+  @Test
+  void testConstructDefault() {
+    var color = new Color8Bit();
+
+    assertEquals(0, color.red);
+    assertEquals(0, color.green);
+    assertEquals(0, color.blue);
+  }
+
+  @Test
+  void testConstructFromInts() {
+    var color = new Color8Bit(255, 128, 64);
+
+    assertEquals(255, color.red);
+    assertEquals(128, color.green);
+    assertEquals(64, color.blue);
+  }
+
+  @Test
+  void testConstructFromColor() {
+    var color = new Color8Bit(new Color(255, 128, 64));
+
+    assertEquals(255, color.red);
+    assertEquals(128, color.green);
+    assertEquals(64, color.blue);
+  }
+
+  @Test
+  void testConstructFromHexString() {
+    var color = new Color8Bit("#FF8040");
+
+    assertEquals(255, color.red);
+    assertEquals(128, color.green);
+    assertEquals(64, color.blue);
+
+    // No leading #
+    assertThrows(IllegalArgumentException.class, () -> new Color8Bit("112233"));
+
+    // Too long
+    assertThrows(IllegalArgumentException.class, () -> new Color8Bit("#11223344"));
+
+    // Invalid hex characters
+    assertThrows(IllegalArgumentException.class, () -> new Color8Bit("#$$$$$$"));
+  }
+
+  @Test
+  void testToHexString() {
+    var color = new Color8Bit(255, 128, 64);
+
+    assertEquals("#FF8040", color.toHexString());
+    assertEquals("#FF8040", color.toString());
+  }
+}

wpilibj/src/test/java/org/wpilib/util/ColorTest.java

@@ -0,0 +1,126 @@
+// 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.util;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.params.provider.Arguments.arguments;
+
+import java.util.stream.Stream;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+
+class ColorTest {
+  @Test
+  void testConstructDefault() {
+    var color = new Color();
+
+    assertEquals(0.0, color.red);
+    assertEquals(0.0, color.green);
+    assertEquals(0.0, color.blue);
+  }
+
+  @Test
+  void testConstructFromDoubles() {
+    {
+      var color = new Color(1.0, 0.5, 0.25);
+
+      assertEquals(1.0, color.red, 1e-2);
+      assertEquals(0.5, color.green, 1e-2);
+      assertEquals(0.25, color.blue, 1e-2);
+    }
+
+    {
+      var color = new Color(1.0, 0.0, 0.0);
+
+      // Check for exact match to ensure round-and-clamp is correct
+      assertEquals(1.0, color.red);
+      assertEquals(0.0, color.green);
+      assertEquals(0.0, color.blue);
+    }
+  }
+
+  @Test
+  void testConstructFromInts() {
+    var color = new Color(255, 128, 64);
+
+    assertEquals(1.0, color.red, 1e-2);
+    assertEquals(0.5, color.green, 1e-2);
+    assertEquals(0.25, color.blue, 1e-2);
+  }
+
+  @Test
+  void testConstructFromHexString() {
+    var color = new Color("#FF8040");
+
+    assertEquals(1.0, color.red, 1e-2);
+    assertEquals(0.5, color.green, 1e-2);
+    assertEquals(0.25, color.blue, 1e-2);
+
+    // No leading #
+    assertThrows(IllegalArgumentException.class, () -> new Color("112233"));
+
+    // Too long
+    assertThrows(IllegalArgumentException.class, () -> new Color("#11223344"));
+
+    // Invalid hex characters
+    assertThrows(IllegalArgumentException.class, () -> new Color("#$$$$$$"));
+  }
+
+  @Test
+  void testFromHSV() {
+    var color = Color.fromHSV(90, 128, 64);
+
+    assertEquals(0.125732421875, color.red);
+    assertEquals(0.251220703125, color.green);
+    assertEquals(0.251220703125, color.blue);
+  }
+
+  @Test
+  void testToHexString() {
+    var color = new Color(255, 128, 64);
+
+    assertEquals("#FF8040", color.toHexString());
+    assertEquals("#FF8040", color.toString());
+  }
+
+  @ParameterizedTest
+  @MethodSource("hsvToRgbProvider")
+  void hsvToRgb(int h, int s, int v, int r, int g, int b) {
+    int rgb = Color.hsvToRgb(h, s, v);
+    int R = Color.unpackRGB(rgb, Color.RGBChannel.kRed);
+    int G = Color.unpackRGB(rgb, Color.RGBChannel.kGreen);
+    int B = Color.unpackRGB(rgb, Color.RGBChannel.kBlue);
+
+    assertAll(
+        () -> assertEquals(r, R, "R value didn't match"),
+        () -> assertEquals(g, G, "G value didn't match"),
+        () -> assertEquals(b, B, "B value didn't match"));
+  }
+
+  private static Stream<Arguments> hsvToRgbProvider() {
+    return Stream.of(
+        arguments(0, 0, 0, 0, 0, 0), // Black
+        arguments(0, 0, 255, 255, 255, 255), // White
+        arguments(0, 255, 255, 255, 0, 0), // Red
+        arguments(60, 255, 255, 0, 255, 0), // Lime
+        arguments(120, 255, 255, 0, 0, 255), // Blue
+        arguments(30, 255, 255, 255, 255, 0), // Yellow
+        arguments(90, 255, 255, 0, 255, 255), // Cyan
+        arguments(150, 255, 255, 255, 0, 255), // Magenta
+        arguments(0, 0, 191, 191, 191, 191), // Silver
+        arguments(0, 0, 128, 128, 128, 128), // Gray
+        arguments(0, 255, 128, 128, 0, 0), // Maroon
+        arguments(30, 255, 128, 128, 128, 0), // Olive
+        arguments(60, 255, 128, 0, 128, 0), // Green
+        arguments(150, 255, 128, 128, 0, 128), // Purple
+        arguments(90, 255, 128, 0, 128, 128), // Teal
+        arguments(120, 255, 128, 0, 0, 128) // Navy
+        );
+  }
+}

wpilibj/src/test/java/org/wpilib/util/PreferencesTest.java

@@ -0,0 +1,208 @@
+// 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;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+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 static org.junit.jupiter.api.Assertions.fail;
+import static org.junit.jupiter.api.parallel.ExecutionMode.SAME_THREAD;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.Topic;
+import java.io.IOException;
+import java.io.InputStream;
+import java.nio.file.Files;
+import java.nio.file.Path;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+import java.util.stream.Stream;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Disabled;
+import org.junit.jupiter.api.Nested;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.io.TempDir;
+import org.junit.jupiter.api.parallel.Execution;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.MethodSource;
+
+@Execution(SAME_THREAD)
+class PreferencesTest {
+  private NetworkTableInstance m_inst;
+  private NetworkTable m_table;
+
+  private static final String kFilename = "networktables.json";
+
+  @BeforeEach
+  void setup(@TempDir Path tempDir) {
+    m_inst = NetworkTableInstance.create();
+    m_table = m_inst.getTable("Preferences");
+    Preferences.setNetworkTableInstance(m_inst);
+
+    Path filepath = tempDir.resolve(kFilename);
+    try (InputStream is = getClass().getResource("PreferencesTestDefault.json").openStream()) {
+      Files.copy(is, filepath);
+    } catch (IOException ex) {
+      fail(ex);
+    }
+
+    m_inst.startServer(filepath.toString(), "", 0);
+    try {
+      int count = 0;
+      while (m_inst.getNetworkMode().contains(NetworkTableInstance.NetworkMode.kStarting)) {
+        Thread.sleep(100);
+        count++;
+        if (count > 30) {
+          throw new InterruptedException();
+        }
+      }
+    } catch (InterruptedException ex) {
+      fail("interrupted while waiting for server to start");
+    }
+  }
+
+  @AfterEach
+  void cleanup() {
+    m_inst.close();
+  }
+
+  @Disabled("Fails often with 'Preferences was not empty!'")
+  @Test
+  void removeAllTest() {
+    Preferences.removeAll();
+
+    List<String> keys = new ArrayList<>();
+    boolean anyPersistent = false;
+    for (Topic topic : m_table.getTopics()) {
+      if (topic.isPersistent()) {
+        anyPersistent = true;
+        keys.add(topic.getName());
+      }
+    }
+
+    assertFalse(
+        anyPersistent,
+        "Preferences was not empty!  Preferences in table: " + Arrays.toString(keys.toArray()));
+  }
+
+  @Test
+  void getNetworkTableTest() {
+    NetworkTable networkTable = Preferences.getNetworkTable();
+
+    assertEquals(m_table, networkTable);
+  }
+
+  @ParameterizedTest
+  @MethodSource("defaultKeyProvider")
+  void defaultKeysTest(String key) {
+    assertTrue(Preferences.containsKey(key));
+  }
+
+  @ParameterizedTest
+  @MethodSource("defaultKeyProvider")
+  void defaultKeysAllTest(String key) {
+    assertTrue(Preferences.getKeys().contains(key));
+  }
+
+  @Test
+  void defaultValueTest() {
+    assertAll(
+        () -> assertEquals(172L, Preferences.getLong("checkedValueLong", 0)),
+        () -> assertEquals(0.2, Preferences.getDouble("checkedValueDouble", 0), 1e-6),
+        () -> assertEquals("Hello. How are you?", Preferences.getString("checkedValueString", "")),
+        () -> assertEquals(2, Preferences.getInt("checkedValueInt", 0)),
+        () -> assertEquals(3.4, Preferences.getFloat("checkedValueFloat", 0), 1e-6),
+        () -> assertFalse(Preferences.getBoolean("checkedValueBoolean", true)));
+  }
+
+  @Nested
+  class PutGetTests {
+    @Test
+    void intTest() {
+      final String key = "test";
+      final int value = 123;
+
+      Preferences.setInt(key, value);
+
+      assertAll(
+          () -> assertEquals(value, Preferences.getInt(key, -1)),
+          () -> assertEquals(value, m_table.getEntry(key).getNumber(-1).intValue()));
+    }
+
+    @Test
+    void longTest() {
+      final String key = "test";
+      final long value = 190L;
+
+      Preferences.setLong(key, value);
+
+      assertAll(
+          () -> assertEquals(value, Preferences.getLong(key, -1)),
+          () -> assertEquals(value, m_table.getEntry(key).getNumber(-1).longValue()));
+    }
+
+    @Test
+    void floatTest() {
+      final String key = "test";
+      final float value = 9.42f;
+
+      Preferences.setFloat(key, value);
+
+      assertAll(
+          () -> assertEquals(value, Preferences.getFloat(key, -1), 1e-6),
+          () -> assertEquals(value, m_table.getEntry(key).getNumber(-1).floatValue(), 1e-6));
+    }
+
+    @Test
+    void doubleTest() {
+      final String key = "test";
+      final double value = 6.28;
+
+      Preferences.setDouble(key, value);
+
+      assertAll(
+          () -> assertEquals(value, Preferences.getDouble(key, -1), 1e-6),
+          () -> assertEquals(value, m_table.getEntry(key).getNumber(-1).doubleValue(), 1e-6));
+    }
+
+    @Test
+    void stringTest() {
+      final String key = "test";
+      final String value = "value";
+
+      Preferences.setString(key, value);
+
+      assertAll(
+          () -> assertEquals(value, Preferences.getString(key, "")),
+          () -> assertEquals(value, m_table.getEntry(key).getString("")));
+    }
+
+    @Test
+    void booleanTest() {
+      final String key = "test";
+      final boolean value = true;
+
+      Preferences.setBoolean(key, value);
+
+      assertAll(
+          () -> assertEquals(value, Preferences.getBoolean(key, false)),
+          () -> assertEquals(value, m_table.getEntry(key).getBoolean(false)));
+    }
+  }
+
+  static Stream<String> defaultKeyProvider() {
+    return Stream.of(
+        "checkedValueLong",
+        "checkedValueDouble",
+        "checkedValueString",
+        "checkedValueInt",
+        "checkedValueFloat",
+        "checkedValueBoolean");
+  }
+}