[hal] Fix USB serial port assignment in SerialHelper (#8689)

This is just #7793 with requested changes applied.

.github/workflows/gradle.yml

@@ -52,7 +52,7 @@ jobs:
         run: echo "EXTRA_GRADLE_ARGS=-PreleaseMode" >> $GITHUB_ENV
         if: startsWith(github.ref, 'refs/tags/v') && !contains(github.ref, '2027')
       - name: Build with Gradle
-        uses: addnab/docker-run-action@3e77f186b7a929ef010f183a9e24c0f9955ea609
+        uses: wpilibsuite/docker-run-action@v4
         with:
           image: ${{ matrix.container }}
           options: -v ${{ github.workspace }}:/work -w /work -e ARTIFACTORY_PUBLISH_USERNAME -e ARTIFACTORY_PUBLISH_PASSWORD -e GITHUB_REF -e CI

.github/workflows/sentinel-build.yml

@@ -53,7 +53,7 @@ jobs:
         with:
           fetch-depth: 0
       - name: Build with Gradle
-        uses: addnab/docker-run-action@3e77f186b7a929ef010f183a9e24c0f9955ea609
+        uses: wpilibsuite/docker-run-action@v4
         with:
           image: ${{ matrix.container }}
           options: -v ${{ github.workspace }}:/work -w /work -e GITHUB_REF -e CI

.github/workflows/tools.yml

@@ -31,11 +31,11 @@ jobs:
           fetch-depth: 0
       - uses: gradle/actions/wrapper-validation@v4
       - name: Build WPILib with Gradle
-        uses: addnab/docker-run-action@v3
+        uses: wpilibsuite/docker-run-action@v4
         with:
           image: wpilib/roborio-cross-ubuntu:2025-22.04
           options: -v ${{ github.workspace }}:/work -w /work -e GITHUB_REF -e CI -e DISPLAY
-          run: df . && rm -f semicolon_delimited_script && ./gradlew :wpilibc:publish :wpilibj:publish :wpilibNewCommands:publish :hal:publish :cameraserver:publish :ntcore:publish :cscore:publish :wpimath:publish :wpinet:publish :wpiutil:publish :apriltag:publish :wpiunits:publish :simulation:halsim_gui:publish :simulation:halsim_ds_socket:publish :simulation:halsim_ws_server:publish :simulation:halsim_ws_client:publish :simulation:halsim_xrp:publish :fieldImages:publish :romiVendordep:publish :xrpVendordep:publish :epilogue-processor:publish :epilogue-runtime:publish :thirdparty:googletest:publish -x test -x Javadoc -x doxygen --build-cache && cp -r /root/releases/maven/development /work
+          run: df . && ./gradlew :wpilibc:publish :wpilibj:publish :wpilibNewCommands:publish :hal:publish :cameraserver:publish :ntcore:publish :cscore:publish :wpimath:publish :wpinet:publish :wpiutil:publish :apriltag:publish :wpiunits:publish :simulation:halsim_gui:publish :simulation:halsim_ds_socket:publish :simulation:halsim_ws_server:publish :simulation:halsim_ws_client:publish :simulation:halsim_xrp:publish :fieldImages:publish :romiVendordep:publish :xrpVendordep:publish :epilogue-processor:publish :epilogue-runtime:publish :thirdparty:googletest:publish -x test -x Javadoc -x doxygen --build-cache && cp -r /root/releases/maven/development /work
       - uses: actions/upload-artifact@v4
         with:
           name: MavenArtifacts

cscore/src/main/native/objcpp/UvcControlImpl.mm

@@ -484,7 +484,7 @@ const propertyInfo_t propertyInfo[] =
                 UVCERROR("USB control interface not found");
                 break;
             default:
-                UVCERROR("ControlRequest failed (KR=sys:sub:code) = {:02Xh}:{:03Xh}:{:04Xh}", 
+                UVCERROR("ControlRequest failed (KR=sys:sub:code) = {:02X}h:{:03X}h:{:04X}h", 
                     sys, sub, code);
                 break; 
         }

docs/build.gradle

@@ -152,7 +152,9 @@ configurations {
 task generateJavaDocs(type: Javadoc) {
     classpath += project(":wpilibj").sourceSets.main.compileClasspath
     options.links("https://docs.oracle.com/en/java/javase/17/docs/api/")
-    options.links("https://docs.opencv.org/4.x/javadoc/")
+    // workaround for opencv site blocking javadoc tool. If the link is changed,
+    // docs/opencv/element-list must be redownloaded
+    options.linksOffline("https://docs.opencv.org/4.10.0/javadoc/", "opencv")
     options.addStringOption("tag", "pre:a:Pre-Condition")
     options.addBooleanOption("Xdoclint/package:" +
             // TODO: v Document these, then remove them from the list

docs/opencv/element-list

@@ -0,0 +1,29 @@
+org.opencv.aruco
+org.opencv.bgsegm
+org.opencv.bioinspired
+org.opencv.calib3d
+org.opencv.core
+org.opencv.dnn
+org.opencv.dnn_superres
+org.opencv.face
+org.opencv.features2d
+org.opencv.highgui
+org.opencv.img_hash
+org.opencv.imgcodecs
+org.opencv.imgproc
+org.opencv.ml
+org.opencv.objdetect
+org.opencv.osgi
+org.opencv.phase_unwrapping
+org.opencv.photo
+org.opencv.plot
+org.opencv.structured_light
+org.opencv.text
+org.opencv.tracking
+org.opencv.utils
+org.opencv.video
+org.opencv.videoio
+org.opencv.wechat_qrcode
+org.opencv.xfeatures2d
+org.opencv.ximgproc
+org.opencv.xphoto

glass/src/lib/native/cpp/other/FMS.cpp

@@ -71,11 +71,11 @@ void glass::DisplayFMS(FMSModel* model, bool editableDsAttached) {
     }
     ImGui::SameLine();
     if (ImGui::Button("Auto") && !enabled) {
-      model->SetMatchTime(15.0);
+      model->SetMatchTime(20.0);
     }
     ImGui::SameLine();
     if (ImGui::Button("Teleop") && !enabled) {
-      model->SetMatchTime(135.0);
+      model->SetMatchTime(140.0);
     }
   }
 

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

@@ -308,16 +308,19 @@ public class DriverStationJNI extends JNIWrapper {
   public static native int getJoystickAxisType(byte joystickNum, byte axis);
 
   /**
-   * Returns the approximate match time.
+   * Return the approximate match time. The FMS does not send an official match time to the robots,
+   * but does send an approximate match time. The value will count down the time remaining in the
+   * current period (auto or teleop). Warning: This is not an official time (so it cannot be used to
+   * dispute ref calls or guarantee that a function will trigger before the match ends).
    *
-   * <p>The FMS does not send an official match time to the robots, but does send an approximate
-   * match time. The value will count down the time remaining in the current period (auto or
-   * teleop).
+   * <p>When connected to the real field, this number only changes in full integer increments, and
+   * always counts down.
    *
-   * <p>Warning: This is not an official time (so it cannot be used to dispute ref calls or
-   * guarantee that a function will trigger before the match ends).
+   * <p>When the DS is in practice mode, this number is a floating point number, and counts down.
    *
-   * <p>The Practice Match function of the DS approximates the behavior seen on the field.
+   * <p>When the DS is in teleop or autonomous mode, this number returns -1.0.
+   *
+   * <p>Simulation matches DS behavior without an FMS connected.
    *
    * @return time remaining in current match period (auto or teleop)
    * @see "HAL_GetMatchTime"

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

@@ -138,10 +138,10 @@ void SerialHelper::SortHubPathVector() {
   m_sortedHubPath = m_unsortedHubPath;
   std::sort(m_sortedHubPath.begin(), m_sortedHubPath.end(),
             [](const wpi::SmallVectorImpl<char>& lhs,
-               const wpi::SmallVectorImpl<char>& rhs) -> int {
+               const wpi::SmallVectorImpl<char>& rhs) -> bool {
               std::string_view lhsRef(lhs.begin(), lhs.size());
               std::string_view rhsRef(rhs.begin(), rhs.size());
-              return lhsRef.compare(rhsRef);
+              return lhsRef < rhsRef;
             });
 }
 
@@ -162,6 +162,11 @@ void SerialHelper::CoiteratedSort(
 }
 
 void SerialHelper::QueryHubPaths(int32_t* status) {
+  m_unsortedHubPath.clear();
+  m_visaResource.clear();
+  m_osResource.clear();
+  m_sortedHubPath.clear();
+
   // VISA resource matching string
   const char* str = "?*";
   // Items needed for VISA
@@ -275,12 +280,14 @@ void SerialHelper::QueryHubPaths(int32_t* status) {
         continue;
       }
 
+      std::string_view hubPathComponent = pathSplitVec[hubIndex - 2];
+      std::string_view osPath =
+          wpi::split(wpi::split(osName, "(").second, ")").first;
+
       // Add our devices to our list
-      m_unsortedHubPath.emplace_back(
-          std::string_view{pathSplitVec[hubIndex - 2]});
+      m_unsortedHubPath.emplace_back(hubPathComponent);
       m_visaResource.emplace_back(desc);
-      m_osResource.emplace_back(
-          wpi::split(wpi::split(osName, "(").second, ")").first);
+      m_osResource.emplace_back(osPath);
       break;
     }
   }
@@ -303,14 +310,16 @@ int32_t SerialHelper::GetIndexForPort(HAL_SerialPort port, int32_t* status) {
 
   // If port has not been assigned, find the one to assign
   if (portString.empty()) {
+    // local copy to avoid modifying original
+    auto availableHubPaths = m_sortedHubPath;
+
     for (size_t i = 0; i < 2; i++) {
-      // Remove all used ports
+      // Remove all used ports from the local copy
       auto idx = std::find_if(
-          m_sortedHubPath.begin(), m_sortedHubPath.end(),
+          availableHubPaths.begin(), availableHubPaths.end(),
           [&](const auto& s) { return wpi::equals(s, m_usbNames[i]); });
-      if (idx != m_sortedHubPath.end()) {
-        // found
-        m_sortedHubPath.erase(idx);
+      if (idx != availableHubPaths.end()) {
+        availableHubPaths.erase(idx);
       }
       if (m_usbNames[i] == "") {
         indices.push_back(i);
@@ -330,12 +339,12 @@ int32_t SerialHelper::GetIndexForPort(HAL_SerialPort port, int32_t* status) {
       return -1;
     }
 
-    if (idx >= static_cast<int32_t>(m_sortedHubPath.size())) {
+    if (idx >= static_cast<int32_t>(availableHubPaths.size())) {
       *status = HAL_SERIAL_PORT_NOT_FOUND;
       return -1;
     }
 
-    portString = m_sortedHubPath[idx].str();
+    portString = availableHubPaths[idx].str();
     m_usbNames[port - 2] = portString;
   }
 

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

@@ -187,8 +187,7 @@ int32_t HAL_SetJoystickOutputs(int32_t joystickNum, int64_t outputs,
  * <p>When the DS is in practice mode, this number is a floating point number,
  * and counts down.
  *
- * <p>When the DS is in teleop or autonomous mode, this number is a floating
- * point number, and counts up.
+ * <p>When the DS is in teleop or autonomous mode, this number returns -1.0.
  *
  * <p>Simulation matches DS behavior without an FMS connected.
  *

ntcore/src/main/java/edu/wpi/first/networktables/Topic.java

@@ -330,8 +330,8 @@ public class Topic {
   }
 
   /** NetworkTables instance. */
-  protected NetworkTableInstance m_inst;
+  protected final NetworkTableInstance m_inst;
 
   /** NetworkTables handle. */
-  protected int m_handle;
+  protected final int m_handle;
 }

ntcore/src/main/native/cpp/NetworkServer.cpp

@@ -362,6 +362,21 @@ void NetworkServer::ProcessAllLocal() {
 }
 
 void NetworkServer::LoadPersistent() {
+  // check if SavePersistent was interrupted and left a backup file;
+  // if so, try to restore it
+  auto bak = fmt::format("{}.bck", m_persistentFilename);
+  if (!fs::exists(m_persistentFilename) && fs::exists(bak)) {
+    INFO(
+        "restoring persistent file from backup '{}', since original '{}' is "
+        "missing",
+        bak, m_persistentFilename);
+    std::error_code ec;
+    fs::rename(bak, m_persistentFilename, ec);
+    if (ec.value() != 0) {
+      INFO("failed to restore backup: {}", ec.message());
+    }
+  }
+
   auto fileBuffer = wpi::MemoryBuffer::GetFile(m_persistentFilename);
   if (!fileBuffer) {
     INFO(

ntcore/src/test/native/cpp/NetworkServerTest.cpp

@@ -0,0 +1,179 @@
+// 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 <chrono>
+#include <filesystem>
+#include <fstream>
+#include <string>
+#include <thread>
+
+#include <gtest/gtest.h>
+
+#include "networktables/IntegerTopic.h"
+#include "networktables/NetworkTableInstance.h"
+
+// Valid persistent JSON containing a single persistent integer topic.
+static constexpr const char* kPersistentJson = R"([
+  {
+    "name": "/test/persistent_value",
+    "type": "int",
+    "value": 42,
+    "properties": {"persistent": true}
+  }
+])";
+
+class NetworkServerPersistentTest : public ::testing::Test {
+ public:
+  NetworkServerPersistentTest() {
+    // Create a unique temp directory for each test
+    m_tempDir =
+        std::filesystem::temp_directory_path() /
+        ("ntcore_test_" +
+         std::to_string(
+             std::chrono::steady_clock::now().time_since_epoch().count()));
+    std::filesystem::create_directories(m_tempDir);
+    m_persistFile = (m_tempDir / "test_persistent.json").string();
+  }
+
+  ~NetworkServerPersistentTest() override {
+    std::error_code ec;
+    std::filesystem::remove_all(m_tempDir, ec);
+  }
+
+ protected:
+  // Write content to a file.
+  static void WriteFile(const std::string& path, const std::string& content) {
+    std::ofstream os{path};
+    ASSERT_TRUE(os.is_open()) << "Failed to create file: " << path;
+    os << content;
+  }
+
+  // Wait for the server to finish initializing.  Returns true if a topic with
+  // the given name was seen before the timeout expired.
+  bool WaitForTopic(
+      nt::NetworkTableInstance& inst, std::string_view name,
+      std::chrono::milliseconds timeout = std::chrono::milliseconds{3000}) {
+    auto deadline = std::chrono::steady_clock::now() + timeout;
+    while (std::chrono::steady_clock::now() < deadline) {
+      auto infos = inst.GetTopicInfo(name);
+      if (!infos.empty()) {
+        return true;
+      }
+      std::this_thread::sleep_for(std::chrono::milliseconds{50});
+    }
+    return false;
+  }
+
+  std::filesystem::path m_tempDir;
+  std::string m_persistFile;
+};
+
+// Verify that LoadPersistent restores from the .bck backup file when the
+// original persistent file is missing.  This simulates SavePersistent being
+// interrupted after renaming the original file to .bck but before the
+// temporary file has been renamed to the original filename.
+TEST_F(NetworkServerPersistentTest,
+       LoadPersistentRestoresFromBackupWhenOriginalMissing) {
+  // Set up "interrupted" state: only .bck file exists, no original.
+  std::string backupFile = m_persistFile + ".bck";
+  WriteFile(backupFile, kPersistentJson);
+  ASSERT_TRUE(std::filesystem::exists(backupFile));
+  ASSERT_FALSE(std::filesystem::exists(m_persistFile));
+
+  // Start a server that references the (missing) persistent file.
+  // Subscribe BEFORE starting the server so the server's local client has a
+  // matching subscriber when persistent topics are announced.
+  auto inst = nt::NetworkTableInstance::Create();
+  nt::IntegerSubscriber sub =
+      inst.GetIntegerTopic("/test/persistent_value").Subscribe(0);
+  inst.StartServer(m_persistFile, "127.0.0.1");
+
+  // Wait for the persistent topic to appear.
+  EXPECT_TRUE(WaitForTopic(inst, "/test/persistent_value"))
+      << "LoadPersistent did not restore from the .bck backup file";
+
+  // Also verify the value is correct.
+  EXPECT_EQ(sub.Get(), 42);
+
+  // The .bck should have been renamed to the original filename.
+  EXPECT_TRUE(std::filesystem::exists(m_persistFile));
+
+  inst.StopServer();
+  nt::NetworkTableInstance::Destroy(inst);
+}
+
+// Verify that LoadPersistent works normally when the original persistent file
+// is present (no interruption scenario).
+TEST_F(NetworkServerPersistentTest, LoadPersistentNormalLoad) {
+  // Write the persistent file directly (no backup).
+  WriteFile(m_persistFile, kPersistentJson);
+  ASSERT_TRUE(std::filesystem::exists(m_persistFile));
+
+  auto inst = nt::NetworkTableInstance::Create();
+  nt::IntegerSubscriber sub =
+      inst.GetIntegerTopic("/test/persistent_value").Subscribe(0);
+  inst.StartServer(m_persistFile, "127.0.0.1");
+
+  EXPECT_TRUE(WaitForTopic(inst, "/test/persistent_value"))
+      << "LoadPersistent did not load the persistent file";
+
+  EXPECT_EQ(sub.Get(), 42);
+
+  inst.StopServer();
+  nt::NetworkTableInstance::Destroy(inst);
+}
+
+// Verify that when both the original file and .bck exist, the original file
+// takes precedence (the backup is not used).
+TEST_F(NetworkServerPersistentTest, LoadPersistentPrefersOriginalOverBackup) {
+  // Original file with value 100.
+  static constexpr const char* kOriginalJson = R"([
+  {
+    "name": "/test/persistent_value",
+    "type": "int",
+    "value": 100,
+    "properties": {"persistent": true}
+  }
+])";
+
+  // Backup file with a different value (42).
+  WriteFile(m_persistFile, kOriginalJson);
+  WriteFile(m_persistFile + ".bck", kPersistentJson);
+  ASSERT_TRUE(std::filesystem::exists(m_persistFile));
+  ASSERT_TRUE(std::filesystem::exists(m_persistFile + ".bck"));
+
+  auto inst = nt::NetworkTableInstance::Create();
+  nt::IntegerSubscriber sub =
+      inst.GetIntegerTopic("/test/persistent_value").Subscribe(0);
+  inst.StartServer(m_persistFile, "127.0.0.1");
+
+  EXPECT_TRUE(WaitForTopic(inst, "/test/persistent_value"))
+      << "LoadPersistent did not load any persistent file";
+
+  // The value should come from the original (100), not the backup (42).
+  EXPECT_EQ(sub.Get(), 100);
+
+  inst.StopServer();
+  nt::NetworkTableInstance::Destroy(inst);
+}
+
+// Verify that LoadPersistent handles a missing persistent file and no backup
+// gracefully (no crash, no topics loaded).
+TEST_F(NetworkServerPersistentTest, LoadPersistentNoFile) {
+  ASSERT_FALSE(std::filesystem::exists(m_persistFile));
+  ASSERT_FALSE(std::filesystem::exists(m_persistFile + ".bck"));
+
+  auto inst = nt::NetworkTableInstance::Create();
+  inst.StartServer(m_persistFile, "127.0.0.1");
+
+  // Give the server time to initialize.
+  std::this_thread::sleep_for(std::chrono::milliseconds{500});
+
+  // No persistent topics should exist.
+  auto infos = inst.GetTopicInfo("/test/persistent_value");
+  EXPECT_TRUE(infos.empty());
+
+  inst.StopServer();
+  nt::NetworkTableInstance::Destroy(inst);
+}

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

@@ -34,10 +34,15 @@ public class WaitUntilCommand extends Command {
    * guarantee that the time at which the action is performed will be judged to be legal by the
    * referees. When in doubt, add a safety factor or time the action manually.
    *
+   * <p>The match time counts down when connected to FMS or the DS is in practice mode for the
+   * current mode. When the DS is not connected to FMS or in practice mode, the command will not
+   * wait.
+   *
    * @param time the match time after which to end, in seconds
+   * @see edu.wpi.first.wpilibj.DriverStation#getMatchTime()
    */
   public WaitUntilCommand(double time) {
-    this(() -> Timer.getMatchTime() - time > 0);
+    this(() -> Timer.getMatchTime() < time);
   }
 
   @Override

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

@@ -14,7 +14,7 @@ WaitUntilCommand::WaitUntilCommand(std::function<bool()> condition)
     : m_condition{std::move(condition)} {}
 
 WaitUntilCommand::WaitUntilCommand(units::second_t time)
-    : m_condition{[=] { return frc::Timer::GetMatchTime() - time > 0_s; }} {}
+    : m_condition{[=] { return frc::Timer::GetMatchTime() < time; }} {}
 
 bool WaitUntilCommand::IsFinished() {
   return m_condition();

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

@@ -36,7 +36,12 @@ class WaitUntilCommand : public CommandHelper<Command, WaitUntilCommand> {
    * will be judged to be legal by the referees.  When in doubt, add a safety
    * factor or time the action manually.
    *
+   * The match time counts down when connected to FMS or the DS is in practice
+   * mode for the current mode. When the DS is not connected to FMS or in
+   * practice mode, the command will not wait.
+   *
    * @param time the match time after which to end, in seconds
+   * @see frc::DriverStation::GetMatchTime()
    */
   explicit WaitUntilCommand(units::second_t time);
 

wpilibc/src/main/native/include/frc/DriverStation.h

@@ -339,8 +339,7 @@ class DriverStation final {
    * <p>When the DS is in practice mode, this number is a floating point number,
    * and counts down.
    *
-   * <p>When the DS is in teleop or autonomous mode, this number is a floating
-   * point number, and counts up.
+   * <p>When the DS is in teleop or autonomous mode, this number returns -1.0.
    *
    * <p>Simulation matches DS behavior without an FMS connected.
    *

wpilibc/src/main/native/include/frc/TimedRobot.h

@@ -65,7 +65,7 @@ class TimedRobot : public IterativeRobotBase {
   ~TimedRobot() override;
 
   /**
-   * Return the system clock time in micrseconds for the start of the current
+   * Return the system clock time in microseconds for the start of the current
    * periodic loop. This is in the same time base as Timer.GetFPGATimestamp(),
    * but is stable through a loop. It is updated at the beginning of every
    * periodic callback (including the normal periodic loop).

wpilibc/src/main/native/include/frc/Timer.h

@@ -139,19 +139,23 @@ class Timer {
   static units::second_t GetFPGATimestamp();
 
   /**
-   * Return the approximate match time.
-   *
-   * The FMS does not send an official match time to the robots, but does send
-   * an approximate match time. The value will count down the time remaining in
-   * the current period (auto or teleop).
-   *
+   * Return the approximate match time. The FMS does not send an official match
+   * time to the robots, but does send an approximate match time. The value will
+   * count down the time remaining in the current period (auto or teleop).
    * Warning: This is not an official time (so it cannot be used to dispute ref
    * calls or guarantee that a function will trigger before the match ends).
    *
-   * The Practice Match function of the DS approximates the behavior seen on the
-   * field.
+   * <p>When connected to the real field, this number only changes in full
+   * integer increments, and always counts down.
    *
-   * @return Time remaining in current match period (auto or teleop)
+   * <p>When the DS is in practice mode, this number is a floating point number,
+   * and counts down.
+   *
+   * <p>When the DS is in teleop or autonomous mode, this number returns -1.0.
+   *
+   * <p>Simulation matches DS behavior without an FMS connected.
+   *
+   * @return Time remaining in current match period (auto or teleop) in seconds
    */
   static units::second_t GetMatchTime();
 

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

@@ -30,10 +30,13 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 2> {
    * @param gearing            The gear ratio of the arm (numbers greater than 1
    *                           represent reductions).
    * @param armLength          The length of the arm.
-   * @param minAngle           The minimum angle that the arm is capable of.
-   * @param maxAngle           The maximum angle that the arm is capable of.
+   * @param minAngle           The minimum angle that the arm is capable of,
+   *                           with 0 being horizontal.
+   * @param maxAngle           The maximum angle that the arm is capable of,
+   *                           with 0 being horizontal.
    * @param simulateGravity    Whether gravity should be simulated or not.
-   * @param startingAngle      The initial position of the arm.
+   * @param startingAngle      The initial position of the arm, with 0 being
+   *                           horizontal.
    * @param measurementStdDevs The standard deviations of the measurements.
    */
   SingleJointedArmSim(const LinearSystem<2, 1, 2>& system,
@@ -52,10 +55,13 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 2> {
    * @param moi                The moment of inertia of the arm. This can be
    *                           calculated from CAD software.
    * @param armLength          The length of the arm.
-   * @param minAngle           The minimum angle that the arm is capable of.
-   * @param maxAngle           The maximum angle that the arm is capable of.
+   * @param minAngle           The minimum angle that the arm is capable of,
+   *                           with 0 being horizontal.
+   * @param maxAngle           The maximum angle that the arm is capable of,
+   *                           with 0 being horizontal.
    * @param simulateGravity    Whether gravity should be simulated or not.
-   * @param startingAngle      The initial position of the arm.
+   * @param startingAngle      The initial position of the arm, with 0 being
+   *                           horizontal.
    * @param measurementStdDevs The standard deviation of the measurement noise.
    */
   SingleJointedArmSim(const DCMotor& gearbox, double gearing,

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

@@ -0,0 +1,57 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "Robot.h"
+
+Robot::Robot() {
+  wpi::SendableRegistry::AddChild(&m_drive, &m_leftMotor);
+  wpi::SendableRegistry::AddChild(&m_drive, &m_rightMotor);
+
+  // We need to invert one side of the drivetrain so that positive voltages
+  // result in both sides moving forward. Depending on how your robot's
+  // gearbox is constructed, you might have to invert the left side instead.
+  m_rightMotor.SetInverted(true);
+}
+
+/**
+ * This function is called every 20 ms, no matter the mode. Use
+ * this for items like diagnostics that you want ran during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() {}
+
+// This function is only once each time Autonomous is enabled
+void Robot::AutonomousInit() {
+  m_timer.Restart();
+}
+
+// This function is called periodically during autonomous mode
+void Robot::AutonomousPeriodic() {
+  // Drive for 2 seconds
+  if (m_timer.Get() < 2_s) {
+    // Drive forwards half speed, make sure to turn input squaring off
+    m_drive.ArcadeDrive(0.5, 0.0, false);
+  } else {
+    // Stop robot
+    m_drive.ArcadeDrive(0.0, 0.0, false);
+  }
+}
+
+// This function is only once each time telop is enabled
+void Robot::TeleopInit() {}
+
+// This function is called periodically during teleop mode
+void Robot::TeleopPeriodic() {
+  m_drive.ArcadeDrive(-m_XboxController.GetLeftY(),
+                      -m_XboxController.GetRightX());
+}
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif

wpilibcExamples/src/main/cpp/examples/Xrptimed/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 <frc/TimedRobot.h>
+#include <frc/Timer.h>
+#include <frc/XboxController.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc/xrp/XRPMotor.h>
+
+class Robot : public frc::TimedRobot {
+ public:
+  Robot();
+  void RobotPeriodic() override;
+  void AutonomousInit() override;
+  void AutonomousPeriodic() override;
+  void TeleopInit() override;
+  void TeleopPeriodic() override;
+
+ private:
+  frc::XRPMotor m_leftMotor{0};
+  frc::XRPMotor m_rightMotor{1};
+  frc::XboxController m_XboxController{0};
+  frc::Timer m_timer;
+
+  frc::DifferentialDrive m_drive{
+      [&](double output) { m_leftMotor.Set(output); },
+      [&](double output) { m_rightMotor.Set(output); }};
+};

wpilibcExamples/src/main/cpp/examples/examples.json

@@ -609,6 +609,22 @@
       "xrp"
     ]
   },
+  {
+    "name": "XRP Timed",
+    "description": "An very basic timed robot program that can be used with the XRP reference robot design.",
+    "tags": [
+      "XRP",
+      "Differential Drive",
+      "XboxController"
+    ],
+    "foldername": "Xrptimed",
+    "gradlebase": "cppxrp",
+    "mainclass": "Main",
+    "commandversion": 2,
+    "extravendordeps": [
+      "xrp"
+    ]
+  },
   {
     "name": "StateSpaceFlywheel",
     "description": "Control a flywheel using a state-space model (based on values from CAD), with a Kalman Filter and LQR.",

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

@@ -1182,8 +1182,7 @@ public final class DriverStation {
    *
    * <p>When the DS is in practice mode, this number is a floating point number, and counts down.
    *
-   * <p>When the DS is in teleop or autonomous mode, this number is a floating point number, and
-   * counts up.
+   * <p>When the DS is in teleop or autonomous mode, this number returns -1.0.
    *
    * <p>Simulation matches DS behavior without an FMS connected.
    *

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

@@ -83,6 +83,8 @@ public final class Preferences {
     if (m_listener != null) {
       m_listener.close();
     }
+
+    Topic typePublisherTopic = m_typePublisher.getTopic();
     m_listener =
         NetworkTableListener.createListener(
             m_tableSubscriber,
@@ -90,8 +92,8 @@ public final class Preferences {
             event -> {
               if (event.topicInfo != null) {
                 Topic topic = event.topicInfo.getTopic();
-                if (!topic.equals(m_typePublisher.getTopic())) {
-                  event.topicInfo.getTopic().setPersistent(true);
+                if (!topic.equals(typePublisherTopic)) {
+                  topic.setPersistent(true);
                 }
               }
             });

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

@@ -181,9 +181,9 @@ public class TimedRobot extends IterativeRobotBase {
   }
 
   /**
-   * Return the system clock time in micrseconds for the start of the current periodic loop. This is
-   * in the same time base as Timer.getFPGATimestamp(), but is stable through a loop. It is updated
-   * at the beginning of every periodic callback (including the normal periodic loop).
+   * Return the system clock time in microseconds for the start of the current periodic loop. This
+   * is in the same time base as Timer.getFPGATimestamp(), but is stable through a loop. It is
+   * updated at the beginning of every periodic callback (including the normal periodic loop).
    *
    * @return Robot running time in microseconds, as of the start of the current periodic function.
    */

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

@@ -40,8 +40,16 @@ public class Timer {
    * Return the approximate match time. The FMS does not send an official match time to the robots,
    * but does send an approximate match time. The value will count down the time remaining in the
    * current period (auto or teleop). Warning: This is not an official time (so it cannot be used to
-   * dispute ref calls or guarantee that a function will trigger before the match ends) The Practice
-   * Match function of the DS approximates the behavior seen on the field.
+   * dispute ref calls or guarantee that a function will trigger before the match ends).
+   *
+   * <p>When connected to the real field, this number only changes in full integer increments, and
+   * always counts down.
+   *
+   * <p>When the DS is in practice mode, this number is a floating point number, and counts down.
+   *
+   * <p>When the DS is in teleop or autonomous mode, this number returns -1.0.
+   *
+   * <p>Simulation matches DS behavior without an FMS connected.
    *
    * @return Time remaining in current match period (auto or teleop) in seconds
    */

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

@@ -44,10 +44,13 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
    * @param gearbox The type of and number of motors in the arm gearbox.
    * @param gearing The gearing of the arm (numbers greater than 1 represent reductions).
    * @param armLengthMeters The length of the arm.
-   * @param minAngleRads The minimum angle that the arm is capable of.
-   * @param maxAngleRads The maximum angle that the arm is capable of.
+   * @param minAngleRads The minimum angle that the arm is capable of, with 0 radians being
+   *     horizontal.
+   * @param maxAngleRads The maximum angle that the arm is capable of, with 0 radians being
+   *     horizontal.
    * @param simulateGravity Whether gravity should be simulated or not.
-   * @param startingAngleRads The initial position of the Arm simulation in radians.
+   * @param startingAngleRads The initial position of the Arm simulation in radians, with 0 radians
+   *     being horizontal.
    * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
    *     noise is desired. If present must have 1 element for position.
    */
@@ -80,10 +83,13 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
    * @param gearing The gearing of the arm (numbers greater than 1 represent reductions).
    * @param jKgMetersSquared The moment of inertia of the arm; can be calculated from CAD software.
    * @param armLengthMeters The length of the arm.
-   * @param minAngleRads The minimum angle that the arm is capable of.
-   * @param maxAngleRads The maximum angle that the arm is capable of.
+   * @param minAngleRads The minimum angle that the arm is capable of, with 0 radians being
+   *     horizontal.
+   * @param maxAngleRads The maximum angle that the arm is capable of, with 0 radians being
+   *     horizontal.
    * @param simulateGravity Whether gravity should be simulated or not.
-   * @param startingAngleRads The initial position of the Arm simulation in radians.
+   * @param startingAngleRads The initial position of the Arm simulation in radians, with 0 radians
+   *     being horizontal.
    * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
    *     noise is desired. If present must have 1 element for position.
    */

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

@@ -812,6 +812,22 @@
       "xrp"
     ]
   },
+  {
+    "name": "XRP Timed",
+    "description": "An very basic timed robot program that can be used with the XRP reference robot design.",
+    "tags": [
+      "XRP",
+      "Differential Drive",
+      "XboxController"
+    ],
+    "foldername": "xrptimed",
+    "gradlebase": "javaxrp",
+    "mainclass": "Main",
+    "commandversion": 2,
+    "extravendordeps": [
+      "xrp"
+    ]
+  },
   {
     "name": "Digital Communication Sample",
     "description": "Communicates with external devices (such as an Arduino) using the roboRIO's DIO.",

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrptimed/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.xrptimed;
+
+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/xrptimed/Robot.java

@@ -0,0 +1,73 @@
+// 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.xrptimed;
+
+import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.xrp.XRPMotor;
+
+/**
+ * The methods in this class are called automatically corresponding to each mode, as described in
+ * the TimedRobot documentation. If you change the name of this class or the package after creating
+ * this project, you must also update the manifest file in the resource directory.
+ */
+public class Robot extends TimedRobot {
+  private final XRPMotor m_leftDrive = new XRPMotor(0);
+  private final XRPMotor m_rightDrive = new XRPMotor(1);
+  private final DifferentialDrive m_robotDrive =
+      new DifferentialDrive(m_leftDrive::set, m_rightDrive::set);
+  private final XboxController m_controller = new XboxController(0);
+  private final Timer m_timer = new Timer();
+
+  /** Called once at the beginning of the robot program. */
+  public Robot() {
+    SendableRegistry.addChild(m_robotDrive, m_leftDrive);
+    SendableRegistry.addChild(m_robotDrive, m_rightDrive);
+
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightDrive.setInverted(true);
+  }
+
+  /** This function is run once each time the robot enters autonomous mode. */
+  @Override
+  public void autonomousInit() {
+    m_timer.restart();
+  }
+
+  /** This function is called periodically during autonomous. */
+  @Override
+  public void autonomousPeriodic() {
+    // Drive for 2 seconds
+    if (m_timer.get() < 2.0) {
+      // Drive forwards half speed, make sure to turn input squaring off
+      m_robotDrive.arcadeDrive(0.5, 0.0, false);
+    } else {
+      m_robotDrive.stopMotor(); // stop robot
+    }
+  }
+
+  /** This function is called once each time the robot enters teleoperated mode. */
+  @Override
+  public void teleopInit() {}
+
+  /** This function is called periodically during teleoperated mode. */
+  @Override
+  public void teleopPeriodic() {
+    m_robotDrive.arcadeDrive(-m_controller.getLeftY(), -m_controller.getRightX());
+  }
+
+  /** This function is called once each time the robot enters test mode. */
+  @Override
+  public void testInit() {}
+
+  /** This function is called periodically during test mode. */
+  @Override
+  public void testPeriodic() {}
+}

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

@@ -8,8 +8,6 @@ import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.Nat;
-import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
@@ -40,8 +38,12 @@ import java.util.TreeMap;
  */
 public class PoseEstimator<T> {
   private final Odometry<T> m_odometry;
-  private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
-  private final Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
+
+  // Diagonal of process noise covariance matrix Q
+  private final double[] m_q = new double[] {0.0, 0.0, 0.0};
+
+  // Diagonal of Kalman gain matrix K
+  private final double[] m_vision_k = new double[] {0.0, 0.0, 0.0};
 
   private static final double kBufferDuration = 1.5;
   // Maps timestamps to odometry-only pose estimates
@@ -79,7 +81,7 @@ public class PoseEstimator<T> {
     m_poseEstimate = m_odometry.getPoseMeters();
 
     for (int i = 0; i < 3; ++i) {
-      m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
+      m_q[i] = stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0);
     }
     setVisionMeasurementStdDevs(visionMeasurementStdDevs);
     MathSharedStore.getMathShared().reportUsage(MathUsageId.kEstimator_PoseEstimator, 1);
@@ -95,6 +97,7 @@ public class PoseEstimator<T> {
    *     theta]ᵀ, with units in meters and radians.
    */
   public final void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
+    // Diagonal of measurement noise covariance matrix R
     var r = new double[3];
     for (int i = 0; i < 3; ++i) {
       r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
@@ -103,11 +106,10 @@ public class PoseEstimator<T> {
     // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
     // and C = I. See wpimath/algorithms.md.
     for (int row = 0; row < 3; ++row) {
-      if (m_q.get(row, 0) == 0.0) {
-        m_visionK.set(row, row, 0.0);
+      if (m_q[row] == 0.0) {
+        m_vision_k[row] = 0.0;
       } else {
-        m_visionK.set(
-            row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
+        m_vision_k[row] = m_q[row] / (m_q[row] + Math.sqrt(m_q[row] * r[row]));
       }
     }
   }
@@ -307,29 +309,23 @@ public class PoseEstimator<T> {
     var transform = visionRobotPoseMeters.minus(visionSample.get());
 
     // Step 5: We should not trust the transform entirely, so instead we scale this transform by a
-    // Kalman
-    // gain matrix representing how much we trust vision measurements compared to our current pose.
-    var k_times_transform =
-        m_visionK.times(
-            VecBuilder.fill(
-                transform.getX(), transform.getY(), transform.getRotation().getRadians()));
-
-    // Step 6: Convert back to Transform2d.
+    // Kalman gain matrix representing how much we trust vision measurements compared to our current
+    // pose. Then, we convert the result back to a Transform2d.
     var scaledTransform =
         new Transform2d(
-            k_times_transform.get(0, 0),
-            k_times_transform.get(1, 0),
-            Rotation2d.fromRadians(k_times_transform.get(2, 0)));
+            m_vision_k[0] * transform.getX(),
+            m_vision_k[1] * transform.getY(),
+            Rotation2d.fromRadians(m_vision_k[2] * transform.getRotation().getRadians()));
 
-    // Step 7: Calculate and record the vision update.
+    // Step 6: Calculate and record the vision update.
     var visionUpdate =
         new VisionUpdate(visionSample.get().plus(scaledTransform), odometrySample.get());
     m_visionUpdates.put(timestampSeconds, visionUpdate);
 
-    // Step 8: Remove later vision measurements. (Matches previous behavior)
+    // Step 7: Remove later vision measurements. (Matches previous behavior)
     m_visionUpdates.tailMap(timestampSeconds, false).entrySet().clear();
 
-    // Step 9: Update latest pose estimate. Since we cleared all updates after this vision update,
+    // Step 8: Update latest pose estimate. Since we cleared all updates after this vision update,
     // it's guaranteed to be the latest vision update.
     m_poseEstimate = visionUpdate.compensate(m_odometry.getPoseMeters());
   }

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

@@ -8,8 +8,6 @@ import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.Nat;
-import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -22,7 +20,6 @@ import edu.wpi.first.math.kinematics.Kinematics;
 import edu.wpi.first.math.kinematics.Odometry3d;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N4;
-import edu.wpi.first.math.numbers.N6;
 import java.util.NavigableMap;
 import java.util.Optional;
 import java.util.TreeMap;
@@ -48,8 +45,12 @@ import java.util.TreeMap;
  */
 public class PoseEstimator3d<T> {
   private final Odometry3d<T> m_odometry;
-  private final Matrix<N4, N1> m_q = new Matrix<>(Nat.N4(), Nat.N1());
-  private final Matrix<N6, N6> m_visionK = new Matrix<>(Nat.N6(), Nat.N6());
+
+  // Diagonal of process noise covariance matrix Q
+  private final double[] m_q = new double[] {0.0, 0.0, 0.0, 0.0};
+
+  // Diagonal of Kalman gain matrix K
+  private final double[] m_vision_k = new double[] {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
 
   private static final double kBufferDuration = 1.5;
   // Maps timestamps to odometry-only pose estimates
@@ -87,7 +88,7 @@ public class PoseEstimator3d<T> {
     m_poseEstimate = m_odometry.getPoseMeters();
 
     for (int i = 0; i < 4; ++i) {
-      m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
+      m_q[i] = stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0);
     }
     setVisionMeasurementStdDevs(visionMeasurementStdDevs);
     MathSharedStore.getMathShared().reportUsage(MathUsageId.kEstimator_PoseEstimator3d, 1);
@@ -103,6 +104,7 @@ public class PoseEstimator3d<T> {
    *     theta]ᵀ, with units in meters and radians.
    */
   public final void setVisionMeasurementStdDevs(Matrix<N4, N1> visionMeasurementStdDevs) {
+    // Diagonal of measurement covariance matrix R
     var r = new double[4];
     for (int i = 0; i < 4; ++i) {
       r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
@@ -111,17 +113,16 @@ public class PoseEstimator3d<T> {
     // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
     // and C = I. See wpimath/algorithms.md.
     for (int row = 0; row < 4; ++row) {
-      if (m_q.get(row, 0) == 0.0) {
-        m_visionK.set(row, row, 0.0);
+      if (m_q[row] == 0.0) {
+        m_vision_k[row] = 0.0;
       } else {
-        m_visionK.set(
-            row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
+        m_vision_k[row] = m_q[row] / (m_q[row] + Math.sqrt(m_q[row] * r[row]));
       }
     }
     // Fill in the gains for the other components of the rotation vector
-    double angle_gain = m_visionK.get(3, 3);
-    m_visionK.set(4, 4, angle_gain);
-    m_visionK.set(5, 5, angle_gain);
+    double angle_gain = m_vision_k[3];
+    m_vision_k[4] = angle_gain;
+    m_vision_k[5] = angle_gain;
   }
 
   /**
@@ -320,38 +321,27 @@ public class PoseEstimator3d<T> {
     var transform = visionRobotPoseMeters.minus(visionSample.get());
 
     // Step 5: We should not trust the transform entirely, so instead we scale this transform by a
-    // Kalman
-    // gain matrix representing how much we trust vision measurements compared to our current pose.
-    var k_times_transform =
-        m_visionK.times(
-            VecBuilder.fill(
-                transform.getX(),
-                transform.getY(),
-                transform.getZ(),
-                transform.getRotation().getX(),
-                transform.getRotation().getY(),
-                transform.getRotation().getZ()));
-
-    // Step 6: Convert back to Transform3d.
+    // Kalman gain matrix representing how much we trust vision measurements compared to our current
+    // pose. Then we convert the result back to a Transform3d.
     var scaledTransform =
         new Transform3d(
-            k_times_transform.get(0, 0),
-            k_times_transform.get(1, 0),
-            k_times_transform.get(2, 0),
+            m_vision_k[0] * transform.getX(),
+            m_vision_k[1] * transform.getY(),
+            m_vision_k[2] * transform.getZ(),
             new Rotation3d(
-                k_times_transform.get(3, 0),
-                k_times_transform.get(4, 0),
-                k_times_transform.get(5, 0)));
+                m_vision_k[3] * transform.getRotation().getX(),
+                m_vision_k[4] * transform.getRotation().getY(),
+                m_vision_k[5] * transform.getRotation().getZ()));
 
-    // Step 7: Calculate and record the vision update.
+    // Step 6: Calculate and record the vision update.
     var visionUpdate =
         new VisionUpdate(visionSample.get().plus(scaledTransform), odometrySample.get());
     m_visionUpdates.put(timestampSeconds, visionUpdate);
 
-    // Step 8: Remove later vision measurements. (Matches previous behavior)
+    // Step 7: Remove later vision measurements. (Matches previous behavior)
     m_visionUpdates.tailMap(timestampSeconds, false).entrySet().clear();
 
-    // Step 9: Update latest pose estimate. Since we cleared all updates after this vision update,
+    // Step 8: Update latest pose estimate. Since we cleared all updates after this vision update,
     // it's guaranteed to be the latest vision update.
     m_poseEstimate = visionUpdate.compensate(m_odometry.getPoseMeters());
   }

wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java

@@ -14,8 +14,8 @@ import edu.wpi.first.math.MathUtil;
  * edu.wpi.first.math.trajectory.TrapezoidProfile} instead.
  */
 public class SlewRateLimiter {
-  private final double m_positiveRateLimit;
-  private final double m_negativeRateLimit;
+  private double m_positiveRateLimit;
+  private double m_negativeRateLimit;
   private double m_prevVal;
   private double m_prevTime;
 
@@ -82,4 +82,29 @@ public class SlewRateLimiter {
     m_prevVal = value;
     m_prevTime = MathSharedStore.getTimestamp();
   }
+
+  /**
+   * Sets the rate-of-change limit to the given positive and negative rate limits.
+   *
+   * @param positiveRateLimit The rate-of-change limit in the positive direction, in units per
+   *     second. This is expected to be positive.
+   * @param negativeRateLimit The rate-of-change limit in the negative direction, in units per
+   *     second. This is expected to be negative.
+   */
+  public void setLimit(double positiveRateLimit, double negativeRateLimit) {
+    m_positiveRateLimit = positiveRateLimit;
+    m_negativeRateLimit = negativeRateLimit;
+  }
+
+  /**
+   * Sets the rate-of-change limit to the given positive rate limit and negative rate limit of
+   * -rateLimit.
+   *
+   * @param rateLimit The rate-of-change limit in both directions, in units per second. This is
+   *     expected to be positive.
+   */
+  public void setLimit(double rateLimit) {
+    m_positiveRateLimit = rateLimit;
+    m_negativeRateLimit = -rateLimit;
+  }
 }

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

@@ -86,6 +86,7 @@ class WPILIB_DLLEXPORT PoseEstimator {
    */
   void SetVisionMeasurementStdDevs(
       const wpi::array<double, 3>& visionMeasurementStdDevs) {
+    // Diagonal of measurement covariance matrix R
     wpi::array<double, 3> r{wpi::empty_array};
     for (size_t i = 0; i < 3; ++i) {
       r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
@@ -95,9 +96,9 @@ class WPILIB_DLLEXPORT PoseEstimator {
     // and C = I. See wpimath/algorithms.md.
     for (size_t row = 0; row < 3; ++row) {
       if (m_q[row] == 0.0) {
-        m_visionK(row, row) = 0.0;
+        m_vision_K.diagonal()[row] = 0.0;
       } else {
-        m_visionK(row, row) =
+        m_vision_K.diagonal()[row] =
             m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
       }
     }
@@ -318,9 +319,9 @@ class WPILIB_DLLEXPORT PoseEstimator {
     // this transform by a Kalman gain matrix representing how much we trust
     // vision measurements compared to our current pose.
     Eigen::Vector3d k_times_transform =
-        m_visionK * Eigen::Vector3d{transform.X().value(),
-                                    transform.Y().value(),
-                                    transform.Rotation().Radians().value()};
+        m_vision_K * Eigen::Vector3d{transform.X().value(),
+                                     transform.Y().value(),
+                                     transform.Rotation().Radians().value()};
 
     // Step 6: Convert back to Transform2d.
     Transform2d scaledTransform{
@@ -475,8 +476,13 @@ class WPILIB_DLLEXPORT PoseEstimator {
   static constexpr units::second_t kBufferDuration = 1.5_s;
 
   Odometry<WheelSpeeds, WheelPositions>& m_odometry;
+
+  // Diagonal of process noise covariance matrix Q
   wpi::array<double, 3> m_q{wpi::empty_array};
-  Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
+
+  // Kalman gain matrix K
+  Eigen::DiagonalMatrix<double, 3> m_vision_K =
+      Eigen::DiagonalMatrix<double, 3>::Zero();
 
   // Maps timestamps to odometry-only pose estimates
   TimeInterpolatableBuffer<Pose2d> m_odometryPoseBuffer{kBufferDuration};

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

@@ -92,6 +92,7 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
    */
   void SetVisionMeasurementStdDevs(
       const wpi::array<double, 4>& visionMeasurementStdDevs) {
+    // Diagonal of measurement noise covariance matrix R
     wpi::array<double, 4> r{wpi::empty_array};
     for (size_t i = 0; i < 4; ++i) {
       r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
@@ -101,15 +102,15 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
     // and C = I. See wpimath/algorithms.md.
     for (size_t row = 0; row < 4; ++row) {
       if (m_q[row] == 0.0) {
-        m_visionK(row, row) = 0.0;
+        m_vision_K.diagonal()[row] = 0.0;
       } else {
-        m_visionK(row, row) =
+        m_vision_K.diagonal()[row] =
             m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
       }
     }
-    double angle_gain = m_visionK(3, 3);
-    m_visionK(4, 4) = angle_gain;
-    m_visionK(5, 5) = angle_gain;
+    double angle_gain = m_vision_K.diagonal()[3];
+    m_vision_K.diagonal()[4] = angle_gain;
+    m_vision_K.diagonal()[5] = angle_gain;
   }
 
   /**
@@ -327,12 +328,12 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
     // this transform by a Kalman gain matrix representing how much we trust
     // vision measurements compared to our current pose.
     frc::Vectord<6> k_times_transform =
-        m_visionK * frc::Vectord<6>{transform.X().value(),
-                                    transform.Y().value(),
-                                    transform.Z().value(),
-                                    transform.Rotation().X().value(),
-                                    transform.Rotation().Y().value(),
-                                    transform.Rotation().Z().value()};
+        m_vision_K * frc::Vectord<6>{transform.X().value(),
+                                     transform.Y().value(),
+                                     transform.Z().value(),
+                                     transform.Rotation().X().value(),
+                                     transform.Rotation().Y().value(),
+                                     transform.Rotation().Z().value()};
 
     // Step 6: Convert back to Transform3d.
     Transform3d scaledTransform{
@@ -490,8 +491,13 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
   static constexpr units::second_t kBufferDuration = 1.5_s;
 
   Odometry3d<WheelSpeeds, WheelPositions>& m_odometry;
+
+  // Diagonal of process noise covariance matrix Q
   wpi::array<double, 4> m_q{wpi::empty_array};
-  frc::Matrixd<6, 6> m_visionK = frc::Matrixd<6, 6>::Zero();
+
+  // Kalman gain matrix K
+  Eigen::DiagonalMatrix<double, 6> m_vision_K =
+      Eigen::DiagonalMatrix<double, 6>::Zero();
 
   // Maps timestamps to odometry-only pose estimates
   TimeInterpolatableBuffer<Pose3d> m_odometryPoseBuffer{kBufferDuration};

wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h

@@ -92,6 +92,32 @@ class SlewRateLimiter {
     m_prevTime = wpi::math::MathSharedStore::GetTimestamp();
   }
 
+  /**
+   * Sets the rate-of-change limit to the given positive and negative rate
+   * limits.
+   *
+   * @param positiveRateLimit The rate-of-change limit in the positive
+   * direction, in units per second. This is expected to be positive.
+   * @param negativeRateLimit The rate-of-change limit in the negative
+   * direction, in units per second. This is expected to be negative.
+   */
+  void SetLimit(Rate_t positiveRateLimit, Rate_t negativeRateLimit) {
+    m_positiveRateLimit = positiveRateLimit;
+    m_negativeRateLimit = negativeRateLimit;
+  }
+
+  /**
+   * Sets the rate-of-change limit to the given positive rate limit and negative
+   * rate limit of -rateLimit.
+   *
+   * @param rateLimit The rate-of-change limit in both directions, in units per
+   * second. This is expected to be positive.
+   */
+  void SetLimit(Rate_t rateLimit) {
+    m_positiveRateLimit = rateLimit;
+    m_negativeRateLimit = -rateLimit;
+  }
+
  private:
   Rate_t m_positiveRateLimit;
   Rate_t m_negativeRateLimit;

wpiunits/src/main/java/edu/wpi/first/units/MutableMeasure.java

@@ -73,7 +73,7 @@ public interface MutableMeasure<
    * @return the measure
    */
   default MutSelf mut_acc(double raw) {
-    return mut_setBaseUnitMagnitude(magnitude() + raw);
+    return mut_setMagnitude(magnitude() + raw);
   }
 
   /**
@@ -107,7 +107,7 @@ public interface MutableMeasure<
    * @return this measure
    */
   default MutSelf mut_plus(double magnitude, U otherUnit) {
-    return mut_setBaseUnitMagnitude(magnitude() + otherUnit.toBaseUnits(magnitude));
+    return mut_setBaseUnitMagnitude(baseUnitMagnitude() + otherUnit.toBaseUnits(magnitude));
   }
 
   /**

wpiunits/src/main/java/edu/wpi/first/units/Units.java

@@ -166,7 +166,13 @@ public final class Units {
    * A unit of angular velocity equivalent to spinning at a rate of one {@link #Rotations Rotation}
    * per {@link #Minute}. Motor spec sheets often list maximum speeds in terms of RPM.
    */
-  public static final AngularVelocityUnit RPM = Rotations.per(Minute);
+  public static final AngularVelocityUnit RotationsPerMinute = Rotations.per(Minute);
+
+  /**
+   * A unit of angular velocity equivalent to spinning at a rate of one {@link #Rotations Rotation}
+   * per {@link #Minute}. Motor spec sheets often list maximum speeds in terms of RPM.
+   */
+  public static final AngularVelocityUnit RPM = RotationsPerMinute; // alias
 
   /**
    * The standard SI unit of angular velocity, equivalent to spinning at a rate of one {@link

wpiunits/src/test/java/edu/wpi/first/units/MutableMeasureTest.java

@@ -0,0 +1,168 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.units;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertSame;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.units.measure.MutDistance;
+import org.junit.jupiter.api.Test;
+
+class MutableMeasureTest {
+  @Test
+  void testBasics() {
+    DistanceUnit unit = Units.Feet;
+    double magnitude = 10;
+    MutDistance m = unit.mutable(magnitude);
+    assertEquals(unit, m.unit(), "Wrong units");
+    assertEquals(magnitude, m.magnitude(), 0, "Wrong magnitude");
+  }
+
+  @Test
+  void testMultiply() {
+    MutDistance m = Units.Feet.mutable(1);
+    MutDistance m2 = m.mut_times(Units.Value.of(10));
+    assertEquals(10, m.in(Units.Feet), 1e-12);
+    assertSame(m, m2);
+  }
+
+  @Test
+  void testMultiplyScalar() {
+    MutDistance m = Units.Feet.mutable(1);
+    MutDistance m2 = m.mut_times(10);
+    assertEquals(10, m.in(Units.Feet), 1e-12);
+    assertSame(m, m2);
+  }
+
+  @Test
+  void testDivide() {
+    MutDistance m = Units.Meters.mutable(1);
+    MutDistance m2 = m.mut_divide(Units.Value.of(10));
+    assertEquals(0.1, m.magnitude(), 0);
+    assertSame(m, m2);
+  }
+
+  @Test
+  void testDivideScalar() {
+    MutDistance m = Units.Meters.mutable(1);
+    MutDistance m2 = m.mut_divide(10);
+    assertEquals(0.1, m.magnitude(), 0);
+    assertSame(m, m2);
+  }
+
+  @Test
+  void testAdd() {
+    MutDistance m1 = Units.Feet.mutable(1);
+    MutDistance m2 = Units.Inches.mutable(2);
+
+    Distance sum1 = m1.mut_plus(Units.Inches.of(2));
+    assertTrue(sum1.isEquivalent(Units.Feet.of(1 + 2 / 12d)));
+    assertSame(m1, sum1);
+
+    Distance sum2 = m2.mut_plus(Units.Feet.of(1));
+    assertTrue(sum2.isEquivalent(Units.Inches.of(14)));
+    assertSame(m2, sum2);
+  }
+
+  @Test
+  void testAddScalar() {
+    MutDistance m1 = Units.Feet.mutable(1);
+    MutDistance m2 = Units.Inches.mutable(2);
+
+    Distance sum1 = m1.mut_plus(2, Units.Inches);
+    assertTrue(sum1.isEquivalent(Units.Feet.of(1 + 2 / 12d)));
+    assertSame(m1, sum1);
+
+    Distance sum2 = m2.mut_plus(1, Units.Feet);
+    assertTrue(sum2.isEquivalent(Units.Inches.of(14)));
+    assertSame(m2, sum2);
+  }
+
+  @Test
+  void testAcc() {
+    MutDistance m1 = Units.Feet.mutable(1);
+    MutDistance m2 = Units.Inches.mutable(2);
+
+    Distance acc1 = m1.mut_acc(Units.Inches.of(2));
+    assertTrue(acc1.isEquivalent(Units.Feet.of(1 + 2 / 12d)));
+    assertSame(m1, acc1);
+
+    Distance acc2 = m2.mut_acc(Units.Feet.of(1));
+    assertTrue(acc2.isEquivalent(Units.Inches.of(14)));
+    assertSame(m2, acc2);
+  }
+
+  @Test
+  void testAccScalar() {
+    MutDistance m1 = Units.Feet.mutable(1);
+    MutDistance m2 = Units.Inches.mutable(2);
+
+    Distance acc1 = m1.mut_acc(2 / 12d);
+    assertTrue(acc1.isEquivalent(Units.Feet.of(1 + 2 / 12d)));
+    assertSame(m1, acc1);
+
+    Distance acc2 = m2.mut_acc(12);
+    assertTrue(acc2.isEquivalent(Units.Inches.of(14)));
+    assertSame(m2, acc2);
+  }
+
+  @Test
+  void testSubtract() {
+    MutDistance m1 = Units.Feet.mutable(1);
+    MutDistance m2 = Units.Inches.mutable(2);
+
+    Distance sub1 = m1.mut_minus(Units.Inches.of(2));
+    assertTrue(sub1.isEquivalent(Units.Feet.of(1 - 2 / 12d)));
+    assertSame(m1, sub1);
+
+    Distance sub2 = m2.mut_minus(Units.Feet.of(1));
+    assertTrue(sub2.isEquivalent(Units.Inches.of(-10)));
+    assertSame(m2, sub2);
+  }
+
+  @Test
+  void testSubtractScalar() {
+    MutDistance m1 = Units.Feet.mutable(1);
+    MutDistance m2 = Units.Inches.mutable(2);
+
+    Distance sub1 = m1.mut_minus(2, Units.Inches);
+    assertTrue(sub1.isEquivalent(Units.Feet.of(1 - 2 / 12d)));
+    assertSame(m1, sub1);
+
+    Distance sub2 = m2.mut_minus(1, Units.Feet);
+    assertTrue(sub2.isEquivalent(Units.Inches.of(-10)));
+    assertSame(m2, sub2);
+  }
+
+  @Test
+  void testReplace() {
+    MutDistance m1 = Units.Feet.mutable(1);
+    MutDistance m2 = Units.Inches.mutable(2);
+
+    Distance replace1 = m1.mut_replace(Units.Inches.of(2));
+    assertTrue(replace1.isEquivalent(Units.Inches.of(2)));
+    assertSame(m1, replace1);
+
+    Distance replace2 = m2.mut_replace(Units.Feet.of(1));
+    assertTrue(replace2.isEquivalent(Units.Feet.of(1)));
+    assertSame(m2, replace2);
+  }
+
+  @Test
+  void testReplaceScalar() {
+    MutDistance m1 = Units.Feet.mutable(1);
+    MutDistance m2 = Units.Inches.mutable(2);
+
+    Distance replace1 = m1.mut_replace(2, Units.Inches);
+    assertTrue(replace1.isEquivalent(Units.Inches.of(2)));
+    assertSame(m1, replace1);
+
+    Distance replace2 = m2.mut_replace(1, Units.Feet);
+    assertTrue(replace2.isEquivalent(Units.Feet.of(1)));
+    assertSame(m2, replace2);
+  }
+}