[wpilib, examples] Cleanup PotentiometerPID, Ultrasonic, UltrasonicPID examples (#4893)

Fix C++ Ultrasonic to return correct units.

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json

@@ -115,11 +115,13 @@
   },
   {
     "name": "Ultrasonic",
-    "description": "Demonstrate maintaining a set distance using an ultrasonic sensor.",
+    "description": "Demonstrate using the Ultrasonic class with a ping-response ultrasonic sensor.",
     "tags": [
       "Sensors",
-      "Robot and Motor",
-      "Analog"
+      "Hardware",
+      "Ultrasonic",
+      "SmartDashboard",
+      "Shuffleboard"
     ],
     "foldername": "ultrasonic",
     "gradlebase": "java",
@@ -131,8 +133,9 @@
     "description": "Demonstrate maintaining a set distance using an ultrasonic sensor and PID Control.",
     "tags": [
       "Sensors",
-      "Robot and Motor",
-      "Analog"
+      "Ultrasonic",
+      "PID",
+      "Differential Drive"
     ],
     "foldername": "ultrasonicpid",
     "gradlebase": "java",
@@ -141,11 +144,13 @@
   },
   {
     "name": "Potentiometer PID",
-    "description": "An example to demonstrate the use of a potentiometer and PID control to reach elevator position setpoints.",
+    "description": "An example to demonstrate the use of a potentiometer and PID control to maintain elevator position setpoints.",
     "tags": [
       "Sensors",
       "Actuators",
       "Analog",
+      "Elevator",
+      "PID",
       "Joystick"
     ],
     "foldername": "potentiometerpid",

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java

@@ -5,7 +5,7 @@
 package edu.wpi.first.wpilibj.examples.potentiometerpid;
 
 import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.AnalogPotentiometer;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.motorcontrol.MotorController;
@@ -16,53 +16,55 @@ import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
  * reach and maintain position setpoints on an elevator mechanism.
  */
 public class Robot extends TimedRobot {
-  private static final int kPotChannel = 1;
-  private static final int kMotorChannel = 7;
-  private static final int kJoystickChannel = 0;
+  static final int kPotChannel = 1;
+  static final int kMotorChannel = 7;
+  static final int kJoystickChannel = 0;
 
-  // bottom, middle, and top elevator setpoints
-  private static final double[] kSetPoints = {1.0, 2.6, 4.3};
+  // The elevator can move 1.5 meters from top to bottom
+  static final double kFullHeightMeters = 1.5;
 
-  // proportional, integral, and derivative speed constants; motor inverted
+  // Bottom, middle, and top elevator setpoints
+  static final double[] kSetpointsMeters = {0.2, 0.8, 1.4};
+
+  // proportional, integral, and derivative speed constants
   // DANGER: when tuning PID constants, high/inappropriate values for kP, kI,
   // and kD may cause dangerous, uncontrollable, or undesired behavior!
-  // these may need to be positive for a non-inverted motor
-  private static final double kP = -5.0;
-  private static final double kI = -0.02;
-  private static final double kD = -2.0;
+  private static final double kP = 0.7;
+  private static final double kI = 0.35;
+  private static final double kD = 0.25;
 
-  private PIDController m_pidController;
-  private AnalogInput m_potentiometer;
-  private MotorController m_elevatorMotor;
-  private Joystick m_joystick;
+  private final PIDController m_pidController = new PIDController(kP, kI, kD);
+  // Scaling is handled internally
+  private final AnalogPotentiometer m_potentiometer =
+      new AnalogPotentiometer(kPotChannel, kFullHeightMeters);
+  private final MotorController m_elevatorMotor = new PWMSparkMax(kMotorChannel);
+  private final Joystick m_joystick = new Joystick(kJoystickChannel);
 
   private int m_index;
-  private boolean m_previousButtonValue;
 
   @Override
-  public void robotInit() {
-    m_potentiometer = new AnalogInput(kPotChannel);
-    m_elevatorMotor = new PWMSparkMax(kMotorChannel);
-    m_joystick = new Joystick(kJoystickChannel);
-
-    m_pidController = new PIDController(kP, kI, kD);
-    m_pidController.setSetpoint(kSetPoints[m_index]);
+  public void teleopInit() {
+    // Move to the bottom setpoint when teleop starts
+    m_index = 0;
+    m_pidController.setSetpoint(kSetpointsMeters[m_index]);
   }
 
   @Override
   public void teleopPeriodic() {
+    // Read from the sensor
+    double position = m_potentiometer.get();
+
     // Run the PID Controller
-    double pidOut = m_pidController.calculate(m_potentiometer.getAverageVoltage());
+    double pidOut = m_pidController.calculate(position);
+
+    // Apply PID output
     m_elevatorMotor.set(pidOut);
 
-    // when the button is pressed once, the selected elevator setpoint
-    // is incremented
-    boolean currentButtonValue = m_joystick.getTrigger();
-    if (currentButtonValue && !m_previousButtonValue) {
+    // when the button is pressed once, the selected elevator setpoint is incremented
+    if (m_joystick.getTriggerPressed()) {
       // index of the elevator setpoint wraps around.
-      m_index = (m_index + 1) % kSetPoints.length;
-      m_pidController.setSetpoint(kSetPoints[m_index]);
+      m_index = (m_index + 1) % kSetpointsMeters.length;
+      m_pidController.setSetpoint(kSetpointsMeters[m_index]);
     }
-    m_previousButtonValue = currentButtonValue;
   }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java

@@ -4,52 +4,61 @@
 
 package edu.wpi.first.wpilibj.examples.ultrasonic;
 
-import edu.wpi.first.math.filter.MedianFilter;
-import edu.wpi.first.wpilibj.AnalogInput;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj.Ultrasonic;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 /**
- * This is a sample program demonstrating how to use an ultrasonic sensor and proportional control
- * to maintain a set distance from an object.
+ * This is a sample program demonstrating how to read from a ping-response ultrasonic sensor with
+ * the {@link Ultrasonic class}.
  */
 public class Robot extends TimedRobot {
-  // distance in inches the robot wants to stay from an object
-  private static final double kHoldDistance = 12.0;
+  // Creates a ping-response Ultrasonic object on DIO 1 and 2.
+  Ultrasonic m_rangeFinder = new Ultrasonic(1, 2);
 
-  // factor to convert sensor values to a distance in inches
-  private static final double kValueToInches = 0.125;
+  @Override
+  public void robotInit() {
+    // Add the ultrasonic on the "Sensors" tab of the dashboard
+    // Data will update automatically
+    Shuffleboard.getTab("Sensors").add(m_rangeFinder);
+  }
 
-  // proportional speed constant
-  private static final double kP = 0.05;
-
-  private static final int kLeftMotorPort = 0;
-  private static final int kRightMotorPort = 1;
-  private static final int kUltrasonicPort = 0;
-
-  // median filter to discard outliers; filters over 10 samples
-  private final MedianFilter m_filter = new MedianFilter(10);
-
-  private final AnalogInput m_ultrasonic = new AnalogInput(kUltrasonicPort);
-  private final DifferentialDrive m_robotDrive =
-      new DifferentialDrive(new PWMSparkMax(kLeftMotorPort), new PWMSparkMax(kRightMotorPort));
-
-  /**
-   * Tells the robot to drive to a set distance (in inches) from an object using proportional
-   * control.
-   */
   @Override
   public void teleopPeriodic() {
-    // sensor returns a value from 0-4095 that is scaled to inches
-    // returned value is filtered with a rolling median filter, since ultrasonics
-    // tend to be quite noisy and susceptible to sudden outliers
-    double currentDistance = m_filter.calculate(m_ultrasonic.getValue()) * kValueToInches;
+    // We can read the distance in millimeters
+    double distanceMillimeters = m_rangeFinder.getRangeMM();
+    // ... or in inches
+    double distanceInches = m_rangeFinder.getRangeInches();
 
-    // convert distance error to a motor speed
-    double currentSpeed = (kHoldDistance - currentDistance) * kP;
+    // We can also publish the data itself periodically
+    SmartDashboard.putNumber("Distance[mm]", distanceMillimeters);
+    SmartDashboard.putNumber("Distance[inch]", distanceInches);
+  }
 
-    // drive robot
-    m_robotDrive.arcadeDrive(currentSpeed, 0);
+  @Override
+  public void testInit() {
+    // By default, the Ultrasonic class polls all ultrasonic sensors in a round-robin to prevent
+    // them from interfering from one another.
+    // However, manual polling is also possible -- note that this disables automatic mode!
+    m_rangeFinder.ping();
+  }
+
+  @Override
+  public void testPeriodic() {
+    if (m_rangeFinder.isRangeValid()) {
+      // Data is valid, publish it
+      SmartDashboard.putNumber("Distance[mm]", m_rangeFinder.getRangeMM());
+      SmartDashboard.putNumber("Distance[inch]", m_rangeFinder.getRangeInches());
+
+      // Ping for next measurement
+      m_rangeFinder.ping();
+    }
+  }
+
+  @Override
+  public void testExit() {
+    // Enable automatic mode
+    Ultrasonic.setAutomaticMode(true);
   }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java

@@ -6,8 +6,8 @@ package edu.wpi.first.wpilibj.examples.ultrasonicpid;
 
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.filter.MedianFilter;
-import edu.wpi.first.wpilibj.AnalogInput;
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.Ultrasonic;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
@@ -16,45 +16,56 @@ import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
  * reach and maintain a set distance from an object.
  */
 public class Robot extends TimedRobot {
-  // distance in inches the robot wants to stay from an object
-  private static final double kHoldDistance = 12.0;
-
-  // factor to convert sensor values to a distance in inches
-  private static final double kValueToInches = 0.125;
+  // distance the robot wants to stay from an object
+  // (one meter)
+  static final double kHoldDistanceMillimeters = 1.0e3;
 
   // proportional speed constant
-  private static final double kP = 7.0;
-
+  // negative because applying positive voltage will bring us closer to the target
+  private static final double kP = -0.001;
   // integral speed constant
-  private static final double kI = 0.018;
-
+  private static final double kI = 0.0;
   // derivative speed constant
-  private static final double kD = 1.5;
+  private static final double kD = 0.0;
 
-  private static final int kLeftMotorPort = 0;
-  private static final int kRightMotorPort = 1;
-  private static final int kUltrasonicPort = 0;
+  static final int kLeftMotorPort = 0;
+  static final int kRightMotorPort = 1;
 
-  // median filter to discard outliers; filters over 5 samples
+  static final int kUltrasonicPingPort = 0;
+  static final int kUltrasonicEchoPort = 1;
+
+  // Ultrasonic sensors tend to be quite noisy and susceptible to sudden outliers,
+  // so measurements are filtered with a 5-sample median filter
   private final MedianFilter m_filter = new MedianFilter(5);
 
-  private final AnalogInput m_ultrasonic = new AnalogInput(kUltrasonicPort);
-  private final DifferentialDrive m_robotDrive =
-      new DifferentialDrive(new PWMSparkMax(kLeftMotorPort), new PWMSparkMax(kRightMotorPort));
+  private final Ultrasonic m_ultrasonic = new Ultrasonic(kUltrasonicPingPort, kUltrasonicEchoPort);
+  private final PWMSparkMax m_leftMotor = new PWMSparkMax(kLeftMotorPort);
+  private final PWMSparkMax m_rightMotor = new PWMSparkMax(kRightMotorPort);
+  private final DifferentialDrive m_robotDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
   private final PIDController m_pidController = new PIDController(kP, kI, kD);
 
   @Override
-  public void teleopInit() {
+  public void autonomousInit() {
     // Set setpoint of the pid controller
-    m_pidController.setSetpoint(kHoldDistance * kValueToInches);
+    m_pidController.setSetpoint(kHoldDistanceMillimeters);
   }
 
   @Override
-  public void teleopPeriodic() {
-    // returned value is filtered with a rolling median filter, since ultrasonics
-    // tend to be quite noisy and susceptible to sudden outliers
-    double pidOutput = m_pidController.calculate(m_filter.calculate(m_ultrasonic.getVoltage()));
+  public void autonomousPeriodic() {
+    double measurement = m_ultrasonic.getRangeMM();
+    double filteredMeasurement = m_filter.calculate(measurement);
+    double pidOutput = m_pidController.calculate(filteredMeasurement);
 
-    m_robotDrive.arcadeDrive(pidOutput, 0);
+    // disable input squaring -- PID output is linear
+    m_robotDrive.arcadeDrive(pidOutput, 0, false);
+  }
+
+  @Override
+  public void close() {
+    m_leftMotor.close();
+    m_rightMotor.close();
+    m_ultrasonic.close();
+    m_robotDrive.close();
+    super.close();
   }
 }

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

wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/UltrasonicPIDTest.java

@@ -0,0 +1,135 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.ultrasonicpid;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.HAL.SimPeriodicBeforeCallback;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
+import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim.KitbotGearing;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import edu.wpi.first.wpilibj.simulation.PWMSim;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import edu.wpi.first.wpilibj.simulation.UltrasonicSim;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.parallel.ResourceLock;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.ValueSource;
+
+@ResourceLock("timing")
+class UltrasonicPIDTest {
+  private final DCMotor m_gearbox = DCMotor.getFalcon500(2);
+  private static final double kGearing = KitbotGearing.k10p71.value;
+  public static final double kvVoltSecondsPerMeter = 1.98;
+  public static final double kaVoltSecondsSquaredPerMeter = 0.2;
+  private static final double kvVoltSecondsPerRadian = 1.5;
+  private static final double kaVoltSecondsSquaredPerRadian = 0.3;
+  private static final double kWheelDiameterMeters = 0.15;
+  private static final double kTrackwidthMeters = 0.7;
+
+  private Robot m_robot;
+  private Thread m_thread;
+
+  private DifferentialDrivetrainSim m_driveSim;
+  private PWMSim m_leftMotorSim;
+  private PWMSim m_rightMotorSim;
+  private UltrasonicSim m_ultrasonicSim;
+  private SimPeriodicBeforeCallback m_callback;
+
+  // distance between the robot's starting position and the object
+  // we will update this in a moment
+  private double m_startToObject = Double.POSITIVE_INFINITY;
+  private double m_distanceMM;
+
+  // We're not using @BeforeEach so m_startToObject gets initialized properly
+  private void startThread() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+    DriverStationSim.resetData();
+    m_robot = new Robot();
+    m_thread = new Thread(m_robot::startCompetition);
+    m_driveSim =
+        new DifferentialDrivetrainSim(
+            LinearSystemId.identifyDrivetrainSystem(
+                kvVoltSecondsPerMeter,
+                kaVoltSecondsSquaredPerMeter,
+                kvVoltSecondsPerRadian,
+                kaVoltSecondsSquaredPerRadian),
+            m_gearbox,
+            kGearing,
+            kTrackwidthMeters,
+            kWheelDiameterMeters / 2.0,
+            null);
+    m_ultrasonicSim = new UltrasonicSim(Robot.kUltrasonicPingPort, Robot.kUltrasonicEchoPort);
+    m_leftMotorSim = new PWMSim(Robot.kLeftMotorPort);
+    m_rightMotorSim = new PWMSim(Robot.kRightMotorPort);
+
+    m_callback =
+        HAL.registerSimPeriodicBeforeCallback(
+            () -> {
+              m_driveSim.setInputs(
+                  m_leftMotorSim.getSpeed() * RobotController.getBatteryVoltage(),
+                  m_rightMotorSim.getSpeed() * RobotController.getBatteryVoltage());
+              m_driveSim.update(0.02);
+
+              double startingDistance = m_startToObject;
+              double range = startingDistance - m_driveSim.getLeftPositionMeters();
+
+              m_ultrasonicSim.setRangeMeters(range);
+              m_distanceMM = range * 1.0e3;
+            });
+
+    m_thread.start();
+    SimHooks.stepTiming(0.0); // Wait for Notifiers
+    SimHooks.stepTiming(0.02); // Have once iteration on disabled
+  }
+
+  @AfterEach
+  void stopThread() {
+    m_robot.endCompetition();
+    try {
+      m_thread.interrupt();
+      m_thread.join();
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    m_robot.close();
+    m_callback.close();
+    m_leftMotorSim.resetData();
+    m_rightMotorSim.resetData();
+  }
+
+  @ValueSource(doubles = {1.3, 0.5, 5.0})
+  @ParameterizedTest
+  void autoTest(double distance) {
+    // set up distance
+    {
+      m_startToObject = distance;
+    }
+    startThread();
+
+    // auto init
+    {
+      DriverStationSim.setAutonomous(true);
+      DriverStationSim.setEnabled(true);
+      DriverStationSim.notifyNewData();
+
+      assertTrue(m_leftMotorSim.getInitialized());
+      assertTrue(m_rightMotorSim.getInitialized());
+    }
+
+    {
+      // advance 100 timesteps
+      SimHooks.stepTiming(2.0);
+
+      assertEquals(Robot.kHoldDistanceMillimeters, m_distanceMM, 10);
+    }
+  }
+}