[examples] Add example programs for AprilTags detection (#4932)

Co-authored-by: Peter Johnson <johnson.peter@gmail.com>

apriltag/CMakeLists.txt

@@ -40,7 +40,7 @@ if (WITH_JAVA)
   set(CMAKE_JAVA_INCLUDE_PATH apriltag.jar ${EJML_JARS} ${JACKSON_JARS})
 
   file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
-  file(GLOB_RECURSE JAVA_RESOURCES src/main/native/resources/*.json)
+  file(GLOB_RECURSE JAVA_RESOURCES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} src/main/native/resources/*.json)
   add_jar(apriltag_jar
     SOURCES ${JAVA_SOURCES}
     RESOURCES NAMESPACE "edu/wpi/first/apriltag" ${JAVA_RESOURCES}

apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java

@@ -37,6 +37,9 @@ import java.util.Optional;
  * at the bottom-right corner of the blue alliance wall. {@link #setOrigin(OriginPosition)} can be
  * used to change the poses returned from {@link AprilTagFieldLayout#getTagPose(int)} to be from the
  * perspective of a specific alliance.
+ *
+ * <p>Tag poses represent the center of the tag, with a zero rotation representing a tag that is
+ * upright and facing away from the (blue) alliance wall (that is, towards the opposing alliance).
  */
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)

apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java

@@ -5,12 +5,13 @@
 package edu.wpi.first.apriltag;
 
 public enum AprilTagFields {
-  k2022RapidReact("2022-rapidreact.json");
+  k2022RapidReact("2022-rapidreact.json"),
+  k2023ChargedUp("2023-chargedup.json");
 
   public static final String kBaseResourceDir = "/edu/wpi/first/apriltag/";
 
   /** Alias to the current game. */
-  public static final AprilTagFields kDefaultField = k2022RapidReact;
+  public static final AprilTagFields kDefaultField = k2023ChargedUp;
 
   public final String m_resourceFile;
 

apriltag/src/main/native/cpp/AprilTagFields.cpp

@@ -10,6 +10,7 @@ namespace frc {
 
 // C++ generated from resource files
 std::string_view GetResource_2022_rapidreact_json();
+std::string_view GetResource_2023_chargedup_json();
 
 AprilTagFieldLayout LoadAprilTagLayoutField(AprilTagField field) {
   std::string_view fieldString;
@@ -17,6 +18,9 @@ AprilTagFieldLayout LoadAprilTagLayoutField(AprilTagField field) {
     case AprilTagField::k2022RapidReact:
       fieldString = GetResource_2022_rapidreact_json();
       break;
+    case AprilTagField::k2023ChargedUp:
+      fieldString = GetResource_2023_chargedup_json();
+      break;
     case AprilTagField::kNumFields:
       throw std::invalid_argument("Invalid Field");
   }

apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h

@@ -34,7 +34,11 @@ namespace frc {
  * Pose3ds in the JSON are measured using the normal FRC coordinate system, NWU
  * with the origin at the bottom-right corner of the blue alliance wall.
  * SetOrigin(OriginPosition) can be used to change the poses returned from
- * GetTagPose(int) to be from the perspective of a specific alliance. */
+ * GetTagPose(int) to be from the perspective of a specific alliance.
+ *
+ * Tag poses represent the center of the tag, with a zero rotation representing
+ * a tag that is upright and facing away from the (blue) alliance wall (that is,
+ * towards the opposing alliance). */
 class WPILIB_DLLEXPORT AprilTagFieldLayout {
  public:
   enum class OriginPosition {

apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h

@@ -14,6 +14,7 @@ namespace frc {
 
 enum class AprilTagField {
   k2022RapidReact,
+  k2023ChargedUp,
 
   // This is a placeholder for denoting the last supported field. This should
   // always be the last entry in the enum and should not be used by users

apriltag/src/main/native/resources/edu/wpi/first/apriltag/2023-chargedup.json

@@ -0,0 +1,152 @@
+{
+  "tags": [
+    {
+      "ID": 1,
+      "pose": {
+        "translation": {
+          "x": 15.513558,
+          "y": 1.071626,
+          "z": 0.462788
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 2,
+      "pose": {
+        "translation": {
+          "x": 15.513558,
+          "y": 2.748026,
+          "z": 0.462788
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 3,
+      "pose": {
+        "translation": {
+          "x": 15.513558,
+          "y": 4.424426,
+          "z": 0.462788
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 4,
+      "pose": {
+        "translation": {
+          "x": 16.178784,
+          "y": 6.749796,
+          "z": 0.695452
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 5,
+      "pose": {
+        "translation": {
+          "x": 0.36195,
+          "y": 6.749796,
+          "z": 0.695452
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 6,
+      "pose": {
+        "translation": {
+          "x": 1.02743,
+          "y": 4.424426,
+          "z": 0.462788
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 7,
+      "pose": {
+        "translation": {
+          "x": 1.02743,
+          "y": 2.748026,
+          "z": 0.462788
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 8,
+      "pose": {
+        "translation": {
+          "x": 1.02743,
+          "y": 1.071626,
+          "z": 0.462788
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    }
+  ],
+  "field": {
+    "length": 16.54175,
+    "width": 8.0137
+  }
+}

fieldImages/CMakeLists.txt

@@ -11,7 +11,7 @@ if (WITH_JAVA)
     set(CMAKE_JAVA_INCLUDE_PATH fieldImages.jar ${JACKSON_JARS})
 
     file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
-    file(GLOB_RECURSE JAVA_RESOURCES src/main/native/resources/*.json src/main/native/resources/*.png src/main/native/resources/*.jpg)
+    file(GLOB_RECURSE JAVA_RESOURCES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} src/main/native/resources/*.json src/main/native/resources/*.png src/main/native/resources/*.jpg)
     add_jar(field_images_jar SOURCES ${JAVA_SOURCES} RESOURCES NAMESPACE "edu/wpi/first/fields" ${JAVA_RESOURCES} OUTPUT_NAME fieldImages)
 
     get_property(FIELD_IMAGES_JAR_FILE TARGET field_images_jar PROPERTY JAR_FILE)

fieldImages/src/main/java/edu/wpi/fields/Fields.java

@@ -14,12 +14,13 @@ public enum Fields {
   k2021GalacticSearchA("2021-galacticsearcha.json"),
   k2021GalacticSearchB("2021-galacticsearchb.json"),
   k2021Slalom("2021-slalompath.json"),
-  k2022RapidReact("2022-rapidreact.json");
+  k2022RapidReact("2022-rapidreact.json"),
+  k2023ChargedUp("2023-chargedup.json");
 
   public static final String kBaseResourceDir = "/edu/wpi/first/fields/";
 
   /** Alias to the current game. */
-  public static final Fields kDefaultField = k2022RapidReact;
+  public static final Fields kDefaultField = k2023ChargedUp;
 
   public final String m_resourceFile;
 

fieldImages/src/main/native/include/fields/2023-chargedup.h

@@ -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.
+
+#pragma once
+
+#include <string_view>
+
+namespace fields {
+std::string_view GetResource_2023_chargedup_json();
+std::string_view GetResource_2023_field_png();
+}  // namespace fields

fieldImages/src/main/native/resources/edu/wpi/first/fields/2023-chargedup.json

@@ -0,0 +1,10 @@
+{
+    "game": "Charged Up",
+    "field-image": "2023-field.png",
+    "field-corners": {
+      "top-left": [46, 36],
+      "bottom-right": [1088, 544]
+    },
+    "field-size": [54.27083, 26.2916],
+    "field-unit": "foot"
+  }

hal/src/main/java/edu/wpi/first/hal/SimDevice.java

@@ -106,6 +106,15 @@ public class SimDevice implements AutoCloseable {
     return m_handle;
   }
 
+  /**
+   * Get the name of the simulated device.
+   *
+   * @return the name
+   */
+  public String getName() {
+    return SimDeviceJNI.getSimDeviceName(m_handle);
+  }
+
   /**
    * Creates a value on the simulated device.
    *

hal/src/main/java/edu/wpi/first/hal/SimDeviceJNI.java

@@ -33,6 +33,14 @@ public class SimDeviceJNI extends JNIWrapper {
    */
   public static native void freeSimDevice(int handle);
 
+  /**
+   * Get the name of a simulated device.
+   *
+   * @param handle simulated device handle
+   * @return name of the simulated device
+   */
+  public static native String getSimDeviceName(int handle);
+
   private static native int createSimValueNative(
       int device, String name, int direction, int type, long value1, double value2);
 

hal/src/main/java/edu/wpi/first/hal/simulation/EncoderDataJNI.java

@@ -79,6 +79,15 @@ public class EncoderDataJNI extends JNIWrapper {
 
   public static native void setSamplesToAverage(int index, int samplesToAverage);
 
+  public static native int registerDistancePerPulseCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
+  public static native void cancelDistancePerPulseCallback(int index, int uid);
+
+  public static native double getDistancePerPulse(int index);
+
+  public static native void setDistancePerPulse(int index, double distancePerPulse);
+
   public static native void setDistance(int index, double distance);
 
   public static native double getDistance(int index);

hal/src/main/native/athena/SimDevice.cpp

@@ -12,6 +12,10 @@ HAL_SimDeviceHandle HAL_CreateSimDevice(const char* name) {
 
 void HAL_FreeSimDevice(HAL_SimDeviceHandle handle) {}
 
+const char* HAL_GetSimDeviceName(HAL_SimDeviceHandle handle) {
+  return "";
+}
+
 HAL_SimValueHandle HAL_CreateSimValue(HAL_SimDeviceHandle device,
                                       const char* name, int32_t direction,
                                       const struct HAL_Value* initialValue) {

hal/src/main/native/cpp/jni/SimDeviceJNI.cpp

@@ -63,6 +63,18 @@ Java_edu_wpi_first_hal_SimDeviceJNI_freeSimDevice
   HAL_FreeSimDevice(handle);
 }
 
+/*
+ * Class:     edu_wpi_first_hal_SimDeviceJNI
+ * Method:    getSimDeviceName
+ * Signature: (I)Ljava/lang/String;
+ */
+JNIEXPORT jstring JNICALL
+Java_edu_wpi_first_hal_SimDeviceJNI_getSimDeviceName
+  (JNIEnv* env, jclass, jint handle)
+{
+  return MakeJString(env, HAL_GetSimDeviceName(handle));
+}
+
 /*
  * Class:     edu_wpi_first_hal_SimDeviceJNI
  * Method:    createSimValueNative

hal/src/main/native/cpp/jni/simulation/EncoderDataJNI.cpp

@@ -412,6 +412,56 @@ Java_edu_wpi_first_hal_simulation_EncoderDataJNI_setSamplesToAverage
   HALSIM_SetEncoderSamplesToAverage(index, value);
 }
 
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    registerDistancePerPulseCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_registerDistancePerPulseCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterEncoderDistancePerPulseCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    cancelDistancePerPulseCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_cancelDistancePerPulseCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelEncoderDistancePerPulseCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    getDistancePerPulse
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_getDistancePerPulse
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetEncoderDistancePerPulse(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    setDistancePerPulse
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_setDistancePerPulse
+  (JNIEnv*, jclass, jint index, jdouble value)
+{
+  HALSIM_SetEncoderDistancePerPulse(index, value);
+}
+
 /*
  * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
  * Method:    setDistance

hal/src/main/native/include/hal/SimDevice.h

@@ -9,6 +9,7 @@
 #ifdef __cplusplus
 #include <initializer_list>
 #include <span>
+#include <string>
 #endif
 
 #include "hal/Types.h"
@@ -66,6 +67,14 @@ HAL_SimDeviceHandle HAL_CreateSimDevice(const char* name);
  */
 void HAL_FreeSimDevice(HAL_SimDeviceHandle handle);
 
+/**
+ * Get the name of a simulated device
+ *
+ * @param handle simulated device handle
+ * @return name of the simulated device
+ */
+const char* HAL_GetSimDeviceName(HAL_SimDeviceHandle handle);
+
 /**
  * Creates a value on a simulated device.
  *
@@ -731,6 +740,15 @@ class SimDevice {
    */
   operator HAL_SimDeviceHandle() const { return m_handle; }  // NOLINT
 
+  /**
+   * Get the name of the simulated device.
+   *
+   * @return name
+   */
+  std::string GetName() const {
+    return std::string(HAL_GetSimDeviceName(m_handle));
+  }
+
   /**
    * Creates a value on the simulated device.
    *

hal/src/main/native/sim/Encoder.cpp

@@ -363,7 +363,7 @@ double HAL_GetEncoderDistancePerPulse(HAL_EncoderHandle encoderHandle,
     return 0.0;
   }
 
-  return encoder->distancePerPulse;
+  return SimEncoderData[encoder->index].distancePerPulse;
 }
 
 HAL_EncoderEncodingType HAL_GetEncoderEncodingType(

hal/src/main/native/sim/SimDevice.cpp

@@ -26,6 +26,10 @@ void HAL_FreeSimDevice(HAL_SimDeviceHandle handle) {
   SimSimDeviceData->FreeDevice(handle);
 }
 
+const char* HAL_GetSimDeviceName(HAL_SimDeviceHandle handle) {
+  return SimSimDeviceData->GetDeviceName(handle);
+}
+
 HAL_SimValueHandle HAL_CreateSimValue(HAL_SimDeviceHandle device,
                                       const char* name, int32_t direction,
                                       const struct HAL_Value* initialValue) {

ntcore/src/main/native/cpp/LocalStorage.cpp

@@ -96,6 +96,7 @@ struct TopicData {
   NT_Entry entry{0};  // cached entry for GetEntry()
 
   bool onNetwork{false};  // true if there are any remote publishers
+  bool lastValueFromNetwork{false};
 
   wpi::SmallVector<DataLoggerEntry, 1> datalogs;
   NT_Type datalogType{NT_UNASSIGNED};
@@ -484,6 +485,7 @@ void LSImpl::CheckReset(TopicData* topic) {
   }
   topic->lastValue = {};
   topic->lastValueNetwork = {};
+  topic->lastValueFromNetwork = false;
   topic->type = NT_UNASSIGNED;
   topic->typeStr.clear();
   topic->flags = 0;
@@ -503,6 +505,7 @@ bool LSImpl::SetValue(TopicData* topic, const Value& value,
     // TODO: notify option even if older value
     topic->type = value.type();
     topic->lastValue = value;
+    topic->lastValueFromNetwork = false;
     NotifyValue(topic, eventFlags, isDuplicate, publisher);
   }
   if (!isDuplicate && topic->datalogType == value.type()) {
@@ -858,6 +861,17 @@ SubscriberData* LSImpl::AddLocalSubscriber(TopicData* topic,
     DEBUG4("-> NetworkSubscribe({})", topic->name);
     m_network->Subscribe(subscriber->handle, {{topic->name}}, config);
   }
+
+  // queue current value
+  if (subscriber->active) {
+    if (!topic->lastValueFromNetwork && !config.disableLocal) {
+      subscriber->pollStorage.emplace_back(topic->lastValue);
+      subscriber->handle.Set();
+    } else if (topic->lastValueFromNetwork && !config.disableRemote) {
+      subscriber->pollStorage.emplace_back(topic->lastValueNetwork);
+      subscriber->handle.Set();
+    }
+  }
   return subscriber;
 }
 
@@ -1376,6 +1390,7 @@ void LocalStorage::NetworkSetValue(NT_Topic topicHandle, const Value& value) {
     if (m_impl->SetValue(topic, value, NT_EVENT_VALUE_REMOTE,
                          value == topic->lastValue, nullptr)) {
       topic->lastValueNetwork = value;
+      topic->lastValueFromNetwork = true;
     }
   }
 }

ntcore/src/main/native/cpp/NetworkClient.cpp

@@ -215,7 +215,7 @@ NCImpl3::NCImpl3(int inst, std::string_view id,
     m_sendValuesTimer->timeout.connect([this] {
       if (m_clientImpl) {
         HandleLocal();
-        m_clientImpl->SendPeriodic(m_loop.Now().count());
+        m_clientImpl->SendPeriodic(m_loop.Now().count(), false);
       }
     });
 
@@ -224,7 +224,7 @@ NCImpl3::NCImpl3(int inst, std::string_view id,
     m_flush->wakeup.connect([this] {
       if (m_clientImpl) {
         HandleLocal();
-        m_clientImpl->SendPeriodic(m_loop.Now().count());
+        m_clientImpl->SendPeriodic(m_loop.Now().count(), true);
       }
     });
     m_flushAtomic = m_flush.get();
@@ -355,7 +355,7 @@ NCImpl4::NCImpl4(
     m_sendValuesTimer->timeout.connect([this] {
       if (m_clientImpl) {
         HandleLocal();
-        m_clientImpl->SendValues(m_loop.Now().count());
+        m_clientImpl->SendValues(m_loop.Now().count(), false);
       }
     });
 
@@ -364,7 +364,7 @@ NCImpl4::NCImpl4(
     m_flush->wakeup.connect([this] {
       if (m_clientImpl) {
         HandleLocal();
-        m_clientImpl->SendValues(m_loop.Now().count());
+        m_clientImpl->SendValues(m_loop.Now().count(), true);
       }
     });
     m_flushAtomic = m_flush.get();
@@ -501,16 +501,15 @@ void NetworkClient::StopDSClient() {
 }
 
 void NetworkClient::FlushLocal() {
-  m_impl->m_loopRunner.ExecAsync([this](uv::Loop&) { m_impl->HandleLocal(); });
+  if (auto async = m_impl->m_flushLocalAtomic.load(std::memory_order_relaxed)) {
+    async->UnsafeSend();
+  }
 }
 
 void NetworkClient::Flush() {
-  m_impl->m_loopRunner.ExecAsync([this](uv::Loop&) {
-    m_impl->HandleLocal();
-    if (m_impl->m_clientImpl) {
-      m_impl->m_clientImpl->SendValues(m_impl->m_loop.Now().count());
-    }
-  });
+  if (auto async = m_impl->m_flushAtomic.load(std::memory_order_relaxed)) {
+    async->UnsafeSend();
+  }
 }
 
 class NetworkClient3::Impl final : public NCImpl3 {

ntcore/src/main/native/cpp/net/ClientImpl.cpp

@@ -56,7 +56,7 @@ class CImpl : public ServerMessageHandler {
   void ProcessIncomingBinary(std::span<const uint8_t> data);
   void HandleLocal(std::vector<ClientMessage>&& msgs);
   bool SendControl(uint64_t curTimeMs);
-  void SendValues(uint64_t curTimeMs);
+  void SendValues(uint64_t curTimeMs, bool flush);
   void SendInitialValues();
   bool CheckNetworkReady();
 
@@ -237,7 +237,7 @@ bool CImpl::SendControl(uint64_t curTimeMs) {
   return true;
 }
 
-void CImpl::SendValues(uint64_t curTimeMs) {
+void CImpl::SendValues(uint64_t curTimeMs, bool flush) {
   DEBUG4("SendValues({})", curTimeMs);
 
   // can't send value updates until we have a RTT
@@ -254,7 +254,8 @@ void CImpl::SendValues(uint64_t curTimeMs) {
   bool checkedNetwork = false;
   auto writer = m_wire.SendBinary();
   for (auto&& pub : m_publishers) {
-    if (pub && !pub->outValues.empty() && curTimeMs >= pub->nextSendMs) {
+    if (pub && !pub->outValues.empty() &&
+        (flush || curTimeMs >= pub->nextSendMs)) {
       for (auto&& val : pub->outValues) {
         if (!checkedNetwork) {
           if (!CheckNetworkReady()) {
@@ -474,8 +475,8 @@ void ClientImpl::SendControl(uint64_t curTimeMs) {
   m_impl->m_wire.Flush();
 }
 
-void ClientImpl::SendValues(uint64_t curTimeMs) {
-  m_impl->SendValues(curTimeMs);
+void ClientImpl::SendValues(uint64_t curTimeMs, bool flush) {
+  m_impl->SendValues(curTimeMs, flush);
   m_impl->m_wire.Flush();
 }
 

ntcore/src/main/native/cpp/net/ClientImpl.h

@@ -44,7 +44,7 @@ class ClientImpl {
   void HandleLocal(std::vector<ClientMessage>&& msgs);
 
   void SendControl(uint64_t curTimeMs);
-  void SendValues(uint64_t curTimeMs);
+  void SendValues(uint64_t curTimeMs, bool flush);
 
   void SetLocal(LocalInterface* local);
   void SendInitial();

ntcore/src/main/native/cpp/net3/ClientImpl3.cpp

@@ -91,7 +91,7 @@ class CImpl : public MessageHandler3 {
 
   void ProcessIncoming(std::span<const uint8_t> data);
   void HandleLocal(std::span<const net::ClientMessage> msgs);
-  void SendPeriodic(uint64_t curTimeMs, bool initial);
+  void SendPeriodic(uint64_t curTimeMs, bool initial, bool flush);
   void SendValue(Writer& out, Entry* entry, const Value& value);
   bool CheckNetworkReady();
 
@@ -223,7 +223,7 @@ void CImpl::HandleLocal(std::span<const net::ClientMessage> msgs) {
   }
 }
 
-void CImpl::SendPeriodic(uint64_t curTimeMs, bool initial) {
+void CImpl::SendPeriodic(uint64_t curTimeMs, bool initial, bool flush) {
   DEBUG4("SendPeriodic({})", curTimeMs);
 
   // rate limit sends
@@ -258,7 +258,8 @@ void CImpl::SendPeriodic(uint64_t curTimeMs, bool initial) {
   // send any pending updates due to be sent
   bool checkedNetwork = false;
   for (auto&& pub : m_publishers) {
-    if (pub && !pub->outValues.empty() && curTimeMs >= pub->nextSendMs) {
+    if (pub && !pub->outValues.empty() &&
+        (flush || curTimeMs >= pub->nextSendMs)) {
       if (!checkedNetwork) {
         if (!CheckNetworkReady()) {
           return;
@@ -420,7 +421,7 @@ void CImpl::ServerHelloDone() {
   }
 
   // send initial assignments
-  SendPeriodic(m_initTimeMs, true);
+  SendPeriodic(m_initTimeMs, true, true);
 
   m_state = kStateRunning;
   m_setPeriodic(m_periodMs);
@@ -633,8 +634,8 @@ void ClientImpl3::HandleLocal(std::span<const net::ClientMessage> msgs) {
   m_impl->HandleLocal(msgs);
 }
 
-void ClientImpl3::SendPeriodic(uint64_t curTimeMs) {
-  m_impl->SendPeriodic(curTimeMs, false);
+void ClientImpl3::SendPeriodic(uint64_t curTimeMs, bool flush) {
+  m_impl->SendPeriodic(curTimeMs, false, flush);
 }
 
 void ClientImpl3::SetLocal(net::LocalInterface* local) {

ntcore/src/main/native/cpp/net3/ClientImpl3.h

@@ -38,7 +38,7 @@ class ClientImpl3 {
   void ProcessIncoming(std::span<const uint8_t> data);
   void HandleLocal(std::span<const net::ClientMessage> msgs);
 
-  void SendPeriodic(uint64_t curTimeMs);
+  void SendPeriodic(uint64_t curTimeMs, bool flush);
 
   void SetLocal(net::LocalInterface* local);
 

ntcore/src/test/native/cpp/LocalStorageTest.cpp

@@ -197,9 +197,6 @@ TEST_F(LocalStorageTest, SubscribeNoTypeLocalPubPre) {
   ASSERT_TRUE(value.IsBoolean());
   EXPECT_EQ(value.GetBoolean(), true);
   EXPECT_EQ(value.time(), 5);
-
-  auto vals = storage.ReadQueueValue(sub);  // read queue won't get anything
-  ASSERT_TRUE(vals.empty());
 }
 
 TEST_F(LocalStorageTest, EntryNoTypeLocalSet) {
@@ -916,4 +913,48 @@ TEST_F(LocalStorageTest, EntryExcludeSelf) {
               ElementsAre(TSEq<TimestampedDouble>(2.0, 60)));
 }
 
+TEST_F(LocalStorageTest, ReadQueueInitialLocal) {
+  EXPECT_CALL(network, Publish(_, _, _, _, _, _));
+  EXPECT_CALL(network, SetValue(_, _));
+  EXPECT_CALL(network, Subscribe(_, _, _)).Times(3);
+
+  auto pub = storage.Publish(fooTopic, NT_DOUBLE, "double", {}, {});
+  storage.SetEntryValue(pub, Value::MakeDouble(1.0, 50));
+
+  auto subBoth =
+      storage.Subscribe(fooTopic, NT_DOUBLE, "double", kDefaultPubSubOptions);
+  auto subLocal =
+      storage.Subscribe(fooTopic, NT_DOUBLE, "double", {.disableRemote = true});
+  auto subRemote =
+      storage.Subscribe(fooTopic, NT_DOUBLE, "double", {.disableLocal = true});
+
+  EXPECT_THAT(storage.ReadQueueDouble(subBoth),
+              ElementsAre(TSEq<TimestampedDouble>(1.0, 50)));
+  EXPECT_THAT(storage.ReadQueueDouble(subLocal),
+              ElementsAre(TSEq<TimestampedDouble>(1.0, 50)));
+  EXPECT_THAT(storage.ReadQueueDouble(subRemote), IsEmpty());
+}
+
+TEST_F(LocalStorageTest, ReadQueueInitialRemote) {
+  EXPECT_CALL(network, Subscribe(_, _, _)).Times(3);
+
+  auto remoteTopic =
+      storage.NetworkAnnounce("foo", "double", wpi::json::object(), 0);
+  storage.NetworkSetValue(remoteTopic, Value::MakeDouble(2.0, 60));
+
+  auto subBoth =
+      storage.Subscribe(fooTopic, NT_DOUBLE, "double", kDefaultPubSubOptions);
+  auto subLocal =
+      storage.Subscribe(fooTopic, NT_DOUBLE, "double", {.disableRemote = true});
+  auto subRemote =
+      storage.Subscribe(fooTopic, NT_DOUBLE, "double", {.disableLocal = true});
+
+  // network set
+  EXPECT_THAT(storage.ReadQueueDouble(subBoth),
+              ElementsAre(TSEq<TimestampedDouble>(2.0, 60)));
+  EXPECT_THAT(storage.ReadQueueDouble(subRemote),
+              ElementsAre(TSEq<TimestampedDouble>(2.0, 60)));
+  EXPECT_THAT(storage.ReadQueueDouble(subLocal), IsEmpty());
+}
+
 }  // namespace nt

shared/opencv.gradle

@@ -1,4 +1,4 @@
-def opencvVersion = '4.6.0-2'
+def opencvVersion = '4.6.0-3'
 
 if (project.hasProperty('useCpp') && project.useCpp) {
     model {

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java

@@ -602,7 +602,7 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
   public void requireNotComposed(Command command) {
     if (m_composedCommands.contains(command)) {
       throw new IllegalArgumentException(
-          "Commands that have been composed may not be added to another composition or scheduled"
+          "Commands that have been composed may not be added to another composition or scheduled "
               + "individually!");
     }
   }
@@ -616,7 +616,7 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
   public void requireNotComposed(Collection<Command> commands) {
     if (!Collections.disjoint(commands, getComposedCommands())) {
       throw new IllegalArgumentException(
-          "Commands that have been composed may not be added to another composition or scheduled"
+          "Commands that have been composed may not be added to another composition or scheduled "
               + "individually!");
     }
   }

wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp

@@ -226,7 +226,7 @@ CommandPtr CommandPtr::WithName(std::string_view name) && {
   return std::move(wrapper).ToPtr();
 }
 
-CommandBase* CommandPtr::get() const {
+CommandBase* CommandPtr::get() const& {
   AssertValid();
   return m_ptr.get();
 }
@@ -236,27 +236,27 @@ std::unique_ptr<CommandBase> CommandPtr::Unwrap() && {
   return std::move(m_ptr);
 }
 
-void CommandPtr::Schedule() const {
+void CommandPtr::Schedule() const& {
   AssertValid();
   CommandScheduler::GetInstance().Schedule(*this);
 }
 
-void CommandPtr::Cancel() const {
+void CommandPtr::Cancel() const& {
   AssertValid();
   CommandScheduler::GetInstance().Cancel(*this);
 }
 
-bool CommandPtr::IsScheduled() const {
+bool CommandPtr::IsScheduled() const& {
   AssertValid();
   return CommandScheduler::GetInstance().IsScheduled(*this);
 }
 
-bool CommandPtr::HasRequirement(Subsystem* requirement) const {
+bool CommandPtr::HasRequirement(Subsystem* requirement) const& {
   AssertValid();
   return m_ptr->HasRequirement(requirement);
 }
 
-CommandPtr::operator bool() const {
+CommandPtr::operator bool() const& {
   return m_ptr.operator bool();
 }
 

wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp

@@ -433,9 +433,10 @@ void CommandScheduler::OnCommandFinish(Action action) {
 
 void CommandScheduler::RequireUngrouped(const Command* command) {
   if (command->IsComposed()) {
-    throw FRC_MakeError(
-        frc::err::CommandIllegalUse,
-        "Commands cannot be added to more than one CommandGroup");
+    throw FRC_MakeError(frc::err::CommandIllegalUse,
+                        "Commands that have been composed may not be added to "
+                        "another composition or scheduled "
+                        "individually!");
   }
 }
 

wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h

@@ -232,7 +232,10 @@ class CommandPtr final {
   /**
    * Get a raw pointer to the held command.
    */
-  CommandBase* get() const;
+  CommandBase* get() const&;
+
+  // Prevent calls on a temporary, as the returned pointer would be invalid
+  CommandBase* get() && = delete;
 
   /**
    * Convert to the underlying unique_ptr.
@@ -242,13 +245,19 @@ class CommandPtr final {
   /**
    * Schedules this command.
    */
-  void Schedule() const;
+  void Schedule() const&;
+
+  // Prevent calls on a temporary, as the returned pointer would be invalid
+  void Schedule() && = delete;
 
   /**
    * Cancels this command. Will call End(true). Commands will be canceled
    * regardless of interruption behavior.
    */
-  void Cancel() const;
+  void Cancel() const&;
+
+  // Prevent calls on a temporary, as the returned pointer would be invalid
+  void Cancel() && = delete;
 
   /**
    * Whether or not the command is currently scheduled. Note that this does not
@@ -257,7 +266,10 @@ class CommandPtr final {
    *
    * @return Whether the command is scheduled.
    */
-  bool IsScheduled() const;
+  bool IsScheduled() const&;
+
+  // Prevent calls on a temporary, as the returned pointer would be invalid
+  void IsScheduled() && = delete;
 
   /**
    * Whether the command requires a given subsystem.  Named "HasRequirement"
@@ -267,12 +279,18 @@ class CommandPtr final {
    * @param requirement the subsystem to inquire about
    * @return whether the subsystem is required
    */
-  bool HasRequirement(Subsystem* requirement) const;
+  bool HasRequirement(Subsystem* requirement) const&;
+
+  // Prevent calls on a temporary, as the returned pointer would be invalid
+  void HasRequirement(Subsystem* requirement) && = delete;
 
   /**
    * Check if this CommandPtr object is valid and wasn't moved-from.
    */
-  explicit operator bool() const;
+  explicit operator bool() const&;
+
+  // Prevent calls on a temporary, as the returned pointer would be invalid
+  explicit operator bool() && = delete;
 
   /**
    * Convert a vector of CommandPtr objects to their underlying unique_ptrs.

wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h

@@ -19,71 +19,71 @@
 
 namespace frc2 {
 
+class TestSubsystem : public SubsystemBase {};
+
+/**
+ * NOTE: Moving mock objects causes EXPECT_CALL to not work correctly!
+ */
+class MockCommand : public CommandHelper<CommandBase, MockCommand> {
+ public:
+  MOCK_CONST_METHOD0(GetRequirements, wpi::SmallSet<Subsystem*, 4>());
+  MOCK_METHOD0(IsFinished, bool());
+  MOCK_CONST_METHOD0(RunsWhenDisabled, bool());
+  MOCK_METHOD0(Initialize, void());
+  MOCK_METHOD0(Execute, void());
+  MOCK_METHOD1(End, void(bool interrupted));
+
+  MockCommand() {
+    m_requirements = {};
+    EXPECT_CALL(*this, GetRequirements())
+        .WillRepeatedly(::testing::Return(m_requirements));
+    EXPECT_CALL(*this, IsFinished()).WillRepeatedly(::testing::Return(false));
+    EXPECT_CALL(*this, RunsWhenDisabled())
+        .WillRepeatedly(::testing::Return(true));
+  }
+
+  MockCommand(std::initializer_list<Subsystem*> requirements,
+              bool finished = false, bool runWhenDisabled = true) {
+    m_requirements.insert(requirements.begin(), requirements.end());
+    EXPECT_CALL(*this, GetRequirements())
+        .WillRepeatedly(::testing::Return(m_requirements));
+    EXPECT_CALL(*this, IsFinished())
+        .WillRepeatedly(::testing::Return(finished));
+    EXPECT_CALL(*this, RunsWhenDisabled())
+        .WillRepeatedly(::testing::Return(runWhenDisabled));
+  }
+
+  MockCommand(MockCommand&& other) {
+    EXPECT_CALL(*this, IsFinished())
+        .WillRepeatedly(::testing::Return(other.IsFinished()));
+    EXPECT_CALL(*this, RunsWhenDisabled())
+        .WillRepeatedly(::testing::Return(other.RunsWhenDisabled()));
+    std::swap(m_requirements, other.m_requirements);
+    EXPECT_CALL(*this, GetRequirements())
+        .WillRepeatedly(::testing::Return(m_requirements));
+  }
+
+  MockCommand(const MockCommand& other) : CommandHelper{other} {}
+
+  void SetFinished(bool finished) {
+    EXPECT_CALL(*this, IsFinished())
+        .WillRepeatedly(::testing::Return(finished));
+  }
+
+  ~MockCommand() {  // NOLINT
+    auto& scheduler = CommandScheduler::GetInstance();
+    scheduler.Cancel(this);
+  }
+
+ private:
+  wpi::SmallSet<Subsystem*, 4> m_requirements;
+};
+
 class CommandTestBase : public ::testing::Test {
  public:
   CommandTestBase();
 
-  class TestSubsystem : public SubsystemBase {};
-
  protected:
-  /**
-   * NOTE: Moving mock objects causes EXPECT_CALL to not work correctly!
-   */
-  class MockCommand : public CommandHelper<CommandBase, MockCommand> {
-   public:
-    MOCK_CONST_METHOD0(GetRequirements, wpi::SmallSet<Subsystem*, 4>());
-    MOCK_METHOD0(IsFinished, bool());
-    MOCK_CONST_METHOD0(RunsWhenDisabled, bool());
-    MOCK_METHOD0(Initialize, void());
-    MOCK_METHOD0(Execute, void());
-    MOCK_METHOD1(End, void(bool interrupted));
-
-    MockCommand() {
-      m_requirements = {};
-      EXPECT_CALL(*this, GetRequirements())
-          .WillRepeatedly(::testing::Return(m_requirements));
-      EXPECT_CALL(*this, IsFinished()).WillRepeatedly(::testing::Return(false));
-      EXPECT_CALL(*this, RunsWhenDisabled())
-          .WillRepeatedly(::testing::Return(true));
-    }
-
-    MockCommand(std::initializer_list<Subsystem*> requirements,
-                bool finished = false, bool runWhenDisabled = true) {
-      m_requirements.insert(requirements.begin(), requirements.end());
-      EXPECT_CALL(*this, GetRequirements())
-          .WillRepeatedly(::testing::Return(m_requirements));
-      EXPECT_CALL(*this, IsFinished())
-          .WillRepeatedly(::testing::Return(finished));
-      EXPECT_CALL(*this, RunsWhenDisabled())
-          .WillRepeatedly(::testing::Return(runWhenDisabled));
-    }
-
-    MockCommand(MockCommand&& other) {
-      EXPECT_CALL(*this, IsFinished())
-          .WillRepeatedly(::testing::Return(other.IsFinished()));
-      EXPECT_CALL(*this, RunsWhenDisabled())
-          .WillRepeatedly(::testing::Return(other.RunsWhenDisabled()));
-      std::swap(m_requirements, other.m_requirements);
-      EXPECT_CALL(*this, GetRequirements())
-          .WillRepeatedly(::testing::Return(m_requirements));
-    }
-
-    MockCommand(const MockCommand& other) : CommandHelper{other} {}
-
-    void SetFinished(bool finished) {
-      EXPECT_CALL(*this, IsFinished())
-          .WillRepeatedly(::testing::Return(finished));
-    }
-
-    ~MockCommand() {  // NOLINT
-      auto& scheduler = CommandScheduler::GetInstance();
-      scheduler.Cancel(this);
-    }
-
-   private:
-    wpi::SmallSet<Subsystem*, 4> m_requirements;
-  };
-
   CommandScheduler GetScheduler();
 
   void SetUp() override;
@@ -103,69 +103,7 @@ class CommandTestBaseWithParam : public ::testing::TestWithParam<T> {
     scheduler.GetActiveButtonLoop()->Clear();
   }
 
-  class TestSubsystem : public SubsystemBase {};
-
  protected:
-  class MockCommand : public Command {
-   public:
-    MOCK_CONST_METHOD0(GetRequirements, wpi::SmallSet<Subsystem*, 4>());
-    MOCK_METHOD0(IsFinished, bool());
-    MOCK_CONST_METHOD0(RunsWhenDisabled, bool());
-    MOCK_METHOD0(Initialize, void());
-    MOCK_METHOD0(Execute, void());
-    MOCK_METHOD1(End, void(bool interrupted));
-
-    MockCommand() {
-      m_requirements = {};
-      EXPECT_CALL(*this, GetRequirements())
-          .WillRepeatedly(::testing::Return(m_requirements));
-      EXPECT_CALL(*this, IsFinished()).WillRepeatedly(::testing::Return(false));
-      EXPECT_CALL(*this, RunsWhenDisabled())
-          .WillRepeatedly(::testing::Return(true));
-    }
-
-    MockCommand(std::initializer_list<Subsystem*> requirements,
-                bool finished = false, bool runWhenDisabled = true) {
-      m_requirements.insert(requirements.begin(), requirements.end());
-      EXPECT_CALL(*this, GetRequirements())
-          .WillRepeatedly(::testing::Return(m_requirements));
-      EXPECT_CALL(*this, IsFinished())
-          .WillRepeatedly(::testing::Return(finished));
-      EXPECT_CALL(*this, RunsWhenDisabled())
-          .WillRepeatedly(::testing::Return(runWhenDisabled));
-    }
-
-    MockCommand(MockCommand&& other) {
-      EXPECT_CALL(*this, IsFinished())
-          .WillRepeatedly(::testing::Return(other.IsFinished()));
-      EXPECT_CALL(*this, RunsWhenDisabled())
-          .WillRepeatedly(::testing::Return(other.RunsWhenDisabled()));
-      std::swap(m_requirements, other.m_requirements);
-      EXPECT_CALL(*this, GetRequirements())
-          .WillRepeatedly(::testing::Return(m_requirements));
-    }
-
-    MockCommand(const MockCommand& other) : Command{other} {}
-
-    void SetFinished(bool finished) {
-      EXPECT_CALL(*this, IsFinished())
-          .WillRepeatedly(::testing::Return(finished));
-    }
-
-    ~MockCommand() {  // NOLINT
-      auto& scheduler = CommandScheduler::GetInstance();
-      scheduler.Cancel(this);
-    }
-
-   protected:
-    std::unique_ptr<Command> TransferOwnership() && {  // NOLINT
-      return std::make_unique<MockCommand>(std::move(*this));
-    }
-
-   private:
-    wpi::SmallSet<Subsystem*, 4> m_requirements;
-  };
-
   CommandScheduler GetScheduler() { return CommandScheduler(); }
 
   void SetUp() override { frc::sim::DriverStationSim::SetEnabled(true); }

wpilibc/src/main/native/cpp/Ultrasonic.cpp

@@ -85,10 +85,7 @@ int Ultrasonic::GetEchoChannel() const {
 }
 
 void Ultrasonic::Ping() {
-  if (m_automaticEnabled) {
-    throw FRC_MakeError(err::IncompatibleMode,
-                        "cannot call Ping() in automatic mode");
-  }
+  SetAutomaticMode(false);  // turn off automatic round-robin if pinging
 
   // Reset the counter to zero (invalid data now)
   m_counter.Reset();
@@ -138,7 +135,7 @@ void Ultrasonic::SetAutomaticMode(bool enabling) {
 units::meter_t Ultrasonic::GetRange() const {
   if (IsRangeValid()) {
     if (m_simRange) {
-      return units::meter_t{m_simRange.Get()};
+      return units::inch_t{m_simRange.Get()};
     }
     return m_counter.GetPeriod() * kSpeedOfSound / 2.0;
   } else {

wpilibc/src/main/native/cpp/simulation/SimDeviceSim.cpp

@@ -27,6 +27,12 @@ SimDeviceSim::SimDeviceSim(const char* name, int index, int channel) {
       fmt::format("{}[{},{}]", name, index, channel).c_str());
 }
 
+SimDeviceSim::SimDeviceSim(HAL_SimDeviceHandle handle) : m_handle(handle) {}
+
+std::string SimDeviceSim::GetName() const {
+  return std::string(HALSIM_GetSimDeviceName(m_handle));
+}
+
 hal::SimValue SimDeviceSim::GetValue(const char* name) const {
   return HALSIM_GetSimValueHandle(m_handle, name);
 }

wpilibc/src/main/native/cpp/simulation/UltrasonicSim.cpp

@@ -9,8 +9,11 @@
 
 using namespace frc::sim;
 
-UltrasonicSim::UltrasonicSim(const frc::Ultrasonic& ultrasonic) {
-  frc::sim::SimDeviceSim deviceSim{"Ultrasonic", ultrasonic.GetEchoChannel()};
+UltrasonicSim::UltrasonicSim(const frc::Ultrasonic& ultrasonic)
+    : UltrasonicSim(0, ultrasonic.GetEchoChannel()) {}
+
+UltrasonicSim::UltrasonicSim(int ping, int echo) {
+  frc::sim::SimDeviceSim deviceSim{"Ultrasonic", echo};
   m_simRangeValid = deviceSim.GetBoolean("Range Valid");
   m_simRange = deviceSim.GetDouble("Range (in)");
 }
@@ -19,6 +22,6 @@ void UltrasonicSim::SetRangeValid(bool isValid) {
   m_simRangeValid.Set(isValid);
 }
 
-void UltrasonicSim::SetRange(units::meter_t range) {
+void UltrasonicSim::SetRange(units::inch_t range) {
   m_simRange.Set(range.value());
 }

wpilibc/src/main/native/include/frc/simulation/SimDeviceSim.h

@@ -42,6 +42,20 @@ class SimDeviceSim {
    */
   SimDeviceSim(const char* name, int index, int channel);
 
+  /**
+   * Constructs a SimDeviceSim.
+   *
+   * @param handle the low level handle for the corresponding SimDevice.
+   */
+  explicit SimDeviceSim(HAL_SimDeviceHandle handle);
+
+  /**
+   * Get the name of this object.
+   *
+   * @return name
+   */
+  std::string GetName() const;
+
   /**
    * Get the property object with the given name.
    *

wpilibc/src/main/native/include/frc/simulation/UltrasonicSim.h

@@ -14,16 +14,24 @@ class Ultrasonic;
 namespace sim {
 
 /**
- * Class to control a simulated ADXRS450 gyroscope.
+ * Class to control a simulated {@link Ultrasonic}.
  */
 class UltrasonicSim {
  public:
   /**
-   * Constructs from a ADXRS450_Gyro object.
+   * Constructor.
    *
-   * @param gyro ADXRS450_Gyro to simulate
+   * @param ultrasonic The real ultrasonic to simulate
    */
-  explicit UltrasonicSim(const Ultrasonic& gyro);
+  explicit UltrasonicSim(const Ultrasonic& ultrasonic);
+
+  /**
+   * Constructor.
+   *
+   * @param ping unused.
+   * @param echo the ultrasonic's echo channel.
+   */
+  UltrasonicSim(int ping, int echo);
 
   /**
    * Sets if the range measurement is valid.
@@ -37,7 +45,7 @@ class UltrasonicSim {
    *
    * @param range The range
    */
-  void SetRange(units::meter_t range);
+  void SetRange(units::inch_t range);
 
  private:
   hal::SimBoolean m_simRangeValid;

wpilibc/src/test/native/cpp/UltrasonicTest.cpp

@@ -6,7 +6,7 @@
 #include "frc/simulation/UltrasonicSim.h"
 #include "gtest/gtest.h"
 
-namespace frc {
+using namespace frc;
 
 TEST(UltrasonicTest, SimDevices) {
   Ultrasonic ultrasonic{0, 1};
@@ -23,4 +23,19 @@ TEST(UltrasonicTest, SimDevices) {
   EXPECT_EQ(0, ultrasonic.GetRange().value());
 }
 
-}  // namespace frc
+TEST(UltrasonicTest, AutomaticModeToggle) {
+  frc::Ultrasonic ultrasonic{0, 1};
+  EXPECT_NO_THROW({
+    frc::Ultrasonic::SetAutomaticMode(true);
+    frc::Ultrasonic::SetAutomaticMode(false);
+    frc::Ultrasonic::SetAutomaticMode(true);
+  });
+}
+
+TEST(UltrasonicTest, AutomaticModeOnWithZeroInstances) {
+  EXPECT_NO_THROW({ frc::Ultrasonic::SetAutomaticMode(true); });
+}
+
+TEST(UltrasonicTest, AutomaticModeOffWithZeroInstances) {
+  EXPECT_NO_THROW({ frc::Ultrasonic::SetAutomaticMode(false); });
+}

wpilibc/src/test/native/cpp/simulation/EncoderSimTest.cpp

@@ -198,7 +198,7 @@ TEST(EncoderSimTest, SetDistancePerPulse) {
   DoubleCallback callback;
   auto cb = sim.RegisterDistancePerPulseCallback(callback.GetCallback(), false);
 
-  encoder.SetDistancePerPulse(.03405);
+  sim.SetDistancePerPulse(.03405);
   EXPECT_EQ(.03405, sim.GetDistancePerPulse());
   EXPECT_EQ(.03405, encoder.GetDistancePerPulse());
   EXPECT_TRUE(callback.WasTriggered());

wpilibc/src/test/native/cpp/simulation/SimDeviceSimTest.cpp

@@ -20,6 +20,8 @@ TEST(SimDeviceSimTest, Basic) {
   EXPECT_FALSE(simBool.Get());
   simBool.Set(true);
   EXPECT_TRUE(devBool.Get());
+
+  EXPECT_EQ(sim.GetName(), "test");
 }
 
 TEST(SimDeviceSimTest, EnumerateDevices) {

wpilibcExamples/.styleguide

@@ -11,6 +11,7 @@ cppSrcFileInclude {
 includeOtherLibs {
   ^cameraserver/
   ^cscore
+  ^fmt/
   ^frc/
   ^frc2/
   ^hal/

wpilibcExamples/src/main/cpp/examples/AprilTagsVision/cpp/Robot.cpp

@@ -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.
+
+#include <cstdio>
+#include <span>
+#include <sstream>
+#include <string>
+#include <thread>
+
+#include <cameraserver/CameraServer.h>
+#include <fmt/format.h>
+#include <frc/TimedRobot.h>
+#include <frc/apriltag/AprilTagDetection.h>
+#include <frc/apriltag/AprilTagDetector.h>
+#include <frc/apriltag/AprilTagPoseEstimator.h>
+#include <frc/geometry/Transform3d.h>
+#include <networktables/IntegerArrayTopic.h>
+#include <networktables/NetworkTableInstance.h>
+#include <opencv2/core/core.hpp>
+#include <opencv2/core/types.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <units/angle.h>
+#include <units/length.h>
+
+/**
+ * This is a demo program showing the detection of AprilTags.
+ * The image is acquired from the USB camera, then any detected AprilTags
+ * are marked up on the image and sent to the dashboard.
+ *
+ * Be aware that the performance on this is much worse than a coprocessor
+ * solution!
+ */
+class Robot : public frc::TimedRobot {
+#if defined(__linux__) || defined(_WIN32)
+
+ private:
+  static void VisionThread() {
+    frc::AprilTagDetector detector;
+    // look for tag16h5, don't correct any error bits
+    detector.AddFamily("tag16h5", 0);
+
+    // Set up Pose Estimator - parameters are for a Microsoft Lifecam HD-3000
+    // (https://www.chiefdelphi.com/t/wpilib-apriltagdetector-sample-code/421411/21)
+    frc::AprilTagPoseEstimator::Config poseEstConfig = {
+        .tagSize = units::length::inch_t(6.0),
+        .fx = 699.3778103158814,
+        .fy = 677.7161226393544,
+        .cx = 345.6059345433618,
+        .cy = 207.12741326228522};
+    frc::AprilTagPoseEstimator estimator(poseEstConfig);
+
+    // Get the USB camera from CameraServer
+    cs::UsbCamera camera = frc::CameraServer::StartAutomaticCapture();
+    // Set the resolution
+    camera.SetResolution(640, 480);
+
+    // Get a CvSink. This will capture Mats from the Camera
+    cs::CvSink cvSink = frc::CameraServer::GetVideo();
+    // Setup a CvSource. This will send images back to the Dashboard
+    cs::CvSource outputStream =
+        frc::CameraServer::PutVideo("Detected", 640, 480);
+
+    // Mats are very memory expensive. Lets reuse this Mat.
+    cv::Mat mat;
+    cv::Mat grayMat;
+
+    // Instantiate once
+    std::vector<int64_t> tags;
+    cv::Scalar outlineColor{0, 255, 0};
+    cv::Scalar crossColor{0, 0, 255};
+
+    // We'll output to NT
+    auto tagsTable =
+        nt::NetworkTableInstance::GetDefault().GetTable("apriltags");
+    auto pubTags = tagsTable->GetIntegerArrayTopic("tags").Publish();
+
+    while (true) {
+      // Tell the CvSink to grab a frame from the camera and
+      // put it in the source mat.  If there is an error notify the
+      // output.
+      if (cvSink.GrabFrame(mat) == 0) {
+        // Send the output the error.
+        outputStream.NotifyError(cvSink.GetError());
+        // skip the rest of the current iteration
+        continue;
+      }
+
+      cv::cvtColor(mat, grayMat, cv::COLOR_BGR2GRAY);
+
+      cv::Size g_size = grayMat.size();
+      frc::AprilTagDetector::Results detections =
+          detector.Detect(g_size.width, g_size.height, grayMat.data);
+
+      // have not seen any tags yet
+      tags.clear();
+
+      for (const frc::AprilTagDetection* detection : detections) {
+        // remember we saw this tag
+        tags.push_back(detection->GetId());
+
+        // draw lines around the tag
+        for (int i = 0; i <= 3; i++) {
+          int j = (i + 1) % 4;
+          const frc::AprilTagDetection::Point pti = detection->GetCorner(i);
+          const frc::AprilTagDetection::Point ptj = detection->GetCorner(j);
+          line(mat, cv::Point(pti.x, pti.y), cv::Point(ptj.x, ptj.y),
+               outlineColor, 2);
+        }
+
+        // mark the center of the tag
+        const frc::AprilTagDetection::Point c = detection->GetCenter();
+        int ll = 10;
+        line(mat, cv::Point(c.x - ll, c.y), cv::Point(c.x + ll, c.y),
+             crossColor, 2);
+        line(mat, cv::Point(c.x, c.y - ll), cv::Point(c.x, c.y + ll),
+             crossColor, 2);
+
+        // identify the tag
+        putText(mat, std::to_string(detection->GetId()),
+                cv::Point(c.x + ll, c.y), cv::FONT_HERSHEY_SIMPLEX, 1,
+                crossColor, 3);
+
+        // determine pose
+        frc::Transform3d pose = estimator.Estimate(*detection);
+
+        // put pose into NT
+        frc::Rotation3d rotation = pose.Rotation();
+        tagsTable->GetEntry(fmt::format("pose_{}", detection->GetId()))
+            .SetDoubleArray(
+                {{ pose.X().value(),
+                   pose.Y().value(),
+                   pose.Z().value(),
+                   rotation.X().value(),
+                   rotation.Y().value(),
+                   rotation.Z().value() }});
+      }
+
+      // put list of tags onto NT
+      pubTags.Set(tags);
+
+      // Give the output stream a new image to display
+      outputStream.PutFrame(mat);
+    }
+  }
+#endif
+
+  void RobotInit() override {
+    // We need to run our vision program in a separate thread. If not, our robot
+    // program will not run.
+#if defined(__linux__) || defined(_WIN32)
+    std::thread visionThread(VisionThread);
+    visionThread.detach();
+#else
+    std::fputs("Vision only available on Linux or Windows.\n", stderr);
+    std::fflush(stderr);
+#endif
+  }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif

wpilibcExamples/src/main/cpp/examples/DigitalCommunication/cpp/Robot.cpp

@@ -2,45 +2,28 @@
 // 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/DigitalOutput.h>
+#include "Robot.h"
+
 #include <frc/DriverStation.h>
-#include <frc/TimedRobot.h>
 
-/**
- * This is a sample program demonstrating how to communicate to a light
- * controller from the robot code using the roboRIO's DIO ports.
- */
-class Robot : public frc::TimedRobot {
- public:
-  void RobotPeriodic() override {
-    // pull alliance port high if on red alliance, pull low if on blue alliance
-    m_allianceOutput.Set(frc::DriverStation::GetAlliance() ==
-                         frc::DriverStation::kRed);
+void Robot::RobotPeriodic() {
+  // pull alliance port high if on red alliance, pull low if on blue alliance
+  m_allianceOutput.Set(frc::DriverStation::GetAlliance() ==
+                       frc::DriverStation::kRed);
 
-    // pull enabled port high if enabled, low if disabled
-    m_enabledOutput.Set(frc::DriverStation::IsEnabled());
+  // pull enabled port high if enabled, low if disabled
+  m_enabledOutput.Set(frc::DriverStation::IsEnabled());
 
-    // pull auto port high if in autonomous, low if in teleop (or disabled)
-    m_autonomousOutput.Set(frc::DriverStation::IsAutonomous());
+  // pull auto port high if in autonomous, low if in teleop (or disabled)
+  m_autonomousOutput.Set(frc::DriverStation::IsAutonomous());
 
-    // pull alert port high if match time remaining is between 30 and 25 seconds
-    auto matchTime = frc::DriverStation::GetMatchTime();
-    m_alertOutput.Set(matchTime <= 30 && matchTime >= 25);
-  }
-
- private:
-  // define ports for communication with light controller
-  static constexpr int kAlliancePort = 0;
-  static constexpr int kEnabledPort = 1;
-  static constexpr int kAutonomousPort = 2;
-  static constexpr int kAlertPort = 3;
-
-  frc::DigitalOutput m_allianceOutput{kAlliancePort};
-  frc::DigitalOutput m_enabledOutput{kEnabledPort};
-  frc::DigitalOutput m_autonomousOutput{kAutonomousPort};
-  frc::DigitalOutput m_alertOutput{kAlertPort};
-};
+  // pull alert port high if match time remaining is between 30 and 25 seconds
+  auto matchTime = frc::DriverStation::GetMatchTime();
+  m_alertOutput.Set(matchTime <= 30 && matchTime >= 25);
+}
 
+#ifndef RUNNING_FRC_TESTS
 int main() {
   return frc::StartRobot<Robot>();
 }
+#endif

wpilibcExamples/src/main/cpp/examples/DigitalCommunication/include/Robot.h

@@ -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.
+
+#pragma once
+
+#include <array>
+
+#include <frc/DigitalOutput.h>
+#include <frc/TimedRobot.h>
+
+/**
+ * This is a sample program demonstrating how to communicate to a light
+ * controller from the robot code using the roboRIO's DIO ports.
+ */
+class Robot : public frc::TimedRobot {
+ public:
+  // define ports for communication with light controller
+  static constexpr int kAlliancePort = 0;
+  static constexpr int kEnabledPort = 1;
+  static constexpr int kAutonomousPort = 2;
+  static constexpr int kAlertPort = 3;
+
+  void RobotPeriodic() override;
+
+ private:
+  frc::DigitalOutput m_allianceOutput{kAlliancePort};
+  frc::DigitalOutput m_enabledOutput{kEnabledPort};
+  frc::DigitalOutput m_autonomousOutput{kAutonomousPort};
+  frc::DigitalOutput m_alertOutput{kAlertPort};
+};

wpilibcExamples/src/main/cpp/examples/I2CCommunication/cpp/Robot.cpp

@@ -2,45 +2,33 @@
 // 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 <fmt/format.h>
+#include "Robot.h"
 
+#include <fmt/format.h>
 #include <frc/DriverStation.h>
-#include <frc/I2C.h>
-#include <frc/TimedRobot.h>
 #include <frc/Timer.h>
 
-/**
- * This is a sample program demonstrating how to communicate to a light
- * controller from the robot code using the roboRIO's I2C port.
- */
-class Robot : public frc::TimedRobot {
- public:
-  void RobotPeriodic() override {
-    // Creates a string to hold current robot state information, including
-    // alliance, enabled state, operation mode, and match time. The message
-    // is sent in format "AEM###" where A is the alliance color, (R)ed or
-    // (B)lue, E is the enabled state, (E)nabled or (D)isabled, M is the
-    // operation mode, (A)utonomous or (T)eleop, and ### is the zero-padded
-    // time remaining in the match.
-    //
-    // For example, "RET043" would indicate that the robot is on the red
-    // alliance, enabled in teleop mode, with 43 seconds left in the match.
-    auto string = fmt::format(
-        "{}{}{}{:03}",
-        frc::DriverStation::GetAlliance() == frc::DriverStation::Alliance::kRed
-            ? "R"
-            : "B",
-        frc::DriverStation::IsEnabled() ? "E" : "D",
-        frc::DriverStation::IsAutonomous() ? "A" : "T",
-        static_cast<int>(frc::Timer::GetMatchTime().value()));
+void Robot::RobotPeriodic() {
+  // Creates a string to hold current robot state information, including
+  // alliance, enabled state, operation mode, and match time. The message
+  // is sent in format "AEM###" where A is the alliance color, (R)ed or
+  // (B)lue, E is the enabled state, (E)nabled or (D)isabled, M is the
+  // operation mode, (A)utonomous or (T)eleop, and ### is the zero-padded
+  // time remaining in the match.
+  //
+  // For example, "RET043" would indicate that the robot is on the red
+  // alliance, enabled in teleop mode, with 43 seconds left in the match.
+  auto string = fmt::format(
+      "{}{}{}{:03}",
+      frc::DriverStation::GetAlliance() == frc::DriverStation::Alliance::kRed
+          ? "R"
+          : "B",
+      frc::DriverStation::IsEnabled() ? "E" : "D",
+      frc::DriverStation::IsAutonomous() ? "A" : "T",
+      static_cast<int>(frc::Timer::GetMatchTime().value()));
 
-    arduino.WriteBulk(reinterpret_cast<uint8_t*>(string.data()), string.size());
-  }
-
- private:
-  static constexpr int deviceAddress = 4;
-  frc::I2C arduino{frc::I2C::Port::kOnboard, deviceAddress};
-};
+  arduino.WriteBulk(reinterpret_cast<uint8_t*>(string.data()), string.size());
+}
 
 #ifndef RUNNING_FRC_TESTS
 int main() {

wpilibcExamples/src/main/cpp/examples/I2CCommunication/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 <array>
+
+#include <frc/I2C.h>
+#include <frc/TimedRobot.h>
+
+/**
+ * This is a sample program demonstrating how to communicate to a light
+ * controller from the robot code using the roboRIO's I2C port.
+ */
+class Robot : public frc::TimedRobot {
+ public:
+  void RobotPeriodic() override;
+
+  static constexpr frc::I2C::Port kPort = frc::I2C::Port::kOnboard;
+
+ private:
+  static constexpr int deviceAddress = 4;
+  frc::I2C arduino{kPort, deviceAddress};
+};

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() {

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;
+};

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() {

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};
+};

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() {

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};
+};

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"
     ],
@@ -257,6 +259,17 @@
     "gradlebase": "cpp",
     "commandversion": 2
   },
+  {
+    "name": "AprilTags Vision",
+    "description": "on-roboRIO detection of AprilTags using an attached USB camera.",
+    "tags": [
+      "Vision",
+      "AprilTags"
+    ],
+    "foldername": "AprilTagsVision",
+    "gradlebase": "cpp",
+    "commandversion": 2
+  },
   {
     "name": "I2C Communication",
     "description": "An example program that communicates with external devices (such as an Arduino) using the roboRIO's I2C port",

wpilibcExamples/src/main/cpp/templates/timed/cpp/Robot.cpp

@@ -5,7 +5,6 @@
 #include "Robot.h"
 
 #include <fmt/core.h>
-
 #include <frc/smartdashboard/SmartDashboard.h>
 
 void Robot::RobotInit() {

wpilibcExamples/src/test/cpp/examples/AddressableLED/cpp/RainbowTest.cpp

@@ -2,11 +2,11 @@
 // 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 <fmt/format.h>
 #include <gtest/gtest.h>
 
 #include <array>
 
+#include <fmt/format.h>
 #include <frc/AddressableLED.h>
 #include <frc/simulation/AddressableLEDSim.h>
 #include <frc/util/Color.h>

wpilibcExamples/src/test/cpp/examples/DigitalCommunication/cpp/DigitalCommunicationTest.cpp

@@ -0,0 +1,153 @@
+// 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/simulation/DIOSim.h>
+#include <frc/simulation/DriverStationSim.h>
+#include <frc/simulation/SimHooks.h>
+#include <units/time.h>
+
+#include "Robot.h"
+
+template <typename T>
+class DigitalCommunicationTest : public testing::TestWithParam<T> {
+ public:
+  frc::sim::DIOSim m_allianceOutput{Robot::kAlliancePort};
+  frc::sim::DIOSim m_enabledOutput{Robot::kEnabledPort};
+  frc::sim::DIOSim m_autonomousOutput{Robot::kAutonomousPort};
+  frc::sim::DIOSim m_alertOutput{Robot::kAlertPort};
+  Robot m_robot;
+  std::optional<std::thread> m_thread;
+
+  void SetUp() override {
+    frc::sim::PauseTiming();
+    frc::sim::DriverStationSim::ResetData();
+
+    m_thread = std::thread([&] { m_robot.StartCompetition(); });
+    frc::sim::StepTiming(0.0_ms);
+    // SimHooks.stepTiming(0.0); // Wait for Notifiers
+  }
+
+  void TearDown() override {
+    m_robot.EndCompetition();
+    m_thread->join();
+    m_allianceOutput.ResetData();
+    m_enabledOutput.ResetData();
+    m_autonomousOutput.ResetData();
+    m_alertOutput.ResetData();
+  }
+};
+
+class AllianceTest : public DigitalCommunicationTest<HAL_AllianceStationID> {};
+
+TEST_P(AllianceTest, Alliance) {
+  auto alliance = GetParam();
+  frc::sim::DriverStationSim::SetAllianceStationId(alliance);
+  frc::sim::DriverStationSim::NotifyNewData();
+
+  EXPECT_TRUE(m_allianceOutput.GetInitialized());
+  EXPECT_FALSE(m_allianceOutput.GetIsInput());
+
+  frc::sim::StepTiming(20_ms);
+
+  bool isRed;
+  switch (alliance) {
+    case HAL_AllianceStationID_kBlue1:
+    case HAL_AllianceStationID_kBlue2:
+    case HAL_AllianceStationID_kBlue3:
+      isRed = false;
+      break;
+    case HAL_AllianceStationID_kRed1:
+    case HAL_AllianceStationID_kRed2:
+    case HAL_AllianceStationID_kRed3:
+      isRed = true;
+      break;
+  }
+  EXPECT_EQ(isRed, m_allianceOutput.GetValue());
+}
+
+INSTANTIATE_TEST_SUITE_P(
+    DigitalCommunicationTests, AllianceTest,
+    testing::Values<HAL_AllianceStationID>(
+        HAL_AllianceStationID_kRed1, HAL_AllianceStationID_kRed2,
+        HAL_AllianceStationID_kRed3, HAL_AllianceStationID_kBlue1,
+        HAL_AllianceStationID_kBlue2, HAL_AllianceStationID_kBlue3),
+    [](const testing::TestParamInfo<AllianceTest::ParamType>& info) {
+      switch (info.param) {
+        case HAL_AllianceStationID_kBlue1:
+          return std::string{"Blue1"};
+        case HAL_AllianceStationID_kBlue2:
+          return std::string{"Blue2"};
+        case HAL_AllianceStationID_kBlue3:
+          return std::string{"Blue3"};
+        case HAL_AllianceStationID_kRed1:
+          return std::string{"Red1"};
+        case HAL_AllianceStationID_kRed2:
+          return std::string{"Red2"};
+        case HAL_AllianceStationID_kRed3:
+          return std::string{"Red3"};
+      }
+      return std::string{"Error"};
+    });
+
+class EnabledTest : public DigitalCommunicationTest<bool> {};
+
+TEST_P(EnabledTest, Enabled) {
+  auto enabled = GetParam();
+  frc::sim::DriverStationSim::SetEnabled(enabled);
+  frc::sim::DriverStationSim::NotifyNewData();
+
+  EXPECT_TRUE(m_enabledOutput.GetInitialized());
+  EXPECT_FALSE(m_enabledOutput.GetIsInput());
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(enabled, m_enabledOutput.GetValue());
+}
+
+INSTANTIATE_TEST_SUITE_P(DigitalCommunicationTests, EnabledTest,
+                         testing::Bool(), testing::PrintToStringParamName());
+
+class AutonomousTest : public DigitalCommunicationTest<bool> {};
+
+TEST_P(AutonomousTest, Autonomous) {
+  auto autonomous = GetParam();
+  frc::sim::DriverStationSim::SetAutonomous(autonomous);
+  frc::sim::DriverStationSim::NotifyNewData();
+
+  EXPECT_TRUE(m_autonomousOutput.GetInitialized());
+  EXPECT_FALSE(m_autonomousOutput.GetIsInput());
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(autonomous, m_autonomousOutput.GetValue());
+}
+
+INSTANTIATE_TEST_SUITE_P(DigitalCommunicationTests, AutonomousTest,
+                         testing::Bool(), testing::PrintToStringParamName());
+
+class AlertTest : public DigitalCommunicationTest<double> {};
+
+TEST_P(AlertTest, Alert) {
+  auto matchTime = GetParam();
+  frc::sim::DriverStationSim::SetMatchTime(matchTime);
+  frc::sim::DriverStationSim::NotifyNewData();
+
+  EXPECT_TRUE(m_alertOutput.GetInitialized());
+  EXPECT_FALSE(m_alertOutput.GetIsInput());
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(matchTime <= 30 && matchTime >= 25, m_alertOutput.GetValue());
+}
+
+INSTANTIATE_TEST_SUITE_P(
+    DigitalCommunicationTests, AlertTest, testing::Values(45.0, 27.0, 23.0),
+    [](const testing::TestParamInfo<double>& info) {
+      return testing::PrintToString(info.param).append("_s");
+    });

wpilibcExamples/src/test/cpp/examples/DigitalCommunication/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;
+}

wpilibcExamples/src/test/cpp/examples/I2CCommunication/cpp/I2CCommunicationTest.cpp

@@ -0,0 +1,160 @@
+// 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/simulation/DriverStationSim.h>
+#include <frc/simulation/SimHooks.h>
+#include <hal/simulation/I2CData.h>
+#include <units/time.h>
+
+#include "Robot.h"
+
+static std::string gString;
+
+void callback(const char* name, void* param, const unsigned char* buffer,
+              unsigned int count) {
+  gString.assign(reinterpret_cast<const char*>(buffer),
+                 static_cast<int>(count));
+}
+
+template <typename T>
+class I2CCommunicationTest : public testing::TestWithParam<T> {
+ public:
+  Robot m_robot;
+  std::optional<std::thread> m_thread;
+  int32_t m_callback;
+  int32_t m_port;
+
+  void SetUp() override {
+    gString = std::string();
+    frc::sim::PauseTiming();
+    frc::sim::DriverStationSim::ResetData();
+    m_port = static_cast<int32_t>(Robot::kPort);
+
+    m_callback = HALSIM_RegisterI2CWriteCallback(m_port, &callback, nullptr);
+
+    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_CancelI2CWriteCallback(m_port, m_callback);
+    HALSIM_ResetI2CData(m_port);
+  }
+};
+
+class AllianceTest : public I2CCommunicationTest<HAL_AllianceStationID> {};
+
+TEST_P(AllianceTest, Alliance) {
+  auto alliance = GetParam();
+  frc::sim::DriverStationSim::SetAllianceStationId(alliance);
+  frc::sim::DriverStationSim::NotifyNewData();
+
+  EXPECT_TRUE(HALSIM_GetI2CInitialized(m_port));
+
+  frc::sim::StepTiming(20_ms);
+
+  char expected;
+  switch (alliance) {
+    case HAL_AllianceStationID_kBlue1:
+    case HAL_AllianceStationID_kBlue2:
+    case HAL_AllianceStationID_kBlue3:
+      expected = 'B';
+      break;
+    case HAL_AllianceStationID_kRed1:
+    case HAL_AllianceStationID_kRed2:
+    case HAL_AllianceStationID_kRed3:
+      expected = 'R';
+      break;
+  }
+  EXPECT_EQ(expected, gString.at(0));
+}
+
+INSTANTIATE_TEST_SUITE_P(
+    I2CCommunicationTests, AllianceTest,
+    testing::Values<HAL_AllianceStationID>(
+        HAL_AllianceStationID_kRed1, HAL_AllianceStationID_kRed2,
+        HAL_AllianceStationID_kRed3, HAL_AllianceStationID_kBlue1,
+        HAL_AllianceStationID_kBlue2, HAL_AllianceStationID_kBlue3),
+    [](const testing::TestParamInfo<AllianceTest::ParamType>& info) {
+      switch (info.param) {
+        case HAL_AllianceStationID_kBlue1:
+          return std::string{"Blue1"};
+        case HAL_AllianceStationID_kBlue2:
+          return std::string{"Blue2"};
+        case HAL_AllianceStationID_kBlue3:
+          return std::string{"Blue3"};
+        case HAL_AllianceStationID_kRed1:
+          return std::string{"Red1"};
+        case HAL_AllianceStationID_kRed2:
+          return std::string{"Red2"};
+        case HAL_AllianceStationID_kRed3:
+          return std::string{"Red3"};
+      }
+      return std::string{"Error"};
+    });
+
+class EnabledTest : public I2CCommunicationTest<bool> {};
+
+TEST_P(EnabledTest, Enabled) {
+  auto enabled = GetParam();
+  frc::sim::DriverStationSim::SetEnabled(enabled);
+  frc::sim::DriverStationSim::NotifyNewData();
+
+  EXPECT_TRUE(HALSIM_GetI2CInitialized(m_port));
+
+  frc::sim::StepTiming(20_ms);
+
+  char expected = enabled ? 'E' : 'D';
+  EXPECT_EQ(expected, gString.at(1));
+}
+
+INSTANTIATE_TEST_SUITE_P(I2CCommunicationTests, EnabledTest, testing::Bool(),
+                         testing::PrintToStringParamName());
+
+class AutonomousTest : public I2CCommunicationTest<bool> {};
+
+TEST_P(AutonomousTest, Autonomous) {
+  auto autonomous = GetParam();
+  frc::sim::DriverStationSim::SetAutonomous(autonomous);
+  frc::sim::DriverStationSim::NotifyNewData();
+
+  EXPECT_TRUE(HALSIM_GetI2CInitialized(m_port));
+
+  frc::sim::StepTiming(20_ms);
+
+  char expected = autonomous ? 'A' : 'T';
+  EXPECT_EQ(expected, gString.at(2));
+}
+
+INSTANTIATE_TEST_SUITE_P(I2CCommunicationTests, AutonomousTest, testing::Bool(),
+                         testing::PrintToStringParamName());
+
+class MatchTimeTest : public I2CCommunicationTest<int> {};
+
+TEST_P(MatchTimeTest, Alert) {
+  auto matchTime = GetParam();
+  frc::sim::DriverStationSim::SetMatchTime(matchTime);
+  frc::sim::DriverStationSim::NotifyNewData();
+
+  EXPECT_TRUE(HALSIM_GetI2CInitialized(m_port));
+
+  frc::sim::StepTiming(20_ms);
+
+  std::string expected = fmt::format("{:03}", matchTime);
+  EXPECT_EQ(expected, gString.substr(3));
+}
+
+INSTANTIATE_TEST_SUITE_P(
+    I2CCommunicationTests, MatchTimeTest, testing::Values(112, 45, 27, 23, 3),
+    [](const testing::TestParamInfo<int>& info) {
+      return testing::PrintToString(info.param).append("_s");
+    });

wpilibcExamples/src/test/cpp/examples/I2CCommunication/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;
+}

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);
+  }
+}

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;
+}

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));

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;
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java

@@ -90,9 +90,6 @@ public class Ultrasonic implements Sendable, AutoCloseable {
       m_pingChannel.setSimDevice(m_simDevice);
       m_echoChannel.setSimDevice(m_simDevice);
     }
-    if (m_task == null) {
-      m_task = new UltrasonicChecker();
-    }
     final boolean originalMode = m_automaticEnabled;
     setAutomaticMode(false); // kill task when adding a new sensor
     m_sensors.add(this);
@@ -202,7 +199,7 @@ public class Ultrasonic implements Sendable, AutoCloseable {
    *     sensors fire at the same time. If another scheduling algorithm is preferred, it can be
    *     implemented by pinging the sensors manually and waiting for the results to come back.
    */
-  public static void setAutomaticMode(boolean enabling) {
+  public static synchronized void setAutomaticMode(boolean enabling) {
     if (enabling == m_automaticEnabled) {
       return; // ignore the case of no change
     }
@@ -217,14 +214,18 @@ public class Ultrasonic implements Sendable, AutoCloseable {
       }
 
       // Start round robin task
+      m_task = new UltrasonicChecker();
       m_task.start();
     } else {
-      // Wait for background task to stop running
-      try {
-        m_task.join();
-      } catch (InterruptedException ex) {
-        Thread.currentThread().interrupt();
-        ex.printStackTrace();
+      if (m_task != null) {
+        // Wait for background task to stop running
+        try {
+          m_task.join();
+          m_task = null;
+        } catch (InterruptedException ex) {
+          Thread.currentThread().interrupt();
+          ex.printStackTrace();
+        }
       }
 
       /* Clear all the counters (data now invalid) since automatic mode is

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/EncoderSim.java

@@ -303,6 +303,38 @@ public class EncoderSim {
     EncoderDataJNI.setSamplesToAverage(m_index, samplesToAverage);
   }
 
+  /**
+   * Register a callback on the distance per pulse value of this encoder.
+   *
+   * @param callback the callback that will be called whenever the distance per pulse is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public CallbackStore registerDistancePerPulseCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = EncoderDataJNI.registerDistancePerPulseCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, EncoderDataJNI::cancelDistancePerPulseCallback);
+  }
+
+  /**
+   * Get the distance per pulse value.
+   *
+   * @return the distance per pulse value
+   */
+  public double getDistancePerPulse() {
+    return EncoderDataJNI.getDistancePerPulse(m_index);
+  }
+
+  /**
+   * Set the distance per pulse value.
+   *
+   * @param samplesToAverage the new value
+   */
+  public void setDistancePerPulse(double samplesToAverage) {
+    EncoderDataJNI.setDistancePerPulse(m_index, samplesToAverage);
+  }
+
   /**
    * Change the encoder distance.
    *

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimDeviceSim.java

@@ -24,7 +24,7 @@ public class SimDeviceSim {
    * @param name name of the SimDevice
    */
   public SimDeviceSim(String name) {
-    m_handle = SimDeviceDataJNI.getSimDeviceHandle(name);
+    this(SimDeviceDataJNI.getSimDeviceHandle(name));
   }
 
   /**
@@ -48,6 +48,24 @@ public class SimDeviceSim {
     this(name + "[" + index + "," + channel + "]");
   }
 
+  /**
+   * Constructs a SimDeviceSim.
+   *
+   * @param handle the low level handle for the corresponding SimDevice
+   */
+  public SimDeviceSim(int handle) {
+    m_handle = handle;
+  }
+
+  /**
+   * Get the name of this object.
+   *
+   * @return name
+   */
+  public String getName() {
+    return SimDeviceDataJNI.getSimDeviceName(m_handle);
+  }
+
   /**
    * Get the property object with the given name.
    *

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/UltrasonicSim.java

@@ -9,6 +9,7 @@ import edu.wpi.first.hal.SimDouble;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Ultrasonic;
 
+/** Class to control a simulated {@link edu.wpi.first.wpilibj.Ultrasonic}. */
 public class UltrasonicSim {
   private final SimBoolean m_simRangeValid;
   private final SimDouble m_simRange;
@@ -19,7 +20,18 @@ public class UltrasonicSim {
    * @param ultrasonic The real ultrasonic to simulate
    */
   public UltrasonicSim(Ultrasonic ultrasonic) {
-    SimDeviceSim simDevice = new SimDeviceSim("Ultrasonic", ultrasonic.getEchoChannel());
+    // ping parameter is unused
+    this(-1, ultrasonic.getEchoChannel());
+  }
+
+  /**
+   * Constructor.
+   *
+   * @param ping unused.
+   * @param echo the ultrasonic's echo channel.
+   */
+  public UltrasonicSim(@SuppressWarnings("unused") int ping, int echo) {
+    SimDeviceSim simDevice = new SimDeviceSim("Ultrasonic", echo);
     m_simRangeValid = simDevice.getBoolean("Range Valid");
     m_simRange = simDevice.getDouble("Range (in)");
   }

wpilibj/src/test/java/edu/wpi/first/wpilibj/UltrasonicTest.java

@@ -4,12 +4,15 @@
 
 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 static org.junit.jupiter.api.Assertions.assertTrue;
 
 import edu.wpi.first.wpilibj.simulation.UltrasonicSim;
 import org.junit.jupiter.api.Test;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.ValueSource;
 
 class UltrasonicTest {
   @Test
@@ -28,4 +31,23 @@ class UltrasonicTest {
       assertEquals(0, ultrasonic.getRangeInches());
     }
   }
+
+  @Test
+  void automaticModeToggle() {
+    try (@SuppressWarnings("unused")
+        Ultrasonic ultrasonic = new Ultrasonic(0, 1)) {
+      assertDoesNotThrow(
+          () -> {
+            Ultrasonic.setAutomaticMode(true);
+            Ultrasonic.setAutomaticMode(false);
+            Ultrasonic.setAutomaticMode(true);
+          });
+    }
+  }
+
+  @ValueSource(booleans = {true, false})
+  @ParameterizedTest
+  void automaticModeWithZeroInstances(boolean enabling) {
+    assertDoesNotThrow(() -> Ultrasonic.setAutomaticMode(enabling));
+  }
 }

wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/EncoderSimTest.java

@@ -89,4 +89,23 @@ class EncoderSimTest {
       }
     }
   }
+
+  @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/edu/wpi/first/wpilibj/simulation/SimDeviceSimTest.java

@@ -27,6 +27,8 @@ class SimDeviceSimTest {
       assertFalse(simBool.get());
       simBool.set(true);
       assertTrue(devBool.get());
+
+      assertEquals(dev.getName(), "test");
     }
   }
 

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsvision/Main.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.examples.apriltagsvision;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

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

@@ -0,0 +1,147 @@
+// 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.apriltagsvision;
+
+import edu.wpi.first.apriltag.AprilTagDetection;
+import edu.wpi.first.apriltag.AprilTagDetector;
+import edu.wpi.first.apriltag.AprilTagPoseEstimator;
+import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.cscore.CvSink;
+import edu.wpi.first.cscore.CvSource;
+import edu.wpi.first.cscore.UsbCamera;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.networktables.IntegerArrayPublisher;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.TimedRobot;
+import java.util.ArrayList;
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.core.Scalar;
+import org.opencv.imgproc.Imgproc;
+
+/**
+ * This is a demo program showing the detection of AprilTags. The image is acquired from the USB
+ * camera, then any detected AprilTags are marked up on the image and sent to the dashboard.
+ *
+ * <p>Be aware that the performance on this is much worse than a coprocessor solution!
+ */
+public class Robot extends TimedRobot {
+  @Override
+  public void robotInit() {
+    var visionThread = new Thread(() -> apriltagVisionThreadProc());
+    visionThread.setDaemon(true);
+    visionThread.start();
+  }
+
+  void apriltagVisionThreadProc() {
+    var detector = new AprilTagDetector();
+    // look for tag16h5, don't correct any error bits
+    detector.addFamily("tag16h5", 0);
+
+    // Set up Pose Estimator - parameters are for a Microsoft Lifecam HD-3000
+    // (https://www.chiefdelphi.com/t/wpilib-apriltagdetector-sample-code/421411/21)
+    var poseEstConfig =
+        new AprilTagPoseEstimator.Config(
+            0.1524, 699.3778103158814, 677.7161226393544, 345.6059345433618, 207.12741326228522);
+    var estimator = new AprilTagPoseEstimator(poseEstConfig);
+
+    // Get the UsbCamera from CameraServer
+    UsbCamera camera = CameraServer.startAutomaticCapture();
+    // Set the resolution
+    camera.setResolution(640, 480);
+
+    // Get a CvSink. This will capture Mats from the camera
+    CvSink cvSink = CameraServer.getVideo();
+    // Setup a CvSource. This will send images back to the Dashboard
+    CvSource outputStream = CameraServer.putVideo("Detected", 640, 480);
+
+    // Mats are very memory expensive. Lets reuse these.
+    var mat = new Mat();
+    var grayMat = new Mat();
+
+    // Instantiate once
+    ArrayList<Long> tags = new ArrayList<>();
+    var outlineColor = new Scalar(0, 255, 0);
+    var crossColor = new Scalar(0, 0, 255);
+
+    // We'll output to NT
+    NetworkTable tagsTable = NetworkTableInstance.getDefault().getTable("apriltags");
+    IntegerArrayPublisher pubTags = tagsTable.getIntegerArrayTopic("tags").publish();
+
+    // This cannot be 'true'. The program will never exit if it is. This
+    // lets the robot stop this thread when restarting robot code or
+    // deploying.
+    while (!Thread.interrupted()) {
+      // Tell the CvSink to grab a frame from the camera and put it
+      // in the source mat.  If there is an error notify the output.
+      if (cvSink.grabFrame(mat) == 0) {
+        // Send the output the error.
+        outputStream.notifyError(cvSink.getError());
+        // skip the rest of the current iteration
+        continue;
+      }
+
+      Imgproc.cvtColor(mat, grayMat, Imgproc.COLOR_RGB2GRAY);
+
+      AprilTagDetection[] detections = detector.detect(grayMat);
+
+      // have not seen any tags yet
+      tags.clear();
+
+      for (AprilTagDetection detection : detections) {
+        // remember we saw this tag
+        tags.add((long) detection.getId());
+
+        // draw lines around the tag
+        for (var i = 0; i <= 3; i++) {
+          var j = (i + 1) % 4;
+          var pt1 = new Point(detection.getCornerX(i), detection.getCornerY(i));
+          var pt2 = new Point(detection.getCornerX(j), detection.getCornerY(j));
+          Imgproc.line(mat, pt1, pt2, outlineColor, 2);
+        }
+
+        // mark the center of the tag
+        var cx = detection.getCenterX();
+        var cy = detection.getCenterY();
+        var ll = 10;
+        Imgproc.line(mat, new Point(cx - ll, cy), new Point(cx + ll, cy), crossColor, 2);
+        Imgproc.line(mat, new Point(cx, cy - ll), new Point(cx, cy + ll), crossColor, 2);
+
+        // identify the tag
+        Imgproc.putText(
+            mat,
+            Integer.toString(detection.getId()),
+            new Point(cx + ll, cy),
+            Imgproc.FONT_HERSHEY_SIMPLEX,
+            1,
+            crossColor,
+            3);
+
+        // determine pose
+        Transform3d pose = estimator.estimate(detection);
+
+        // put pose into dashbaord
+        Rotation3d rot = pose.getRotation();
+        tagsTable
+            .getEntry("pose_" + detection.getId())
+            .setDoubleArray(
+                new double[] {
+                  pose.getX(), pose.getY(), pose.getZ(), rot.getX(), rot.getY(), rot.getZ()
+                });
+      }
+
+      // put list of tags onto dashboard
+      pubTags.set(tags.stream().mapToLong(Long::longValue).toArray());
+
+      // Give the output stream a new image to display
+      outputStream.putFrame(mat);
+    }
+
+    pubTags.close();
+    detector.close();
+  }
+}

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

@@ -14,10 +14,10 @@ import edu.wpi.first.wpilibj.TimedRobot;
  */
 public class Robot extends TimedRobot {
   // define ports for digitalcommunication with light controller
-  private static final int kAlliancePort = 0;
-  private static final int kEnabledPort = 1;
-  private static final int kAutonomousPort = 2;
-  private static final int kAlertPort = 3;
+  static final int kAlliancePort = 0;
+  static final int kEnabledPort = 1;
+  static final int kAutonomousPort = 2;
+  static final int kAlertPort = 3;
 
   private final DigitalOutput m_allianceOutput = new DigitalOutput(kAlliancePort);
   private final DigitalOutput m_enabledOutput = new DigitalOutput(kEnabledPort);
@@ -39,4 +39,14 @@ public class Robot extends TimedRobot {
     var matchTime = DriverStation.getMatchTime();
     m_alertOutput.set(matchTime <= 30 && matchTime >= 25);
   }
+
+  /** Close all resources. */
+  @Override
+  public void close() {
+    m_allianceOutput.close();
+    m_enabledOutput.close();
+    m_autonomousOutput.close();
+    m_alertOutput.close();
+    super.close();
+  }
 }

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",
@@ -290,6 +295,18 @@
     "mainclass": "Main",
     "commandversion": 2
   },
+  {
+    "name": "AprilTags Vision",
+    "description": "on-roboRIO detection of AprilTags using an attached USB camera.",
+    "tags": [
+      "Vision",
+      "AprilTags"
+    ],
+    "foldername": "apriltagsvision",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
   {
     "name": "Shuffleboard Sample",
     "description": "An example program that adds data to various Shuffleboard tabs that demonstrates the Shuffleboard API",
@@ -798,7 +815,8 @@
     "name": "Digital Communication Sample",
     "description": "An example program that communicates with external devices (such as an Arduino) using the roboRIO's DIO",
     "tags": [
-      "Digital"
+      "Digital",
+      "Unit Testing"
     ],
     "foldername": "digitalcommunication",
     "gradlebase": "java",

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

@@ -6,6 +6,7 @@ package edu.wpi.first.wpilibj.examples.i2ccommunication;
 
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.I2C;
+import edu.wpi.first.wpilibj.I2C.Port;
 import edu.wpi.first.wpilibj.TimedRobot;
 
 /**
@@ -13,9 +14,10 @@ import edu.wpi.first.wpilibj.TimedRobot;
  * code using the roboRIO's I2C port.
  */
 public class Robot extends TimedRobot {
-  private static int kDeviceAddress = 4;
+  static final Port kPort = Port.kOnboard;
+  private static final int kDeviceAddress = 4;
 
-  private static I2C m_arduino = new I2C(I2C.Port.kOnboard, kDeviceAddress);
+  private final I2C m_arduino = new I2C(kPort, kDeviceAddress);
 
   private void writeString(String input) {
     // Creates a char array from the input string
@@ -30,7 +32,7 @@ public class Robot extends TimedRobot {
     }
 
     // Writes bytes over I2C
-    m_arduino.transaction(data, data.length, null, 0);
+    m_arduino.transaction(data, data.length, new byte[] {}, 0);
   }
 
   @Override
@@ -54,4 +56,11 @@ public class Robot extends TimedRobot {
 
     writeString(stateMessage.toString());
   }
+
+  /** Close all resources. */
+  @Override
+  public void close() {
+    m_arduino.close();
+    super.close();
+  }
 }

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/digitalcommunication/DigitalCommunicationTest.java

@@ -0,0 +1,114 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.digitalcommunication;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.AllianceStationID;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.DIOSim;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.parallel.ResourceLock;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.EnumSource;
+import org.junit.jupiter.params.provider.ValueSource;
+
+@ResourceLock("timing")
+class DigitalCommunicationTest {
+  private Robot m_robot;
+  private Thread m_thread;
+  private final DIOSim m_allianceOutput = new DIOSim(Robot.kAlliancePort);
+  private final DIOSim m_enabledOutput = new DIOSim(Robot.kEnabledPort);
+  private final DIOSim m_autonomousOutput = new DIOSim(Robot.kAutonomousPort);
+  private final DIOSim m_alertOutput = new DIOSim(Robot.kAlertPort);
+
+  @BeforeEach
+  void startThread() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+    DriverStationSim.resetData();
+    m_robot = new Robot();
+    m_thread = new Thread(m_robot::startCompetition);
+    m_thread.start();
+    SimHooks.stepTiming(0.0); // Wait for Notifiers
+  }
+
+  @AfterEach
+  void stopThread() {
+    m_robot.endCompetition();
+    try {
+      m_thread.interrupt();
+      m_thread.join();
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    m_robot.close();
+    m_allianceOutput.resetData();
+    m_enabledOutput.resetData();
+    m_autonomousOutput.resetData();
+    m_alertOutput.resetData();
+  }
+
+  @EnumSource(AllianceStationID.class)
+  @ParameterizedTest(name = "alliance[{index}]: {0}")
+  void allianceTest(AllianceStationID alliance) {
+    DriverStationSim.setAllianceStationId(alliance);
+    DriverStationSim.notifyNewData();
+
+    assertTrue(m_allianceOutput.getInitialized());
+    assertFalse(m_allianceOutput.getIsInput());
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(alliance.name().startsWith("Red"), m_allianceOutput.getValue());
+  }
+
+  @ValueSource(booleans = {true, false})
+  @ParameterizedTest(name = "enabled[{index}]: {0}")
+  void enabledTest(boolean enabled) {
+    DriverStationSim.setEnabled(enabled);
+    DriverStationSim.notifyNewData();
+
+    assertTrue(m_enabledOutput.getInitialized());
+    assertFalse(m_enabledOutput.getIsInput());
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(enabled, m_enabledOutput.getValue());
+  }
+
+  @ValueSource(booleans = {true, false})
+  @ParameterizedTest(name = "autonomous[{index}]: {0}")
+  void autonomousTest(boolean autonomous) {
+    DriverStationSim.setAutonomous(autonomous);
+    DriverStationSim.notifyNewData();
+
+    assertTrue(m_autonomousOutput.getInitialized());
+    assertFalse(m_autonomousOutput.getIsInput());
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(autonomous, m_autonomousOutput.getValue());
+  }
+
+  @ValueSource(doubles = {45.0, 27.0, 23.0})
+  @ParameterizedTest(name = "alert[{index}]: {0}s")
+  void alertTest(double matchTime) {
+    DriverStationSim.setMatchTime(matchTime);
+    DriverStationSim.notifyNewData();
+
+    assertTrue(m_alertOutput.getInitialized());
+    assertFalse(m_alertOutput.getIsInput());
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(matchTime <= 30 && matchTime >= 25, m_alertOutput.getValue());
+  }
+}

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

wpilibjExamples/src/test/java/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);
+    }
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java

@@ -55,6 +55,9 @@ public class PIDController implements Sendable, AutoCloseable {
   private double m_setpoint;
   private double m_measurement;
 
+  private boolean m_haveMeasurement;
+  private boolean m_haveSetpoint;
+
   /**
    * Allocates a PIDController with the given constants for kp, ki, and kd and a default period of
    * 0.02 seconds.
@@ -199,6 +202,7 @@ public class PIDController implements Sendable, AutoCloseable {
    */
   public void setSetpoint(double setpoint) {
     m_setpoint = setpoint;
+    m_haveSetpoint = true;
 
     if (m_continuous) {
       double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
@@ -227,7 +231,9 @@ public class PIDController implements Sendable, AutoCloseable {
    * @return Whether the error is within the acceptable bounds.
    */
   public boolean atSetpoint() {
-    return Math.abs(m_positionError) < m_positionTolerance
+    return m_haveMeasurement
+        && m_haveSetpoint
+        && Math.abs(m_positionError) < m_positionTolerance
         && Math.abs(m_velocityError) < m_velocityTolerance;
   }
 
@@ -333,6 +339,7 @@ public class PIDController implements Sendable, AutoCloseable {
   public double calculate(double measurement) {
     m_measurement = measurement;
     m_prevError = m_positionError;
+    m_haveMeasurement = true;
 
     if (m_continuous) {
       double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
@@ -360,6 +367,7 @@ public class PIDController implements Sendable, AutoCloseable {
     m_prevError = 0;
     m_totalError = 0;
     m_velocityError = 0;
+    m_haveMeasurement = false;
   }
 
   @Override

wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java

@@ -212,7 +212,16 @@ public class DifferentialDrivePoseEstimator {
         sample.get().rightMeters,
         sample.get().poseMeters.exp(scaledTwist));
 
-    // Step 6: Replay odometry inputs between sample time and latest recorded sample to update the
+    // Step 6: Record the current pose to allow multiple measurements from the same timestamp
+    m_poseBuffer.addSample(
+        timestampSeconds,
+        new InterpolationRecord(
+            getEstimatedPosition(),
+            sample.get().gyroAngle,
+            sample.get().leftMeters,
+            sample.get().rightMeters));
+
+    // Step 7: Replay odometry inputs between sample time and latest recorded sample to update the
     // pose buffer and correct odometry.
     for (Map.Entry<Double, InterpolationRecord> entry :
         m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) {

wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java

@@ -199,7 +199,13 @@ public class MecanumDrivePoseEstimator {
         sample.get().wheelPositions,
         sample.get().poseMeters.exp(scaledTwist));
 
-    // Step 6: Replay odometry inputs between sample time and latest recorded sample to update the
+    // Step 6: Record the current pose to allow multiple measurements from the same timestamp
+    m_poseBuffer.addSample(
+        timestampSeconds,
+        new InterpolationRecord(
+            getEstimatedPosition(), sample.get().gyroAngle, sample.get().wheelPositions));
+
+    // Step 7: Replay odometry inputs between sample time and latest recorded sample to update the
     // pose buffer and correct odometry.
     for (Map.Entry<Double, InterpolationRecord> entry :
         m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) {

wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java

@@ -201,7 +201,13 @@ public class SwerveDrivePoseEstimator {
         sample.get().modulePositions,
         sample.get().poseMeters.exp(scaledTwist));
 
-    // Step 6: Replay odometry inputs between sample time and latest recorded sample to update the
+    // Step 6: Record the current pose to allow multiple measurements from the same timestamp
+    m_poseBuffer.addSample(
+        timestampSeconds,
+        new InterpolationRecord(
+            getEstimatedPosition(), sample.get().gyroAngle, sample.get().modulePositions));
+
+    // Step 7: Replay odometry inputs between sample time and latest recorded sample to update the
     // pose buffer and correct odometry.
     for (Map.Entry<Double, InterpolationRecord> entry :
         m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) {

wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java

@@ -39,7 +39,7 @@ public class SwerveDriveKinematics {
 
   private final int m_numModules;
   private final Translation2d[] m_modules;
-  private final SwerveModuleState[] m_moduleStates;
+  private SwerveModuleState[] m_moduleStates;
   private Translation2d m_prevCoR = new Translation2d();
 
   /**
@@ -97,10 +97,12 @@ public class SwerveDriveKinematics {
     if (chassisSpeeds.vxMetersPerSecond == 0.0
         && chassisSpeeds.vyMetersPerSecond == 0.0
         && chassisSpeeds.omegaRadiansPerSecond == 0.0) {
+      SwerveModuleState[] newStates = new SwerveModuleState[m_numModules];
       for (int i = 0; i < m_numModules; i++) {
-        m_moduleStates[i].speedMetersPerSecond = 0.0;
+        newStates[i] = new SwerveModuleState(0.0, m_moduleStates[i].angle);
       }
 
+      m_moduleStates = newStates;
       return m_moduleStates;
     }
 
@@ -132,6 +134,7 @@ public class SwerveDriveKinematics {
 
     var moduleStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
 
+    m_moduleStates = new SwerveModuleState[m_numModules];
     for (int i = 0; i < m_numModules; i++) {
       double x = moduleStatesMatrix.get(i * 2, 0);
       double y = moduleStatesMatrix.get(i * 2 + 1, 0);

wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java

@@ -32,14 +32,15 @@ public class SwerveModulePosition implements Comparable<SwerveModulePosition> {
   @Override
   public boolean equals(Object obj) {
     if (obj instanceof SwerveModulePosition) {
-      return Double.compare(distanceMeters, ((SwerveModulePosition) obj).distanceMeters) == 0;
+      SwerveModulePosition other = (SwerveModulePosition) obj;
+      return Math.abs(other.distanceMeters - distanceMeters) < 1E-9 && angle.equals(other.angle);
     }
     return false;
   }
 
   @Override
   public int hashCode() {
-    return Objects.hash(distanceMeters);
+    return Objects.hash(distanceMeters, angle);
   }
 
   /**

wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java

@@ -32,15 +32,16 @@ public class SwerveModuleState implements Comparable<SwerveModuleState> {
   @Override
   public boolean equals(Object obj) {
     if (obj instanceof SwerveModuleState) {
-      return Double.compare(speedMetersPerSecond, ((SwerveModuleState) obj).speedMetersPerSecond)
-          == 0;
+      SwerveModuleState other = (SwerveModuleState) obj;
+      return Math.abs(other.speedMetersPerSecond - speedMetersPerSecond) < 1E-9
+          && angle.equals(other.angle);
     }
     return false;
   }
 
   @Override
   public int hashCode() {
-    return Objects.hash(speedMetersPerSecond);
+    return Objects.hash(speedMetersPerSecond, angle);
   }
 
   /**

wpimath/src/main/native/cpp/controller/PIDController.cpp

@@ -78,6 +78,7 @@ double PIDController::GetVelocityTolerance() const {
 
 void PIDController::SetSetpoint(double setpoint) {
   m_setpoint = setpoint;
+  m_haveSetpoint = true;
 
   if (m_continuous) {
     double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
@@ -95,7 +96,8 @@ double PIDController::GetSetpoint() const {
 }
 
 bool PIDController::AtSetpoint() const {
-  return std::abs(m_positionError) < m_positionTolerance &&
+  return m_haveMeasurement && m_haveSetpoint &&
+         std::abs(m_positionError) < m_positionTolerance &&
          std::abs(m_velocityError) < m_velocityTolerance;
 }
 
@@ -137,6 +139,7 @@ double PIDController::GetVelocityError() const {
 double PIDController::Calculate(double measurement) {
   m_measurement = measurement;
   m_prevError = m_positionError;
+  m_haveMeasurement = true;
 
   if (m_continuous) {
     double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
@@ -167,6 +170,7 @@ void PIDController::Reset() {
   m_prevError = 0;
   m_totalError = 0;
   m_velocityError = 0;
+  m_haveMeasurement = false;
 }
 
 void PIDController::InitSendable(wpi::SendableBuilder& builder) {

wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp

@@ -121,7 +121,13 @@ void DifferentialDrivePoseEstimator::AddVisionMeasurement(
       sample.value().gyroAngle, sample.value().leftDistance,
       sample.value().rightDistance, sample.value().pose.Exp(scaledTwist));
 
-  // Step 6: Replay odometry inputs between sample time and latest recorded
+  // Step 6: Record the current pose to allow multiple measurements from the
+  // same timestamp
+  m_poseBuffer.AddSample(
+      timestamp, {GetEstimatedPosition(), sample.value().gyroAngle,
+                  sample.value().leftDistance, sample.value().rightDistance});
+
+  // Step 7: Replay odometry inputs between sample time and latest recorded
   // sample to update the pose buffer and correct odometry.
   auto internal_buf = m_poseBuffer.GetInternalBuffer();
 

wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp

@@ -132,7 +132,13 @@ void frc::MecanumDrivePoseEstimator::AddVisionMeasurement(
                            sample.value().wheelPositions,
                            sample.value().pose.Exp(scaledTwist));
 
-  // Step 6: Replay odometry inputs between sample time and latest recorded
+  // Step 6: Record the current pose to allow multiple measurements from the
+  // same timestamp
+  m_poseBuffer.AddSample(timestamp,
+                         {GetEstimatedPosition(), sample.value().gyroAngle,
+                          sample.value().wheelPositions});
+
+  // Step 7: Replay odometry inputs between sample time and latest recorded
   // sample to update the pose buffer and correct odometry.
   auto internal_buf = m_poseBuffer.GetInternalBuffer();
 

wpimath/src/main/native/cpp/kinematics/SwerveModulePosition.cpp

@@ -0,0 +1,15 @@
+// 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 "frc/kinematics/SwerveModulePosition.h"
+
+#include "frc/kinematics/SwerveModuleState.h"
+#include "units/math.h"
+
+using namespace frc;
+
+bool SwerveModulePosition::operator==(const SwerveModulePosition& other) const {
+  return units::math::abs(distance - other.distance) < 1E-9_m &&
+         angle == other.angle;
+}

wpimath/src/main/native/cpp/kinematics/SwerveModuleState.cpp

@@ -0,0 +1,24 @@
+// 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 "frc/kinematics/SwerveModuleState.h"
+
+#include "units/math.h"
+
+using namespace frc;
+
+bool SwerveModuleState::operator==(const SwerveModuleState& other) const {
+  return units::math::abs(speed - other.speed) < 1E-9_mps &&
+         angle == other.angle;
+}
+
+SwerveModuleState SwerveModuleState::Optimize(
+    const SwerveModuleState& desiredState, const Rotation2d& currentAngle) {
+  auto delta = desiredState.angle - currentAngle;
+  if (units::math::abs(delta.Degrees()) > 90_deg) {
+    return {-desiredState.speed, desiredState.angle + Rotation2d{180_deg}};
+  } else {
+    return {desiredState.speed, desiredState.angle};
+  }
+}

wpimath/src/main/native/include/frc/controller/PIDController.h

@@ -252,6 +252,9 @@ class WPILIB_DLLEXPORT PIDController
 
   double m_setpoint = 0;
   double m_measurement = 0;
+
+  bool m_haveSetpoint = false;
+  bool m_haveMeasurement = false;
 };
 
 }  // namespace frc2

wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h

@@ -194,10 +194,16 @@ class SwerveDrivePoseEstimator {
 
     // Step 5: Reset Odometry to state at sample with vision adjustment.
     m_odometry.ResetPosition(sample.value().gyroAngle,
-                             sample.value().modulePostions,
+                             sample.value().modulePositions,
                              sample.value().pose.Exp(scaledTwist));
 
-    // Step 6: Replay odometry inputs between sample time and latest recorded
+    // Step 6: Record the current pose to allow multiple measurements from the
+    // same timestamp
+    m_poseBuffer.AddSample(timestamp,
+                           {GetEstimatedPosition(), sample.value().gyroAngle,
+                            sample.value().modulePositions});
+
+    // Step 7: Replay odometry inputs between sample time and latest recorded
     // sample to update the pose buffer and correct odometry.
     auto internal_buf = m_poseBuffer.GetInternalBuffer();
 
@@ -207,7 +213,7 @@ class SwerveDrivePoseEstimator {
 
     for (auto entry = upper_bound; entry != internal_buf.end(); entry++) {
       UpdateWithTime(entry->first, entry->second.gyroAngle,
-                     entry->second.modulePostions);
+                     entry->second.modulePositions);
     }
   }
 
@@ -300,7 +306,7 @@ class SwerveDrivePoseEstimator {
     Rotation2d gyroAngle;
 
     // The distances traveled and rotations meaured at each module.
-    wpi::array<SwerveModulePosition, NumModules> modulePostions;
+    wpi::array<SwerveModulePosition, NumModules> modulePositions;
 
     /**
      * Checks equality between this InterpolationRecord and another object.
@@ -344,14 +350,14 @@ class SwerveDrivePoseEstimator {
 
         for (size_t i = 0; i < NumModules; i++) {
           modulePositions[i].distance =
-              wpi::Lerp(this->modulePostions[i].distance,
-                        endValue.modulePostions[i].distance, i);
+              wpi::Lerp(this->modulePositions[i].distance,
+                        endValue.modulePositions[i].distance, i);
           modulePositions[i].angle =
-              wpi::Lerp(this->modulePostions[i].angle,
-                        endValue.modulePostions[i].angle, i);
+              wpi::Lerp(this->modulePositions[i].angle,
+                        endValue.modulePositions[i].angle, i);
 
           modulesDelta[i].distance =
-              modulePositions[i].distance - this->modulePostions[i].distance;
+              modulePositions[i].distance - this->modulePositions[i].distance;
           modulesDelta[i].angle = modulePositions[i].angle;
         }
 

wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h

@@ -68,11 +68,24 @@ class TimeInterpolatableBuffer {
     if (m_pastSnapshots.size() == 0 || time > m_pastSnapshots.back().first) {
       m_pastSnapshots.emplace_back(time, sample);
     } else {
-      m_pastSnapshots.insert(
-          std::upper_bound(
-              m_pastSnapshots.begin(), m_pastSnapshots.end(), time,
-              [](auto t, const auto& pair) { return t < pair.first; }),
-          std::pair(time, sample));
+      auto first_after = std::upper_bound(
+          m_pastSnapshots.begin(), m_pastSnapshots.end(), time,
+          [](auto t, const auto& pair) { return t < pair.first; });
+
+      auto last_not_greater_than = first_after - 1;
+
+      if (last_not_greater_than == m_pastSnapshots.begin() ||
+          last_not_greater_than->first < time) {
+        // Two cases handled together:
+        // 1. All entries come after the sample
+        // 2. Some entries come before the sample, but none are recorded with
+        // the same time
+        m_pastSnapshots.insert(first_after, std::pair(time, sample));
+      } else {
+        // Final case:
+        // 3. An entry exists with the same recorded time.
+        last_not_greater_than->second = sample;
+      }
     }
     while (time - m_pastSnapshots[0].first > m_historySize) {
       m_pastSnapshots.erase(m_pastSnapshots.begin());

wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h

@@ -25,5 +25,13 @@ struct WPILIB_DLLEXPORT SwerveModulePosition {
    * Angle of the module.
    */
   Rotation2d angle;
+
+  /**
+   * Checks equality between this SwerveModulePosition and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const SwerveModulePosition& other) const;
 };
 }  // namespace frc