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

Fix C++ Ultrasonic to return correct units.
2026-06-23 01:21:42 +00:00 · 2023-01-09 02:33:07 +02:00
parent babb0c1fcf
commit 2cd9be413f
24 changed files with 1052 additions and 301 deletions
--- a/wpilibcExamples/src/main/cpp/examples/PotentiometerPID/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/PotentiometerPID/cpp/Robot.cpp
@@ -2,72 +2,32 @@
 // 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.

-#include <array>
+#include "Robot.h"

-#include <frc/AnalogInput.h>
-#include <frc/Joystick.h>
-#include <frc/TimedRobot.h>
-#include <frc/controller/PIDController.h>
-#include <frc/motorcontrol/PWMSparkMax.h>
+void Robot::TeleopInit() {
+  // Move to the bottom setpoint when teleop starts
+  m_index = 0;
+  m_pidController.SetSetpoint(kSetpoints[m_index].value());
+}

-/**
- * This is a sample program to demonstrate how to use a soft potentiometer and a
- * PID Controller to reach and maintain position setpoints on an elevator
- * mechanism.
- */
-class Robot : public frc::TimedRobot {
- public:
-  void RobotInit() override {
-    m_pidController.SetSetpoint(kSetPoints[m_index]);
+void Robot::TeleopPeriodic() {
+  // Read from the sensor
+  units::meter_t position = units::meter_t{m_potentiometer.Get()};
+
+  // Run the PID Controller
+  double pidOut = m_pidController.Calculate(position.value());
+
+  // Apply PID output
+  m_elevatorMotor.Set(pidOut);
+
+  // 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.size();
+    m_pidController.SetSetpoint(kSetpoints[m_index].value());
  }
-
-  void TeleopPeriodic() override {
-    // When the button is pressed once, the selected elevator setpoint is
-    // incremented.
-    bool currentButtonValue = m_joystick.GetTrigger();
-    if (currentButtonValue && !m_previousButtonValue) {
-      // Index of the elevator setpoint wraps around
-      m_index = (m_index + 1) % (sizeof(kSetPoints) / 8);
-      m_pidController.SetSetpoint(kSetPoints[m_index]);
-    }
-    m_previousButtonValue = currentButtonValue;
-
-    double output =
-        m_pidController.Calculate(m_potentiometer.GetAverageVoltage());
-    m_elevatorMotor.Set(output);
-  }
-
- private:
-  static constexpr int kPotChannel = 1;
-  static constexpr int kMotorChannel = 7;
-  static constexpr int kJoystickChannel = 0;
-
-  // Bottom, middle, and top elevator setpoints
-  static constexpr std::array<double, 3> kSetPoints = {{1.0, 2.6, 4.3}};
-
-  /* Proportional, integral, and derivative speed constants; motor inverted.
-   *
-   * DANGER: When tuning PID constants, high/inappropriate values for pGain,
-   * iGain, and dGain may cause dangerous, uncontrollable, or undesired
-   * behavior!
-   *
-   * These may need to be positive for a non-inverted motor.
-   */
-  static constexpr double kP = -5.0;
-  static constexpr double kI = -0.02;
-  static constexpr double kD = -2.0;
-
-  int m_index = 0;
-  bool m_previousButtonValue = false;
-
-  frc::AnalogInput m_potentiometer{kPotChannel};
-  frc::Joystick m_joystick{kJoystickChannel};
-  frc::PWMSparkMax m_elevatorMotor{kMotorChannel};
-
-  frc2::PIDController m_pidController{kP, kI, kD};
-};
-
-constexpr std::array<double, 3> Robot::kSetPoints;
+}

 #ifndef RUNNING_FRC_TESTS
 int main() {
--- a/wpilibcExamples/src/main/cpp/examples/PotentiometerPID/include/Robot.h
+++ b/wpilibcExamples/src/main/cpp/examples/PotentiometerPID/include/Robot.h
@@ -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.
+
+#pragma once
+
+#include <array>
+
+#include <frc/AnalogPotentiometer.h>
+#include <frc/Joystick.h>
+#include <frc/TimedRobot.h>
+#include <frc/controller/PIDController.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <units/length.h>
+
+/**
+ * This is a sample program to demonstrate how to use a soft potentiometer and a
+ * PID controller to reach and maintain position setpoints on an elevator
+ * mechanism.
+ */
+class Robot : public frc::TimedRobot {
+ public:
+  void TeleopInit() override;
+  void TeleopPeriodic() override;
+
+  static constexpr int kPotChannel = 1;
+  static constexpr int kMotorChannel = 7;
+  static constexpr int kJoystickChannel = 0;
+
+  // The elevator can move 1.5 meters from top to bottom
+  static constexpr units::meter_t kFullHeight = 1.5_m;
+
+  // Bottom, middle, and top elevator setpoints
+  static constexpr std::array<units::meter_t, 3> kSetpoints = {
+      {0.2_m, 0.8_m, 1.4_m}};
+
+ private:
+  // proportional speed constant
+  // negative because applying positive voltage will bring us closer to the
+  // target
+  static constexpr double kP = 0.7;
+  // integral speed constant
+  static constexpr double kI = 0.35;
+  // derivative speed constant
+  static constexpr double kD = 0.25;
+
+  // Scaling is handled internally
+  frc::AnalogPotentiometer m_potentiometer{kPotChannel, kFullHeight.value()};
+
+  frc::PWMSparkMax m_elevatorMotor{kMotorChannel};
+  frc2::PIDController m_pidController{kP, kI, kD};
+  frc::Joystick m_joystick{kJoystickChannel};
+
+  size_t m_index;
+};
--- a/wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp
@@ -2,57 +2,54 @@
 // 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.

-#include <frc/AnalogInput.h>
-#include <frc/TimedRobot.h>
-#include <frc/drive/DifferentialDrive.h>
-#include <frc/filter/MedianFilter.h>
-#include <frc/motorcontrol/PWMSparkMax.h>
+#include "Robot.h"

-/**
- * This is a sample program demonstrating how to use an ultrasonic sensor and
- * proportional control to maintain a set distance from an object.
- */
-class Robot : public frc::TimedRobot {
- public:
-  /**
-   * Tells the robot to drive to a set distance (in inches) from an object using
-   * proportional control.
-   */
-  void TeleopPeriodic() override {
-    // 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.GetVoltage()) * kValueToInches;
-    // Convert distance error to a motor speed
-    double currentSpeed = (kHoldDistance - currentDistance) * kP;
-    // Drive robot
-    m_robotDrive.ArcadeDrive(currentSpeed, 0);
+#include <frc/shuffleboard/Shuffleboard.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <units/length.h>
+
+void Robot::RobotInit() {
+  // Add the ultrasonic on the "Sensors" tab of the dashboard
+  // Data will update automatically
+  frc::Shuffleboard::GetTab("Sensors").Add(m_rangeFinder);
+}
+
+void Robot::TeleopPeriodic() {
+  // We can read the distance
+  units::meter_t distance = m_rangeFinder.GetRange();
+  // units auto-convert
+  units::millimeter_t distanceMillimeters = distance;
+  units::inch_t distanceInches = distance;
+
+  // We can also publish the data itself periodically
+  frc::SmartDashboard::PutNumber("Distance[mm]", distanceMillimeters.value());
+  frc::SmartDashboard::PutNumber("Distance[inch]", distanceInches.value());
+}
+
+void Robot::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();
+}
+
+void Robot::TestPeriodic() {
+  if (m_rangeFinder.IsRangeValid()) {
+    // Data is valid, publish it
+    units::millimeter_t distanceMillimeters = m_rangeFinder.GetRange();
+    units::inch_t distanceInches = m_rangeFinder.GetRange();
+    frc::SmartDashboard::PutNumber("Distance[mm]", distanceMillimeters.value());
+    frc::SmartDashboard::PutNumber("Distance[inch]", distanceInches.value());
+
+    // Ping for next measurement
+    m_rangeFinder.Ping();
  }
+}

- private:
-  // Distance in inches the robot wants to stay from an object
-  static constexpr int kHoldDistance = 12;
-
-  // Factor to convert sensor values to a distance in inches
-  static constexpr double kValueToInches = 0.125;
-
-  // Proportional speed constant
-  static constexpr double kP = 0.05;
-
-  static constexpr int kLeftMotorPort = 0;
-  static constexpr int kRightMotorPort = 1;
-  static constexpr int kUltrasonicPort = 0;
-
-  // median filter to discard outliers; filters over 10 samples
-  frc::MedianFilter<double> m_filter{10};
-
-  frc::AnalogInput m_ultrasonic{kUltrasonicPort};
-
-  frc::PWMSparkMax m_left{kLeftMotorPort};
-  frc::PWMSparkMax m_right{kRightMotorPort};
-  frc::DifferentialDrive m_robotDrive{m_left, m_right};
-};
+void Robot::TestExit() {
+  // Enable automatic mode
+  frc::Ultrasonic::SetAutomaticMode(true);
+}

 #ifndef RUNNING_FRC_TESTS
 int main() {
--- a/wpilibcExamples/src/main/cpp/examples/Ultrasonic/include/Robot.h
+++ b/wpilibcExamples/src/main/cpp/examples/Ultrasonic/include/Robot.h
@@ -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.
+
+#pragma once
+
+#include <frc/TimedRobot.h>
+#include <frc/Ultrasonic.h>
+
+/**
+ * This is a sample program demonstrating how to read from a ping-response
+ * ultrasonic sensor with the {@link Ultrasonic class}.
+ */
+class Robot : public frc::TimedRobot {
+ public:
+  void RobotInit() override;
+  void TeleopPeriodic() override;
+  void TestInit() override;
+  void TestPeriodic() override;
+  void TestExit() override;
+
+ private:
+  // Creates a ping-response Ultrasonic object on DIO 1 and 2.
+  frc::Ultrasonic m_rangeFinder{1, 2};
+};
--- a/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp
@@ -2,65 +2,21 @@
 // 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.

-#include <frc/AnalogInput.h>
-#include <frc/TimedRobot.h>
-#include <frc/controller/PIDController.h>
-#include <frc/drive/DifferentialDrive.h>
-#include <frc/filter/MedianFilter.h>
-#include <frc/motorcontrol/PWMSparkMax.h>
+#include "Robot.h"

-/**
- * This is a sample program demonstrating how to use an ultrasonic sensor and
- * proportional control to maintain a set distance from an object.
- */
-class Robot : public frc::TimedRobot {
- public:
-  /**
-   * Drives the robot a set distance from an object using PID control and the
-   * ultrasonic sensor.
-   */
-  void TeleopInit() override {
-    // Set setpoint of the PID Controller
-    m_pidController.SetSetpoint(kHoldDistance * kValueToInches);
-  }
+void Robot::AutonomousInit() {
+  // Set setpoint of the pid controller
+  m_pidController.SetSetpoint(kHoldDistance.value());
+}

-  void TeleopPeriodic() override {
-    double output =
-        m_pidController.Calculate(m_filter.Calculate(m_ultrasonic.GetValue()));
-    m_robotDrive.ArcadeDrive(output, 0);
-  }
+void Robot::AutonomousPeriodic() {
+  units::millimeter_t measurement = m_ultrasonic.GetRange();
+  units::millimeter_t filteredMeasurement = m_filter.Calculate(measurement);
+  double pidOutput = m_pidController.Calculate(filteredMeasurement.value());

- private:
-  // Distance in inches the robot wants to stay from an object
-  static constexpr int kHoldDistance = 12;
-
-  // Factor to convert sensor values to a distance in inches
-  static constexpr double kValueToInches = 0.125;
-
-  // proportional speed constant
-  static constexpr double kP = 7.0;
-
-  // integral speed constant
-  static constexpr double kI = 0.018;
-
-  // derivative speed constant
-  static constexpr double kD = 1.5;
-
-  static constexpr int kLeftMotorPort = 0;
-  static constexpr int kRightMotorPort = 1;
-  static constexpr int kUltrasonicPort = 0;
-
-  // median filter to discard outliers; filters over 5 samples
-  frc::MedianFilter<double> m_filter{5};
-
-  frc::AnalogInput m_ultrasonic{kUltrasonicPort};
-
-  frc::PWMSparkMax m_left{kLeftMotorPort};
-  frc::PWMSparkMax m_right{kRightMotorPort};
-  frc::DifferentialDrive m_robotDrive{m_left, m_right};
-
-  frc2::PIDController m_pidController{kP, kI, kD};
-};
+  // disable input squaring -- PID output is linear
+  m_robotDrive.ArcadeDrive(pidOutput, 0, false);
+}

 #ifndef RUNNING_FRC_TESTS
 int main() {
--- a/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/include/Robot.h
+++ b/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/include/Robot.h
@@ -0,0 +1,51 @@
+// 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.
+
+#pragma once
+
+#include <frc/TimedRobot.h>
+#include <frc/Ultrasonic.h>
+#include <frc/controller/PIDController.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc/filter/MedianFilter.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <units/length.h>
+
+/**
+ * This is a sample program to demonstrate the use of a PIDController with an
+ * ultrasonic sensor to reach and maintain a set distance from an object.
+ */
+class Robot : public frc::TimedRobot {
+ public:
+  void AutonomousInit() override;
+  void AutonomousPeriodic() override;
+
+  // distance the robot wants to stay from an object
+  static constexpr units::millimeter_t kHoldDistance = 1_m;
+
+  static constexpr int kLeftMotorPort = 0;
+  static constexpr int kRightMotorPort = 1;
+  static constexpr int kUltrasonicPingPort = 0;
+  static constexpr int kUltrasonicEchoPort = 1;
+
+ private:
+  // proportional speed constant
+  // negative because applying positive voltage will bring us closer to the
+  // target
+  static constexpr double kP = -0.001;
+  // integral speed constant
+  static constexpr double kI = 0.0;
+  // derivative speed constant
+  static constexpr double kD = 0.0;
+
+  // Ultrasonic sensors tend to be quite noisy and susceptible to sudden
+  // outliers, so measurements are filtered with a 5-sample median filter
+  frc::MedianFilter<units::millimeter_t> m_filter{5};
+
+  frc::Ultrasonic m_ultrasonic{kUltrasonicPingPort, kUltrasonicEchoPort};
+  frc::PWMSparkMax m_left{kLeftMotorPort};
+  frc::PWMSparkMax m_right{kRightMotorPort};
+  frc::DifferentialDrive m_robotDrive{m_left, m_right};
+  frc2::PIDController m_pidController{kP, kI, kD};
+};
--- a/wpilibcExamples/src/main/cpp/examples/examples.json
+++ b/wpilibcExamples/src/main/cpp/examples/examples.json
@@ -122,12 +122,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": [
-      "Robot and Motor",
-      "Complete List",
      "Sensors",
-      "Analog"
+      "Hardware",
+      "Ultrasonic",
+      "SmartDashboard",
+      "Shuffleboard"
    ],
    "foldername": "Ultrasonic",
    "gradlebase": "cpp",
@@ -135,12 +136,12 @@
  },
  {
    "name": "UltrasonicPID",
-    "description": "Demonstrate maintaining a set distance using an ultrasonic sensor and PID control.",
+    "description": "Demonstrate maintaining a set distance using an ultrasonic sensor and PID Control.",
    "tags": [
-      "Robot and Motor",
-      "Complete List",
      "Sensors",
-      "Analog"
+      "Ultrasonic",
+      "PID",
+      "Differential Drive"
    ],
    "foldername": "UltrasonicPID",
    "gradlebase": "cpp",
@@ -186,11 +187,12 @@
  },
  {
    "name": "PotentiometerPID",
-    "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": [
      "Joystick",
      "Actuators",
-      "Complete List",
+      "PID",
+      "Elevator",
      "Sensors",
      "Analog"
    ],
--- a/wpilibcExamples/src/test/cpp/examples/PotentiometerPID/cpp/PotentiometerPIDTest.cpp
+++ b/wpilibcExamples/src/test/cpp/examples/PotentiometerPID/cpp/PotentiometerPIDTest.cpp
@@ -0,0 +1,167 @@
+// 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.
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <thread>
+
+#include <frc/RobotController.h>
+#include <frc/simulation/AnalogInputSim.h>
+#include <frc/simulation/DriverStationSim.h>
+#include <frc/simulation/ElevatorSim.h>
+#include <frc/simulation/JoystickSim.h>
+#include <frc/simulation/PWMSim.h>
+#include <frc/simulation/SimHooks.h>
+#include <frc/system/plant/DCMotor.h>
+#include <hal/simulation/MockHooks.h>
+#include <units/length.h>
+#include <units/mass.h>
+#include <units/time.h>
+
+#include "Robot.h"
+
+class PotentiometerPIDTest : public testing::Test {
+  frc::DCMotor m_elevatorGearbox = frc::DCMotor::Vex775Pro(4);
+  static constexpr double kElevatorGearing = 10.0;
+  static constexpr units::meter_t kElevatorDrumRadius = 2.0_in;
+  static constexpr units::kilogram_t kCarriageMass = 4.0_kg;
+
+  Robot m_robot;
+  std::optional<std::thread> m_thread;
+
+ protected:
+  frc::sim::ElevatorSim m_elevatorSim{m_elevatorGearbox,
+                                      kElevatorGearing,
+                                      kCarriageMass,
+                                      kElevatorDrumRadius,
+                                      0.0_m,
+                                      Robot::kFullHeight,
+                                      true};
+  frc::sim::PWMSim m_motorSim{Robot::kMotorChannel};
+  frc::sim::AnalogInputSim m_analogSim{Robot::kPotChannel};
+  frc::sim::JoystickSim m_joystickSim{Robot::kJoystickChannel};
+  int32_t m_callback;
+  int32_t m_port;
+
+ public:
+  void SimPeriodicBefore() {
+    m_elevatorSim.SetInputVoltage(m_motorSim.GetSpeed() *
+                                  frc::RobotController::GetBatteryVoltage());
+    m_elevatorSim.Update(20_ms);
+
+    /*
+    meters = (v / 5v) * range
+    meters / range = v / 5v
+    5v * (meters / range) = v
+     */
+    m_analogSim.SetVoltage(
+        (frc::RobotController::GetVoltage5V() *
+         (m_elevatorSim.GetPosition().value() / Robot::kFullHeight))
+            .value());
+  }
+
+  static void CallSimPeriodicBefore(void* param) {
+    static_cast<PotentiometerPIDTest*>(param)->SimPeriodicBefore();
+  }
+
+  void SetUp() override {
+    frc::sim::PauseTiming();
+    frc::sim::DriverStationSim::ResetData();
+
+    m_joystickSim.SetButtonCount(12);
+
+    m_callback =
+        HALSIM_RegisterSimPeriodicBeforeCallback(CallSimPeriodicBefore, this);
+
+    m_thread = std::thread([&] { m_robot.StartCompetition(); });
+    frc::sim::StepTiming(0.0_ms);  // Wait for Notifiers
+  }
+
+  void TearDown() override {
+    m_robot.EndCompetition();
+    m_thread->join();
+
+    HALSIM_CancelSimPeriodicBeforeCallback(m_callback);
+    m_analogSim.ResetData();
+    m_motorSim.ResetData();
+  }
+};
+
+TEST_F(PotentiometerPIDTest, Teleop) {
+  // teleop init
+  {
+    frc::sim::DriverStationSim::SetAutonomous(false);
+    frc::sim::DriverStationSim::SetEnabled(true);
+    frc::sim::DriverStationSim::NotifyNewData();
+
+    EXPECT_TRUE(m_motorSim.GetInitialized());
+    EXPECT_TRUE(m_analogSim.GetInitialized());
+  }
+
+  // first setpoint
+  {
+    // advance 50 timesteps
+    frc::sim::StepTiming(1_s);
+
+    EXPECT_NEAR(Robot::kSetpoints[0].value(),
+                m_elevatorSim.GetPosition().value(), 0.1);
+  }
+
+  // second setpoint
+  {
+    // press button to advance setpoint
+    m_joystickSim.SetTrigger(true);
+    m_joystickSim.NotifyNewData();
+
+    // advance 50 timesteps
+    frc::sim::StepTiming(1_s);
+
+    EXPECT_NEAR(Robot::kSetpoints[1].value(),
+                m_elevatorSim.GetPosition().value(), 0.1);
+  }
+
+  // we need to unpress the button
+  {
+    m_joystickSim.SetTrigger(false);
+    m_joystickSim.NotifyNewData();
+
+    // advance 10 timesteps
+    frc::sim::StepTiming(0.2_s);
+  }
+
+  // third setpoint
+  {
+    // press button to advance setpoint
+    m_joystickSim.SetTrigger(true);
+    m_joystickSim.NotifyNewData();
+
+    // advance 50 timesteps
+    frc::sim::StepTiming(1_s);
+
+    EXPECT_NEAR(Robot::kSetpoints[2].value(),
+                m_elevatorSim.GetPosition().value(), 0.1);
+  }
+
+  // we need to unpress the button
+  {
+    m_joystickSim.SetTrigger(false);
+    m_joystickSim.NotifyNewData();
+
+    // advance 10 timesteps
+    frc::sim::StepTiming(0.2_s);
+  }
+
+  // rollover: first setpoint
+  {
+    // press button to advance setpoint
+    m_joystickSim.SetTrigger(true);
+    m_joystickSim.NotifyNewData();
+
+    // advance 60 timesteps
+    frc::sim::StepTiming(1.2_s);
+    EXPECT_NEAR(Robot::kSetpoints[0].value(),
+                m_elevatorSim.GetPosition().value(), 0.1);
+  }
+}
--- a/wpilibcExamples/src/test/cpp/examples/PotentiometerPID/cpp/main.cpp
+++ b/wpilibcExamples/src/test/cpp/examples/PotentiometerPID/cpp/main.cpp
@@ -0,0 +1,17 @@
+// 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.
+
+#include <hal/HALBase.h>
+
+#include "gtest/gtest.h"
+
+/**
+ * Runs all unit tests.
+ */
+int main(int argc, char** argv) {
+  HAL_Initialize(500, 0);
+  ::testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  return ret;
+}
--- a/wpilibcExamples/src/test/cpp/examples/UltrasonicPID/cpp/UltrasonicPIDTest.cpp
+++ b/wpilibcExamples/src/test/cpp/examples/UltrasonicPID/cpp/UltrasonicPIDTest.cpp
@@ -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.
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <thread>
+
+#include <frc/RobotController.h>
+#include <frc/simulation/DifferentialDrivetrainSim.h>
+#include <frc/simulation/DriverStationSim.h>
+#include <frc/simulation/PWMSim.h>
+#include <frc/simulation/SimHooks.h>
+#include <frc/simulation/UltrasonicSim.h>
+#include <frc/system/plant/DCMotor.h>
+#include <frc/system/plant/LinearSystemId.h>
+#include <hal/simulation/MockHooks.h>
+#include <units/angle.h>
+#include <units/length.h>
+#include <units/mass.h>
+#include <units/time.h>
+
+#include "Robot.h"
+
+class UltrasonicPIDTest : public testing::TestWithParam<double> {
+  frc::DCMotor m_gearbox = frc::DCMotor::Falcon500(2);
+  static constexpr auto kGearing =
+      frc::sim::DifferentialDrivetrainSim::KitbotGearing::k10p71;
+  static constexpr auto kvLinear = 1.98 * 1_V / 1_mps;
+  static constexpr auto kaLinear = 0.2 * 1_V / 1_mps_sq;
+  static constexpr auto kvVoltAngular = 1.5 * 1_V / 1_rad_per_s;
+  static constexpr auto kaAngular = 0.3 * 1_V / 1_rad_per_s_sq;
+  static constexpr auto kWheelDiameter = 0.15_m;
+  static constexpr auto kTrackwidth = 0.7_m;
+
+  Robot m_robot;
+  std::optional<std::thread> m_thread;
+
+ protected:
+  frc::sim::DifferentialDrivetrainSim m_driveSim{
+      frc::LinearSystemId::IdentifyDrivetrainSystem(
+          kvLinear, kaLinear, kvVoltAngular, kaAngular, kTrackwidth),
+      kTrackwidth, m_gearbox, kGearing, kWheelDiameter / 2.0};
+  frc::sim::PWMSim m_leftMotorSim{Robot::kLeftMotorPort};
+  frc::sim::PWMSim m_rightMotorSim{Robot::kRightMotorPort};
+  frc::sim::UltrasonicSim m_ultrasonicSim{Robot::kUltrasonicPingPort,
+                                          Robot::kUltrasonicEchoPort};
+  int32_t m_callback;
+
+  units::millimeter_t m_distance;
+
+ public:
+  void SimPeriodicBefore() {
+    m_driveSim.SetInputs(
+        m_leftMotorSim.GetSpeed() * frc::RobotController::GetBatteryVoltage(),
+        m_rightMotorSim.GetSpeed() * frc::RobotController::GetBatteryVoltage());
+    m_driveSim.Update(20_ms);
+
+    auto startingDistance = units::meter_t{GetParam()};
+    m_distance = startingDistance - m_driveSim.GetLeftPosition();
+
+    m_ultrasonicSim.SetRange(m_distance);
+  }
+
+  static void CallSimPeriodicBefore(void* param) {
+    static_cast<UltrasonicPIDTest*>(param)->SimPeriodicBefore();
+  }
+
+  void SetUp() override {
+    frc::sim::PauseTiming();
+    frc::sim::DriverStationSim::ResetData();
+
+    m_callback =
+        HALSIM_RegisterSimPeriodicBeforeCallback(CallSimPeriodicBefore, this);
+
+    m_thread = std::thread([&] { m_robot.StartCompetition(); });
+    frc::sim::StepTiming(0.0_ms);  // Wait for Notifiers
+  }
+
+  void TearDown() override {
+    m_robot.EndCompetition();
+    m_thread->join();
+
+    HALSIM_CancelSimPeriodicBeforeCallback(m_callback);
+    m_leftMotorSim.ResetData();
+    m_rightMotorSim.ResetData();
+  }
+};
+
+TEST_P(UltrasonicPIDTest, Auto) {
+  // auto init
+  {
+    frc::sim::DriverStationSim::SetAutonomous(true);
+    frc::sim::DriverStationSim::SetEnabled(true);
+    frc::sim::DriverStationSim::NotifyNewData();
+
+    EXPECT_TRUE(m_leftMotorSim.GetInitialized());
+    EXPECT_TRUE(m_rightMotorSim.GetInitialized());
+  }
+
+  {
+    // advance 100 timesteps
+    frc::sim::StepTiming(2_s);
+
+    EXPECT_NEAR(Robot::kHoldDistance.value(), m_distance.value(), 10.0);
+  }
+}
+
+INSTANTIATE_TEST_SUITE_P(UltrasonicPIDTests, UltrasonicPIDTest,
+                         testing::Values(1.3, 0.5, 5.0));
--- a/wpilibcExamples/src/test/cpp/examples/UltrasonicPID/cpp/main.cpp
+++ b/wpilibcExamples/src/test/cpp/examples/UltrasonicPID/cpp/main.cpp
@@ -0,0 +1,17 @@
+// 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.
+
+#include <hal/HALBase.h>
+
+#include "gtest/gtest.h"
+
+/**
+ * Runs all unit tests.
+ */
+int main(int argc, char** argv) {
+  HAL_Initialize(500, 0);
+  ::testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  return ret;
+}