[sysid] Fix wrong position Kd with unnormalized time (#6433)

Fix typo.

.gitattributes

@@ -1,7 +1,7 @@
-*.cpp text
+*.cpp text eol=lf
 *.gradle text eol=lf
-*.h text
-*.inc text
+*.h text eol=lf
+*.inc text eol=lf
 *.java text eol=lf
 *.json text eol=lf
 *.md text eol=lf

.github/workflows/cmake.yml

@@ -40,7 +40,7 @@ jobs:
 
       - name: Set up Python 3.8 (macOS)
         if: runner.os == 'macOS'
-        uses: actions/setup-python@v4
+        uses: actions/setup-python@v5
         with:
           python-version: 3.8
 
@@ -88,28 +88,13 @@ jobs:
         uses: lukka/run-vcpkg@v11.1
         with:
           vcpkgDirectory: ${{ runner.workspace }}/vcpkg
-          vcpkgGitCommitId: 78b61582c9e093fda56a01ebb654be15a0033897 # HEAD on 2023-08-6
+          vcpkgGitCommitId: 37c3e63a1306562f7f59c4c3c8892ddd50fdf992 # HEAD on 2024-02-24
 
       - name: configure
-        run: cmake -S . -B build -G "Ninja" -DCMAKE_C_COMPILER_LAUNCHER=sccache -DCMAKE_CXX_COMPILER_LAUNCHER=sccache -DCMAKE_BUILD_TYPE=Release -DWITH_JAVA=OFF -DWITH_EXAMPLES=ON -DUSE_SYSTEM_FMTLIB=ON -DUSE_SYSTEM_LIBUV=ON -DUSE_SYSTEM_EIGEN=ON -DCMAKE_TOOLCHAIN_FILE=${{ runner.workspace }}/vcpkg/scripts/buildsystems/vcpkg.cmake -DVCPKG_INSTALL_OPTIONS=--clean-after-build -DVCPKG_TARGET_TRIPLET=x64-windows-release -DVCPKG_HOST_TRIPLET=x64-windows-release
+        run: cmake -S . -B build -G "Ninja" -DCMAKE_C_COMPILER_LAUNCHER=sccache -DCMAKE_CXX_COMPILER_LAUNCHER=sccache -DCMAKE_BUILD_TYPE=Release -DWITH_JAVA=OFF -DWITH_EXAMPLES=ON -DUSE_SYSTEM_FMTLIB=ON -DUSE_SYSTEM_LIBUV=ON -DUSE_SYSTEM_EIGEN=OFF -DCMAKE_TOOLCHAIN_FILE=${{ runner.workspace }}/vcpkg/scripts/buildsystems/vcpkg.cmake -DVCPKG_INSTALL_OPTIONS=--clean-after-build -DVCPKG_TARGET_TRIPLET=x64-windows-release -DVCPKG_HOST_TRIPLET=x64-windows-release
         env:
           SCCACHE_GHA_ENABLED: "true"
 
-      # Build wpiutil at full speed, wpimath depends on wpiutil
-      - name: build wpiutil
-        working-directory: build
-        run: cmake --build . --parallel $(nproc) --target wpiutil/all
-        env:
-          SCCACHE_GHA_ENABLED: "true"
-
-      # Build wpimath slow to prevent OOM
-      - name: build wpimath
-        working-directory: build
-        run: cmake --build . --parallel 1 --target wpimath/all
-        env:
-          SCCACHE_GHA_ENABLED: "true"
-
-      # Build everything else fast
       - name: build
         working-directory: build
         run: cmake --build . --parallel $(nproc)

.github/workflows/comment-command.yml

@@ -9,7 +9,7 @@ jobs:
     runs-on: ubuntu-22.04
     steps:
       - name: React Rocket
-        uses: actions/github-script@v6
+        uses: actions/github-script@v7
         with:
           script: |
             const {owner, repo} = context.issue
@@ -34,11 +34,11 @@ jobs:
           GITHUB_TOKEN: "${{ secrets.COMMENT_COMMAND_PAT_TOKEN }}"
           NUMBER: ${{ github.event.issue.number }}
       - name: Set up Python 3.8
-        uses: actions/setup-python@v4
+        uses: actions/setup-python@v5
         with:
           python-version: 3.8
       - name: Setup Java
-        uses: actions/setup-java@v3
+        uses: actions/setup-java@v4
         with:
           distribution: 'zulu'
           java-version: 17

.github/workflows/documentation.yml

@@ -20,7 +20,7 @@ jobs:
         with:
           fetch-depth: 0
           persist-credentials: false
-      - uses: actions/setup-java@v3
+      - uses: actions/setup-java@v4
         with:
           distribution: 'zulu'
           java-version: 13

.github/workflows/gradle.yml

@@ -112,7 +112,7 @@ jobs:
       - uses: actions/checkout@v4
         with:
           fetch-depth: 0
-      - uses: actions/setup-java@v3
+      - uses: actions/setup-java@v4
         with:
           distribution: 'zulu'
           java-version: 17
@@ -171,7 +171,7 @@ jobs:
       - uses: actions/checkout@v4
         with:
           fetch-depth: 0
-      - uses: actions/setup-java@v3
+      - uses: actions/setup-java@v4
         with:
           distribution: 'zulu'
           java-version: 17
@@ -230,7 +230,7 @@ jobs:
         run: |
           cat combiner/products/build/allOutputs/version.txt
           test -s combiner/products/build/allOutputs/version.txt
-      - uses: actions/setup-java@v3
+      - uses: actions/setup-java@v4
         if: |
           github.repository_owner == 'wpilibsuite' &&
           (github.ref == 'refs/heads/main' || startsWith(github.ref, 'refs/tags/v'))

.github/workflows/lint-format.yml

@@ -23,11 +23,11 @@ jobs:
           git checkout -b pr
           git branch -f main origin/main
       - name: Set up Python 3.8
-        uses: actions/setup-python@v4
+        uses: actions/setup-python@v5
         with:
           python-version: 3.8
       - name: Install wpiformat
-        run: pip3 install wpiformat==2023.36
+        run: pip3 install wpiformat==2024.32
       - name: Run
         run: wpiformat
       - name: Check output
@@ -62,7 +62,7 @@ jobs:
           git checkout -b pr
           git branch -f main origin/main
       - name: Set up Python 3.8
-        uses: actions/setup-python@v4
+        uses: actions/setup-python@v5
         with:
           python-version: 3.8
       - name: Install wpiformat
@@ -105,7 +105,7 @@ jobs:
       - uses: actions/checkout@v4
         with:
           fetch-depth: 0
-      - uses: actions/setup-java@v3
+      - uses: actions/setup-java@v4
         with:
           distribution: 'zulu'
           java-version: 17

.github/workflows/pregenerate.yml

@@ -19,7 +19,7 @@ jobs:
         with:
           fetch-depth: 0
       - name: Set up Python 3.9
-        uses: actions/setup-python@v4
+        uses: actions/setup-python@v5
         with:
           python-version: 3.9
       - name: Install jinja

.github/workflows/upstream-utils.yml

@@ -23,7 +23,7 @@ jobs:
           git checkout -b pr
           git branch -f main origin/main
       - name: Set up Python 3.9
-        uses: actions/setup-python@v4
+        uses: actions/setup-python@v5
         with:
           python-version: 3.9
       - name: Configure committer identity

CONTRIBUTING.md

@@ -12,11 +12,11 @@ So you want to contribute your changes back to WPILib. Great! We have a few cont
 
 ## General Contribution Rules
 
-- Everything in the library must work for the 3000+ teams that will be using it.
+- Everything in the library must work for the 4000+ teams that will be using it.
 - We need to be able to maintain submitted changes, even if you are no longer working on the project.
 - Tool suite changes must be generally useful to a broad range of teams
 - Excluding bug fixes, changes in one language generally need to have corresponding changes in other languages.
-    - Some features, such the addition of C++11 for WPILibC or Functional Interfaces for WPILibJ, are specific to that version of WPILib only.
+    - Some features, such the addition of C++23 for WPILibC or Functional Interfaces for WPILibJ, are specific to that version of WPILib only. New language features added to C++ must be wrappable in Python for [RobotPy](https://github.com/robotpy).
     - Substantial changes often need to have corresponding LabVIEW changes. To do this, we will work with NI on these large changes.
 - Changes should have tests.
 - Code should be well documented.
@@ -27,7 +27,8 @@ So you want to contribute your changes back to WPILib. Great! We have a few cont
 - Bug reports and fixes
     - We will generally accept bug fixes without too much question. If they are only implemented for one language, we will implement them for any other necessary languages. Bug reports are also welcome, please submit them to our GitHub issue tracker.
 - While we do welcome improvements to the API, there are a few important rules to consider:
-    - Features must be added to both WPILibC and WPILibJ, with rare exceptions.
+    - Features must be added to Java (WPILibJ), C++ (WPILibC), with rare exceptions.
+        - Most of Python (RobotPy) is created by wrapping WPILibC with pybind11 via robotpy-build. However, new features to the command framework should also be submitted to [robotpy-commands-v2](https://github.com/robotpy/robotpy-commands-v2) as the command framework is reimplemented in Python.
     - During competition season, we will not merge any new feature additions. We want to ensure that the API is stable during the season to help minimize issues for teams.
     - Ask about large changes before spending a bunch of time on them! You can create a new issue on our GitHub tracker for feature request/discussion and talk about it with us there.
     - Features that make it easier for teams with less experience to be more successful are more likely to be accepted.

apriltag/convert_apriltag_layouts.py

@@ -82,6 +82,7 @@ def main():
         # Write JSON
         with open(filename.replace(".csv", ".json"), "w") as f:
             json.dump(json_data, f, indent=2)
+            f.write("\n")
 
 
 if __name__ == "__main__":

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

@@ -16,6 +16,7 @@ import edu.wpi.first.math.geometry.Translation3d;
 import java.io.IOException;
 import java.io.InputStream;
 import java.io.InputStreamReader;
+import java.io.UncheckedIOException;
 import java.nio.file.Path;
 import java.util.ArrayList;
 import java.util.HashMap;
@@ -221,6 +222,22 @@ public class AprilTagFieldLayout {
     new ObjectMapper().writeValue(path.toFile(), this);
   }
 
+  /**
+   * Get an official {@link AprilTagFieldLayout}.
+   *
+   * @param field The loadable AprilTag field layout.
+   * @return AprilTagFieldLayout of the field.
+   * @throws UncheckedIOException If the layout does not exist.
+   */
+  public static AprilTagFieldLayout loadField(AprilTagFields field) {
+    try {
+      return loadFromResource(field.m_resourceFile);
+    } catch (IOException e) {
+      throw new UncheckedIOException(
+          "Could not load AprilTagFieldLayout from " + field.m_resourceFile, e);
+    }
+  }
+
   /**
    * Deserializes a field layout from a resource within a internal jar file.
    *

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

@@ -4,7 +4,6 @@
 
 package edu.wpi.first.apriltag;
 
-import java.io.IOException;
 import java.io.UncheckedIOException;
 
 /** Loadable AprilTag field layouts. */
@@ -36,11 +35,6 @@ public enum AprilTagFields {
    * @throws UncheckedIOException If the layout does not exist
    */
   public AprilTagFieldLayout loadAprilTagLayoutField() {
-    try {
-      return AprilTagFieldLayout.loadFromResource(m_resourceFile);
-    } catch (IOException e) {
-      throw new UncheckedIOException(
-          "Could not load AprilTagFieldLayout from " + m_resourceFile, e);
-    }
+    return AprilTagFieldLayout.loadField(this);
   }
 }

apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp

@@ -125,3 +125,37 @@ void frc::from_json(const wpi::json& json, AprilTagFieldLayout& layout) {
   layout.m_fieldWidth =
       units::meter_t{json.at("field").at("width").get<double>()};
 }
+
+// Use namespace declaration for forward declaration
+namespace frc {
+
+// C++ generated from resource files
+std::string_view GetResource_2022_rapidreact_json();
+std::string_view GetResource_2023_chargedup_json();
+std::string_view GetResource_2024_crescendo_json();
+
+}  // namespace frc
+
+AprilTagFieldLayout AprilTagFieldLayout::LoadField(AprilTagField field) {
+  std::string_view fieldString;
+  switch (field) {
+    case AprilTagField::k2022RapidReact:
+      fieldString = GetResource_2022_rapidreact_json();
+      break;
+    case AprilTagField::k2023ChargedUp:
+      fieldString = GetResource_2023_chargedup_json();
+      break;
+    case AprilTagField::k2024Crescendo:
+      fieldString = GetResource_2024_crescendo_json();
+      break;
+    case AprilTagField::kNumFields:
+      throw std::invalid_argument("Invalid Field");
+  }
+
+  wpi::json json = wpi::json::parse(fieldString);
+  return json.get<AprilTagFieldLayout>();
+}
+
+AprilTagFieldLayout frc::LoadAprilTagLayoutField(AprilTagField field) {
+  return AprilTagFieldLayout::LoadField(field);
+}

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

@@ -1,36 +0,0 @@
-// 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/apriltag/AprilTagFields.h"
-
-#include <wpi/json.h>
-
-namespace frc {
-
-// C++ generated from resource files
-std::string_view GetResource_2022_rapidreact_json();
-std::string_view GetResource_2023_chargedup_json();
-std::string_view GetResource_2024_crescendo_json();
-
-AprilTagFieldLayout LoadAprilTagLayoutField(AprilTagField field) {
-  std::string_view fieldString;
-  switch (field) {
-    case AprilTagField::k2022RapidReact:
-      fieldString = GetResource_2022_rapidreact_json();
-      break;
-    case AprilTagField::k2023ChargedUp:
-      fieldString = GetResource_2023_chargedup_json();
-      break;
-    case AprilTagField::k2024Crescendo:
-      fieldString = GetResource_2024_crescendo_json();
-      break;
-    case AprilTagField::kNumFields:
-      throw std::invalid_argument("Invalid Field");
-  }
-
-  wpi::json json = wpi::json::parse(fieldString);
-  return json.get<AprilTagFieldLayout>();
-}
-
-}  // namespace frc

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

@@ -14,6 +14,7 @@
 #include <wpi/json_fwd.h>
 
 #include "frc/apriltag/AprilTag.h"
+#include "frc/apriltag/AprilTagFields.h"
 #include "frc/geometry/Pose3d.h"
 
 namespace frc {
@@ -48,6 +49,14 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout {
     kRedAllianceWallRightSide,
   };
 
+  /**
+   * Loads an AprilTagFieldLayout from a predefined field
+   *
+   * @param field The predefined field
+   * @return AprilTagFieldLayout of the field
+   */
+  static AprilTagFieldLayout LoadField(AprilTagField field);
+
   AprilTagFieldLayout() = default;
 
   /**
@@ -152,4 +161,13 @@ void to_json(wpi::json& json, const AprilTagFieldLayout& layout);
 WPILIB_DLLEXPORT
 void from_json(const wpi::json& json, AprilTagFieldLayout& layout);
 
+/**
+ * Loads an AprilTagFieldLayout from a predefined field
+ *
+ * @param field The predefined field
+ * @return AprilTagFieldLayout of the field
+ */
+WPILIB_DLLEXPORT AprilTagFieldLayout
+LoadAprilTagLayoutField(AprilTagField field);
+
 }  // namespace frc

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

@@ -8,8 +8,6 @@
 
 #include <wpi/SymbolExports.h>
 
-#include "frc/apriltag/AprilTagFieldLayout.h"
-
 namespace frc {
 
 /**
@@ -28,12 +26,4 @@ enum class AprilTagField {
   kNumFields,
 };
 
-/**
- * Loads an AprilTagFieldLayout from a predefined field
- *
- * @param field The predefined field
- */
-WPILIB_DLLEXPORT AprilTagFieldLayout
-LoadAprilTagLayoutField(AprilTagField field);
-
 }  // namespace frc

apriltag/src/main/native/resources/edu/wpi/first/apriltag/2024-crescendo.json

@@ -290,7 +290,7 @@
     }
   ],
   "field": {
-    "length": 16.451,
+    "length": 16.541,
     "width": 8.211
   }
 }

apriltag/src/test/java/edu/wpi/first/apriltag/LoadConfigTest.java

@@ -22,13 +22,14 @@ class LoadConfigTest {
   @ParameterizedTest
   @EnumSource(AprilTagFields.class)
   void testLoad(AprilTagFields field) {
-    AprilTagFieldLayout layout = Assertions.assertDoesNotThrow(field::loadAprilTagLayoutField);
+    AprilTagFieldLayout layout =
+        Assertions.assertDoesNotThrow(() -> AprilTagFieldLayout.loadField(field));
     assertNotNull(layout);
   }
 
   @Test
   void test2022RapidReact() {
-    AprilTagFieldLayout layout = AprilTagFields.k2022RapidReact.loadAprilTagLayoutField();
+    AprilTagFieldLayout layout = AprilTagFieldLayout.loadField(AprilTagFields.k2022RapidReact);
 
     // Blue Hangar Truss - Hub
     Pose3d expectedPose =

apriltag/src/test/native/cpp/LoadConfigTest.cpp

@@ -4,6 +4,7 @@
 
 #include <gtest/gtest.h>
 
+#include "frc/apriltag/AprilTagFieldLayout.h"
 #include "frc/apriltag/AprilTagFields.h"
 
 namespace frc {
@@ -20,7 +21,7 @@ std::vector<AprilTagField> GetAllFields() {
 
 TEST(AprilTagFieldsTest, TestLoad2022RapidReact) {
   AprilTagFieldLayout layout =
-      LoadAprilTagLayoutField(AprilTagField::k2022RapidReact);
+      AprilTagFieldLayout::LoadField(AprilTagField::k2022RapidReact);
 
   // Blue Hangar Truss - Hub
   auto expectedPose =
@@ -53,7 +54,7 @@ class AllFieldsFixtureTest : public ::testing::TestWithParam<AprilTagField> {};
 
 TEST_P(AllFieldsFixtureTest, CheckEntireEnum) {
   AprilTagField field = GetParam();
-  EXPECT_NO_THROW(LoadAprilTagLayoutField(field));
+  EXPECT_NO_THROW(AprilTagFieldLayout::LoadField(field));
 }
 
 INSTANTIATE_TEST_SUITE_P(ValuesEnumTestInstTests, AllFieldsFixtureTest,

datalogtool/publish.gradle

@@ -29,51 +29,8 @@ model {
                         def applicationPath = binary.executable.file
                         def icon = file("$project.projectDir/src/main/native/mac/datalogtool.icns")
 
-                        // Create the macOS bundle.
-                        def bundleTask = project.tasks.create("bundleDataLogToolOsxApp" + binary.targetPlatform.architecture.name, Copy) {
-                            description("Creates a macOS application bundle for DataLogTool")
-                            from(file("$project.projectDir/Info.plist"))
-                            into(file("$project.buildDir/outputs/bundles/$binary.targetPlatform.architecture.name/DataLogTool.app/Contents"))
-                            into("MacOS") {
-                                with copySpec {
-                                    from binary.executable.file
-                                }
-                            }
-                            into("Resources") {
-                                with copySpec {
-                                    from icon
-                                }
-                            }
-
-                            inputs.property "HasDeveloperId", project.hasProperty("developerID")
-
-                            doLast {
-                                if (project.hasProperty("developerID")) {
-                                    // Get path to binary.
-                                    exec {
-                                        workingDir rootDir
-                                        def args = [
-                                            "sh",
-                                            "-c",
-                                            "codesign --force --strict --deep " +
-                                            "--timestamp --options=runtime " +
-                                            "--verbose -s ${project.findProperty("developerID")} " +
-                                            "$project.buildDir/outputs/bundles/$binary.targetPlatform.architecture.name/DataLogTool.app/"
-                                        ]
-                                        commandLine args
-                                    }
-                                }
-                            }
-                        }
-
-                        // Reset the application path if we are creating a bundle.
-                        if (binary.targetPlatform.operatingSystem.isMacOsX()) {
-                            applicationPath = file("$project.buildDir/outputs/bundles/$binary.targetPlatform.architecture.name")
-                            project.build.dependsOn bundleTask
-                        }
-
                         // Create the ZIP.
-                        def task = project.tasks.create("copyDataLogToolExecutable" + binary.targetPlatform.architecture.name, Zip) {
+                        def task = project.tasks.create("copyDataLogToolExecutable" + binary.targetPlatform.operatingSystem.name + binary.targetPlatform.architecture.name, Zip) {
                             description("Copies the DataLogTool executable to the outputs directory.")
                             destinationDirectory = outputsFolder
 
@@ -85,13 +42,63 @@ model {
                                 into '/'
                             }
 
-                            from(applicationPath)
+                            if (binary.targetPlatform.operatingSystem.isWindows()) {
+                                def exePath = binary.executable.file.absolutePath
+                                exePath = exePath.substring(0, exePath.length() - 4)
+                                def pdbPath = new File(exePath + '.pdb')
+                                from(pdbPath)
+                            }
+
                             into(nativeUtils.getPlatformPath(binary))
                         }
 
                         if (binary.targetPlatform.operatingSystem.isMacOsX()) {
+                            // Create the macOS bundle.
+                            def bundleTask = project.tasks.create("bundleDataLogToolOsxApp" + binary.targetPlatform.architecture.name, Copy) {
+                                description("Creates a macOS application bundle for DataLogTool")
+                                from(file("$project.projectDir/Info.plist"))
+                                into(file("$project.buildDir/outputs/bundles/$binary.targetPlatform.architecture.name/DataLogTool.app/Contents"))
+                                into("MacOS") {
+                                    with copySpec {
+                                        from binary.executable.file
+                                    }
+                                }
+                                into("Resources") {
+                                    with copySpec {
+                                        from icon
+                                    }
+                                }
+
+                                inputs.property "HasDeveloperId", project.hasProperty("developerID")
+
+                                doLast {
+                                    if (project.hasProperty("developerID")) {
+                                        // Get path to binary.
+                                        exec {
+                                            workingDir rootDir
+                                            def args = [
+                                                "sh",
+                                                "-c",
+                                                "codesign --force --strict --deep " +
+                                                "--timestamp --options=runtime " +
+                                                "--verbose -s ${project.findProperty("developerID")} " +
+                                                "$project.buildDir/outputs/bundles/$binary.targetPlatform.architecture.name/DataLogTool.app/"
+                                            ]
+                                            commandLine args
+                                        }
+                                    }
+                                }
+                            }
+
+                            // Reset the application path if we are creating a bundle.
+                            applicationPath = file("$project.buildDir/outputs/bundles/$binary.targetPlatform.architecture.name")
+                            task.from(applicationPath)
+                            project.build.dependsOn bundleTask
+
                             bundleTask.dependsOn binary.tasks.link
                             task.dependsOn(bundleTask)
+                        } else {
+                            task.from(applicationPath)
                         }
 
                         task.dependsOn binary.tasks.link

glass/publish.gradle

@@ -97,8 +97,6 @@ model {
                                 into '/'
                             }
 
-                            from(applicationPath)
-
                             if (binary.targetPlatform.operatingSystem.isWindows()) {
                                 def exePath = binary.executable.file.absolutePath
                                 exePath = exePath.substring(0, exePath.length() - 4)
@@ -149,10 +147,13 @@ model {
 
                             // Reset the application path if we are creating a bundle.
                             applicationPath = file("$project.buildDir/outputs/bundles/$binary.targetPlatform.architecture.name")
+                            task.from(applicationPath)
                             project.build.dependsOn bundleTask
 
                             bundleTask.dependsOn binary.tasks.link
                             task.dependsOn(bundleTask)
+                        } else {
+                            task.from(applicationPath)
                         }
 
                         task.dependsOn binary.tasks.link

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

@@ -5,6 +5,7 @@
 #include "glass/other/FMS.h"
 
 #include <imgui.h>
+#include <imgui_stdlib.h>
 #include <wpi/SmallString.h>
 
 #include "glass/DataSource.h"
@@ -58,8 +59,7 @@ void glass::DisplayFMS(FMSModel* model, bool editableDsAttached) {
   if (auto data = model->GetMatchTimeData()) {
     double val = data->GetValue();
     ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8);
-    if (ImGui::InputDouble("Match Time", &val, 0, 0, "%.1f",
-                           ImGuiInputTextFlags_EnterReturnsTrue)) {
+    if (ImGui::InputDouble("Match Time", &val, 0, 0, "%.1f")) {
       model->SetMatchTime(val);
     }
     data->EmitDrag();
@@ -78,16 +78,12 @@ void glass::DisplayFMS(FMSModel* model, bool editableDsAttached) {
   }
 
   // Game Specific Message
-  // make buffer full 64 width, null terminated, for editability
-  wpi::SmallString<64> gameSpecificMessage;
-  model->GetGameSpecificMessage(gameSpecificMessage);
-  gameSpecificMessage.resize(63);
-  gameSpecificMessage.push_back('\0');
+  wpi::SmallString<64> gameSpecificMessageBuf;
+  std::string gameSpecificMessage{
+      model->GetGameSpecificMessage(gameSpecificMessageBuf)};
   ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8);
-  if (ImGui::InputText("Game Specific", gameSpecificMessage.data(),
-                       gameSpecificMessage.size(),
-                       ImGuiInputTextFlags_EnterReturnsTrue)) {
-    model->SetGameSpecificMessage(gameSpecificMessage.data());
+  if (ImGui::InputText("Game Specific", &gameSpecificMessage)) {
+    model->SetGameSpecificMessage(gameSpecificMessage);
   }
 }
 
@@ -151,9 +147,10 @@ void glass::DisplayFMSReadOnly(FMSModel* model) {
     }
   }
 
-  wpi::SmallString<64> gameSpecificMessage;
-  model->GetGameSpecificMessage(gameSpecificMessage);
-  ImGui::Text("Game Specific: %s", exists ? gameSpecificMessage.c_str() : "?");
+  wpi::SmallString<64> gameSpecificMessageBuf;
+  std::string_view gameSpecificMessage =
+      model->GetGameSpecificMessage(gameSpecificMessageBuf);
+  ImGui::Text("Game Specific: %s", exists ? gameSpecificMessage.data() : "?");
 
   if (!exists) {
     ImGui::PopStyleColor();

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

@@ -222,8 +222,8 @@ class ObjectInfo {
 
 class FieldInfo {
  public:
-  static constexpr auto kDefaultWidth = 15.98_m;
-  static constexpr auto kDefaultHeight = 8.21_m;
+  static constexpr auto kDefaultWidth = 16.541052_m;
+  static constexpr auto kDefaultHeight = 8.211_m;
 
   explicit FieldInfo(Storage& storage);
 

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

@@ -75,6 +75,9 @@ public final class HAL extends JNIWrapper {
    */
   public static native void exitMain();
 
+  /** Terminates the executable (at the native level). Does nothing in simulation. */
+  public static native void terminate();
+
   private static native void simPeriodicBeforeNative();
 
   private static final List<Runnable> s_simPeriodicBefore = new ArrayList<>();

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

@@ -83,6 +83,8 @@ public class PowerDistributionJNI extends JNIWrapper {
   /**
    * Gets the temperature of the PowerDistribution.
    *
+   * <p>Not supported on the Rev PDH and returns 0.
+   *
    * @param handle the module handle
    * @return the module temperature (celsius)
    * @see "HAL_GetPowerDistributionTemperature"
@@ -129,7 +131,9 @@ public class PowerDistributionJNI extends JNIWrapper {
   public static native double getTotalCurrent(int handle);
 
   /**
-   * Gets the total power of the PowerDistribution.
+   * Gets the total power of the Power Distribution Panel.
+   *
+   * <p>Not supported on the Rev PDH and returns 0.
    *
    * @param handle the module handle
    * @return the total power (watts)
@@ -138,7 +142,9 @@ public class PowerDistributionJNI extends JNIWrapper {
   public static native double getTotalPower(int handle);
 
   /**
-   * Gets the total energy of the PowerDistribution.
+   * Gets the total energy of the Power Distribution Panel.
+   *
+   * <p>Not supported on the Rev PDH and does nothing.
    *
    * @param handle the module handle
    * @return the total energy (joules)
@@ -147,7 +153,9 @@ public class PowerDistributionJNI extends JNIWrapper {
   public static native double getTotalEnergy(int handle);
 
   /**
-   * Resets the PowerDistribution accumulated energy.
+   * Resets the Power Distribution Panel accumulated energy.
+   *
+   * <p>Not supported on the Rev PDH and returns 0.
    *
    * @param handle the module handle
    * @see "HAL_ClearPowerDistributionStickyFaults"

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

@@ -539,15 +539,25 @@ HAL_Bool HAL_RefreshDSData(void) {
     }
     // If newest state shows we have a DS attached, just use the
     // control word out of the cache, As it will be the one in sync
-    // with the data. Otherwise use the state that shows disconnected.
-    if (controlWord.dsAttached) {
-      newestControlWord = currentRead->controlWord;
-    } else {
-      // Zero out the control word. When the DS has never been connected
-      // this returns garbage. And there is no way we can detect that.
-      std::memset(&controlWord, 0, sizeof(controlWord));
-      newestControlWord = controlWord;
+    // with the data. If no data has been updated, at this point,
+    // and a DS wasn't attached previously, this will still return
+    // a zeroed out control word, with is the correct state for
+    // no new data.
+    if (!controlWord.dsAttached) {
+      // If the DS is not attached, we need to zero out the control word.
+      // This is because HAL_RefreshDSData is called asynchronously from
+      // the DS data. The dsAttached variable comes directly from netcomm
+      // and could be updated before the caches are. If that happens,
+      // we would end up returning the previous cached control word,
+      // which is out of sync with the current control word and could
+      // break invariants such as which alliance station is in used.
+      // Also, when the DS has never been connected the rest of the fields
+      // in control word are garbage, so we also need to zero out in that
+      // case too
+      std::memset(&currentRead->controlWord, 0,
+                  sizeof(currentRead->controlWord));
     }
+    newestControlWord = currentRead->controlWord;
   }
 
   uint32_t mask = tcpMask.exchange(0);

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

@@ -524,7 +524,7 @@ static bool killExistingProgram(int timeout, int mode) {
   return true;
 }
 
-static void SetupNowRio(void) {
+static bool SetupNowRio(void) {
   nFPGA::nRoboRIO_FPGANamespace::g_currentTargetClass =
       nLoadOut::getTargetClass();
 
@@ -534,13 +534,13 @@ static void SetupNowRio(void) {
   status = dladdr(reinterpret_cast<void*>(tHMB::create), &info);
   if (status == 0) {
     fmt::print(stderr, "Failed to call dladdr on chipobject {}\n", dlerror());
-    return;
+    return false;
   }
 
   void* chipObjectLibrary = dlopen(info.dli_fname, RTLD_LAZY);
   if (chipObjectLibrary == nullptr) {
     fmt::print(stderr, "Failed to call dlopen on chipobject {}\n", dlerror());
-    return;
+    return false;
   }
 
   std::unique_ptr<tHMB> hmb;
@@ -548,9 +548,9 @@ static void SetupNowRio(void) {
   if (hmb == nullptr) {
     fmt::print(stderr, "Failed to open HMB on chipobject {}\n", status);
     dlclose(chipObjectLibrary);
-    return;
+    return false;
   }
-  wpi::impl::SetupNowRio(chipObjectLibrary, std::move(hmb));
+  return wpi::impl::SetupNowRio(chipObjectLibrary, std::move(hmb));
 }
 
 HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) {
@@ -593,7 +593,17 @@ HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) {
     setNewDataSem(nullptr);
   });
 
-  SetupNowRio();
+  if (!SetupNowRio()) {
+    fmt::print(stderr,
+               "Failed to run SetupNowRio(). This is a fatal error. The "
+               "process is being terminated.\n");
+    std::fflush(stderr);
+    // Attempt to force a segfault to get a better java log
+    *reinterpret_cast<int*>(0) = 0;
+    // If that fails, terminate
+    std::terminate();
+    return false;
+  }
 
   int32_t status = 0;
 

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

@@ -6,6 +6,10 @@
 
 #include <jni.h>
 
+#ifdef __FRC_ROBORIO__
+#include <signal.h>
+#endif
+
 #include <cassert>
 #include <cstring>
 
@@ -82,6 +86,20 @@ Java_edu_wpi_first_hal_HAL_exitMain
   HAL_ExitMain();
 }
 
+/*
+ * Class:     edu_wpi_first_hal_HAL
+ * Method:    terminate
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_HAL_terminate
+  (JNIEnv*, jclass)
+{
+#ifdef __FRC_ROBORIO__
+  ::raise(SIGKILL);
+#endif
+}
+
 /*
  * Class:     edu_wpi_first_hal_HAL
  * Method:    simPeriodicBeforeNative

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

@@ -101,7 +101,9 @@ int32_t HAL_GetPowerDistributionNumChannels(HAL_PowerDistributionHandle handle,
                                             int32_t* status);
 
 /**
- * Gets the temperature of the PowerDistribution.
+ * Gets the temperature of the Power Distribution Panel.
+ *
+ * Not supported on the Rev PDH and returns 0.
  *
  * @param[in] handle the module handle
  * @param[out] status Error status variable. 0 on success.
@@ -156,7 +158,9 @@ double HAL_GetPowerDistributionTotalCurrent(HAL_PowerDistributionHandle handle,
                                             int32_t* status);
 
 /**
- * Gets the total power of the PowerDistribution.
+ * Gets the total power of the Power Distribution Panel.
+ *
+ * Not supported on the Rev PDH and returns 0.
  *
  * @param[in] handle the module handle
  * @param[out] status Error status variable. 0 on success.
@@ -166,7 +170,9 @@ double HAL_GetPowerDistributionTotalPower(HAL_PowerDistributionHandle handle,
                                           int32_t* status);
 
 /**
- * Gets the total energy of the PowerDistribution.
+ * Gets the total energy of the Power Distribution Panel.
+ *
+ * Not supported on the Rev PDH and returns 0.
  *
  * @param[in] handle the module handle
  * @param[out] status Error status variable. 0 on success.
@@ -178,6 +184,8 @@ double HAL_GetPowerDistributionTotalEnergy(HAL_PowerDistributionHandle handle,
 /**
  * Resets the PowerDistribution accumulated energy.
  *
+ * Not supported on the Rev PDH and does nothing.
+ *
  * @param[in] handle the module handle
  * @param[out] status Error status variable. 0 on success.
  */

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

@@ -44,6 +44,7 @@ struct JoystickDataCache {
   HAL_JoystickButtons buttons[kJoystickPorts];
   HAL_AllianceStationID allianceStation;
   double matchTime;
+  HAL_ControlWord controlWord;
 };
 static_assert(std::is_standard_layout_v<JoystickDataCache>);
 // static_assert(std::is_trivial_v<JoystickDataCache>);
@@ -65,6 +66,16 @@ void JoystickDataCache::Update() {
   }
   allianceStation = SimDriverStationData->allianceStationId;
   matchTime = SimDriverStationData->matchTime;
+
+  HAL_ControlWord tmpControlWord;
+  std::memset(&tmpControlWord, 0, sizeof(tmpControlWord));
+  tmpControlWord.enabled = SimDriverStationData->enabled;
+  tmpControlWord.autonomous = SimDriverStationData->autonomous;
+  tmpControlWord.test = SimDriverStationData->test;
+  tmpControlWord.eStop = SimDriverStationData->eStop;
+  tmpControlWord.fmsAttached = SimDriverStationData->fmsAttached;
+  tmpControlWord.dsAttached = SimDriverStationData->dsAttached;
+  this->controlWord = tmpControlWord;
 }
 
 #define CHECK_JOYSTICK_NUMBER(stickNum)                  \
@@ -322,20 +333,32 @@ HAL_Bool HAL_RefreshDSData(void) {
   if (gShutdown) {
     return false;
   }
-  HAL_ControlWord controlWord;
-  std::memset(&controlWord, 0, sizeof(controlWord));
-  controlWord.enabled = SimDriverStationData->enabled;
-  controlWord.autonomous = SimDriverStationData->autonomous;
-  controlWord.test = SimDriverStationData->test;
-  controlWord.eStop = SimDriverStationData->eStop;
-  controlWord.fmsAttached = SimDriverStationData->fmsAttached;
-  controlWord.dsAttached = SimDriverStationData->dsAttached;
+  bool dsAttached = SimDriverStationData->dsAttached;
   std::scoped_lock lock{driverStation->cacheMutex};
   JoystickDataCache* prev = currentCache.exchange(nullptr);
   if (prev != nullptr) {
     currentRead = prev;
   }
-  newestControlWord = controlWord;
+  // If newest state shows we have a DS attached, just use the
+  // control word out of the cache, As it will be the one in sync
+  // with the data. If no data has been updated, at this point,
+  // and a DS wasn't attached previously, this will still return
+  // a zeroed out control word, with is the correct state for
+  // no new data.
+  if (!dsAttached) {
+    // If the DS is not attached, we need to zero out the control word.
+    // This is because HAL_RefreshDSData is called asynchronously from
+    // the DS data. The dsAttached variable comes directly from netcomm
+    // and could be updated before the caches are. If that happens,
+    // we would end up returning the previous cached control word,
+    // which is out of sync with the current control word and could
+    // break invariants such as which alliance station is in used.
+    // Also, when the DS has never been connected the rest of the fields
+    // in control word are garbage, so we also need to zero out in that
+    // case too
+    std::memset(&currentRead->controlWord, 0, sizeof(currentRead->controlWord));
+  }
+  newestControlWord = currentRead->controlWord;
   return prev != nullptr;
 }
 
@@ -369,6 +392,7 @@ void NewDriverStationData() {
   if (gShutdown) {
     return;
   }
+  SimDriverStationData->dsAttached = true;
   cacheToUpdate->Update();
 
   JoystickDataCache* given = cacheToUpdate;
@@ -382,7 +406,6 @@ void NewDriverStationData() {
   }
   lastGiven = given;
 
-  SimDriverStationData->dsAttached = true;
   driverStation->newDataEvents.Wakeup();
   SimDriverStationData->CallNewDataCallbacks();
 }

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

@@ -15,7 +15,8 @@ public class PubSubOption {
     disableRemote,
     disableLocal,
     excludePublisher,
-    excludeSelf
+    excludeSelf,
+    hidden
   }
 
   PubSubOption(Kind kind, boolean value) {
@@ -149,6 +150,18 @@ public class PubSubOption {
     return new PubSubOption(Kind.excludeSelf, enabled);
   }
 
+  /**
+   * For subscriptions, don't share the existence of the subscription with the network. Note this
+   * means updates will not be received from the network unless another subscription overlaps with
+   * this one, and the subscription will not appear in metatopics.
+   *
+   * @param enabled True to enable, false to disable
+   * @return option
+   */
+  public static PubSubOption hidden(boolean enabled) {
+    return new PubSubOption(Kind.hidden, enabled);
+  }
+
   final Kind m_kind;
   final boolean m_bValue;
   final int m_iValue;

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

@@ -42,6 +42,9 @@ public class PubSubOptions {
         case excludeSelf:
           excludeSelf = option.m_bValue;
           break;
+        case hidden:
+          hidden = option.m_bValue;
+          break;
         default:
           break;
       }
@@ -58,7 +61,8 @@ public class PubSubOptions {
       boolean prefixMatch,
       boolean disableRemote,
       boolean disableLocal,
-      boolean excludeSelf) {
+      boolean excludeSelf,
+      boolean hidden) {
     this.pollStorage = pollStorage;
     this.periodic = periodic;
     this.excludePublisher = excludePublisher;
@@ -69,6 +73,7 @@ public class PubSubOptions {
     this.disableRemote = disableRemote;
     this.disableLocal = disableLocal;
     this.excludeSelf = excludeSelf;
+    this.hidden = hidden;
   }
 
   /** Default value of periodic. */
@@ -123,4 +128,11 @@ public class PubSubOptions {
 
   /** For entries, don't queue (for readQueue) value updates for the entry's internal publisher. */
   public boolean excludeSelf;
+
+  /**
+   * For subscriptions, don't share the existence of the subscription with the network. Note this
+   * means updates will not be received from the network unless another subscription overlaps with
+   * this one, and the subscription will not appear in metatopics.
+   */
+  public boolean hidden;
 }

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

@@ -611,7 +611,7 @@ LocalStorage::SubscriberData* LocalStorage::Impl::AddLocalSubscriber(
         "published as '{}')",
         topic->name, config.typeStr, topic->typeStr);
   }
-  if (m_network) {
+  if (m_network && !subscriber->config.hidden) {
     DEBUG4("-> NetworkSubscribe({})", topic->name);
     m_network->Subscribe(subscriber->handle, {{topic->name}}, config);
   }
@@ -640,7 +640,7 @@ LocalStorage::Impl::RemoveLocalSubscriber(NT_Subscriber subHandle) {
         listener.getSecond()->subscriber = nullptr;
       }
     }
-    if (m_network) {
+    if (m_network && !subscriber->config.hidden) {
       m_network->Unsubscribe(subscriber->handle);
     }
   }
@@ -676,7 +676,7 @@ LocalStorage::MultiSubscriberData* LocalStorage::Impl::AddMultiSubscriber(
       }
     }
   }
-  if (m_network) {
+  if (m_network && !subscriber->options.hidden) {
     DEBUG4("-> NetworkSubscribe");
     m_network->Subscribe(subscriber->handle, subscriber->prefixes,
                          subscriber->options);
@@ -696,7 +696,7 @@ LocalStorage::Impl::RemoveMultiSubscriber(NT_MultiSubscriber subHandle) {
         listener.getSecond()->multiSubscriber = nullptr;
       }
     }
-    if (m_network) {
+    if (m_network && !subscriber->options.hidden) {
       m_network->Unsubscribe(subscriber->handle);
     }
   }
@@ -1128,12 +1128,16 @@ void LocalStorage::Impl::StartNetwork(net::NetworkInterface* network) {
     }
   }
   for (auto&& subscriber : m_subscribers) {
-    network->Subscribe(subscriber->handle, {{subscriber->topic->name}},
-                       subscriber->config);
+    if (!subscriber->config.hidden) {
+      network->Subscribe(subscriber->handle, {{subscriber->topic->name}},
+                         subscriber->config);
+    }
   }
   for (auto&& subscriber : m_multiSubscribers) {
-    network->Subscribe(subscriber->handle, subscriber->prefixes,
-                       subscriber->options);
+    if (!subscriber->options.hidden) {
+      network->Subscribe(subscriber->handle, subscriber->prefixes,
+                         subscriber->options);
+    }
   }
 }
 

ntcore/src/main/native/cpp/jni/NetworkTablesJNI.cpp

@@ -139,6 +139,7 @@ static nt::PubSubOptions FromJavaPubSubOptions(JNIEnv* env, jobject joptions) {
   FIELD(disableRemote, "Z");
   FIELD(disableLocal, "Z");
   FIELD(excludeSelf, "Z");
+  FIELD(hidden, "Z");
 
 #undef FIELD
 
@@ -154,7 +155,8 @@ static nt::PubSubOptions FromJavaPubSubOptions(JNIEnv* env, jobject joptions) {
           FIELD(bool, Boolean, prefixMatch),
           FIELD(bool, Boolean, disableRemote),
           FIELD(bool, Boolean, disableLocal),
-          FIELD(bool, Boolean, excludeSelf)};
+          FIELD(bool, Boolean, excludeSelf),
+          FIELD(bool, Boolean, hidden)};
 
 #undef GET
 #undef FIELD

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

@@ -1816,7 +1816,8 @@ void ServerImpl::SetValue(ClientData* client, TopicData* topic,
   }
 
   for (auto&& tcd : topic->clients) {
-    if (tcd.second.sendMode != ValueSendMode::kDisabled) {
+    if (tcd.first != client &&
+        tcd.second.sendMode != ValueSendMode::kDisabled) {
       tcd.first->SendValue(topic, value, tcd.second.sendMode);
     }
   }

ntcore/src/main/native/cpp/ntcore_c.cpp

@@ -126,6 +126,7 @@ static PubSubOptions ConvertToCpp(const NT_PubSubOptions* in) {
   out.disableRemote = in->disableRemote;
   out.disableLocal = in->disableLocal;
   out.excludeSelf = in->excludeSelf;
+  out.hidden = in->hidden;
   return out;
 }
 

ntcore/src/main/native/include/networktables/StructArrayTopic.h

@@ -312,6 +312,9 @@ class StructArrayPublisher : public Publisher {
   void Set(std::span<const T> value, int64_t time = 0) {
     std::apply(
         [&](const I&... info) {
+          if (!m_schemaPublished.exchange(true, std::memory_order_relaxed)) {
+            GetTopic().GetInstance().template AddStructSchema<T>(info...);
+          }
           m_buf.Write(
               value,
               [&](auto bytes) { ::nt::SetRaw(m_pubHandle, bytes, time); },
@@ -356,6 +359,9 @@ class StructArrayPublisher : public Publisher {
   void SetDefault(std::span<const T> value) {
     std::apply(
         [&](const I&... info) {
+          if (!m_schemaPublished.exchange(true, std::memory_order_relaxed)) {
+            GetTopic().GetInstance().template AddStructSchema<T>(info...);
+          }
           m_buf.Write(
               value,
               [&](auto bytes) { ::nt::SetDefaultRaw(m_pubHandle, bytes); },

ntcore/src/main/native/include/ntcore_c.h

@@ -367,6 +367,14 @@ struct NT_PubSubOptions {
    * internal publisher.
    */
   NT_Bool excludeSelf;
+
+  /**
+   * For subscriptions, don't share the existence of the subscription with the
+   * network. Note this means updates will not be received from the network
+   * unless another subscription overlaps with this one, and the subscription
+   * will not appear in metatopics.
+   */
+  NT_Bool hidden;
 };
 
 /**

ntcore/src/main/native/include/ntcore_cpp.h

@@ -374,6 +374,14 @@ struct PubSubOptions {
    * internal publisher.
    */
   bool excludeSelf = false;
+
+  /**
+   * For subscriptions, don't share the existence of the subscription with the
+   * network. Note this means updates will not be received from the network
+   * unless another subscription overlaps with this one, and the subscription
+   * will not appear in metatopics.
+   */
+  bool hidden = false;
 };
 
 /**

outlineviewer/publish.gradle

@@ -29,51 +29,8 @@ model {
                         def applicationPath = binary.executable.file
                         def icon = file("$project.projectDir/src/main/native/mac/ov.icns")
 
-                        // Create the macOS bundle.
-                        def bundleTask = project.tasks.create("bundleOutlineViewerOsxApp" + binary.targetPlatform.architecture.name, Copy) {
-                            description("Creates a macOS application bundle for OutlineViewer")
-                            from(file("$project.projectDir/Info.plist"))
-                            into(file("$project.buildDir/outputs/bundles/$binary.targetPlatform.architecture.name/OutlineViewer.app/Contents"))
-                            into("MacOS") {
-                                with copySpec {
-                                    from binary.executable.file
-                                }
-                            }
-                            into("Resources") {
-                                with copySpec {
-                                    from icon
-                                }
-                            }
-
-                            inputs.property "HasDeveloperId", project.hasProperty("developerID")
-
-                            doLast {
-                                if (project.hasProperty("developerID")) {
-                                    // Get path to binary.
-                                    exec {
-                                        workingDir rootDir
-                                        def args = [
-                                            "sh",
-                                            "-c",
-                                            "codesign --force --strict --deep " +
-                                            "--timestamp --options=runtime " +
-                                            "--verbose -s ${project.findProperty("developerID")} " +
-                                            "$project.buildDir/outputs/bundles/$binary.targetPlatform.architecture.name/OutlineViewer.app/"
-                                        ]
-                                        commandLine args
-                                    }
-                                }
-                            }
-                        }
-
-                        // Reset the application path if we are creating a bundle.
-                        if (binary.targetPlatform.operatingSystem.isMacOsX()) {
-                            applicationPath = file("$project.buildDir/outputs/bundles/$binary.targetPlatform.architecture.name")
-                            project.build.dependsOn bundleTask
-                        }
-
                         // Create the ZIP.
-                        def task = project.tasks.create("copyOutlineViewerExecutable" + binary.targetPlatform.architecture.name, Zip) {
+                        def task = project.tasks.create("copyOutlineViewerExecutable" + binary.targetPlatform.operatingSystem.name + binary.targetPlatform.architecture.name, Zip) {
                             description("Copies the OutlineViewer executable to the outputs directory.")
                             destinationDirectory = outputsFolder
 
@@ -85,8 +42,6 @@ model {
                                 into '/'
                             }
 
-                            from(applicationPath)
-
                             if (binary.targetPlatform.operatingSystem.isWindows()) {
                                 def exePath = binary.executable.file.absolutePath
                                 exePath = exePath.substring(0, exePath.length() - 4)
@@ -98,8 +53,52 @@ model {
                         }
 
                         if (binary.targetPlatform.operatingSystem.isMacOsX()) {
+                            // Create the macOS bundle.
+                            def bundleTask = project.tasks.create("bundleOutlineViewerOsxApp" + binary.targetPlatform.architecture.name, Copy) {
+                                description("Creates a macOS application bundle for OutlineViewer")
+                                from(file("$project.projectDir/Info.plist"))
+                                into(file("$project.buildDir/outputs/bundles/$binary.targetPlatform.architecture.name/OutlineViewer.app/Contents"))
+                                into("MacOS") {
+                                    with copySpec {
+                                        from binary.executable.file
+                                    }
+                                }
+                                into("Resources") {
+                                    with copySpec {
+                                        from icon
+                                    }
+                                }
+
+                                inputs.property "HasDeveloperId", project.hasProperty("developerID")
+
+                                doLast {
+                                    if (project.hasProperty("developerID")) {
+                                        // Get path to binary.
+                                        exec {
+                                            workingDir rootDir
+                                            def args = [
+                                                "sh",
+                                                "-c",
+                                                "codesign --force --strict --deep " +
+                                                "--timestamp --options=runtime " +
+                                                "--verbose -s ${project.findProperty("developerID")} " +
+                                                "$project.buildDir/outputs/bundles/$binary.targetPlatform.architecture.name/OutlineViewer.app/"
+                                            ]
+                                            commandLine args
+                                        }
+                                    }
+                                }
+                            }
+
+                            // Reset the application path if we are creating a bundle.
+                            applicationPath = file("$project.buildDir/outputs/bundles/$binary.targetPlatform.architecture.name")
+                            task.from(applicationPath)
+                            project.build.dependsOn bundleTask
+
                             bundleTask.dependsOn binary.tasks.link
                             task.dependsOn(bundleTask)
+                        } else {
+                            task.from(applicationPath)
                         }
 
                         task.dependsOn binary.tasks.link

roborioteamnumbersetter/publish.gradle

@@ -42,8 +42,6 @@ model {
                                 into '/'
                             }
 
-                            from(applicationPath)
-
                             if (binary.targetPlatform.operatingSystem.isWindows()) {
                                 def exePath = binary.executable.file.absolutePath
                                 exePath = exePath.substring(0, exePath.length() - 4)
@@ -94,10 +92,13 @@ model {
 
                             // Reset the application path if we are creating a bundle.
                             applicationPath = file("$project.buildDir/outputs/bundles/$binary.targetPlatform.architecture.name")
+                            task.from(applicationPath)
                             project.build.dependsOn bundleTask
 
                             bundleTask.dependsOn binary.tasks.link
                             task.dependsOn(bundleTask)
+                        } else {
+                            task.from(applicationPath)
                         }
 
                         task.dependsOn binary.tasks.link

simulation/halsim_gui/src/main/native/cpp/main.cpp

@@ -51,6 +51,8 @@ __declspec(dllexport)
 
   glass::SetStorageName("simgui");
 
+  gui::AddInit([] { ImGui::GetIO().ConfigDockingWithShift = true; });
+
   HAL_RegisterExtension(HALSIMGUI_EXT_ADDGUIINIT,
                         reinterpret_cast<void*>((AddGuiInitFn)&AddGuiInit));
   HAL_RegisterExtension(

sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp

@@ -4,9 +4,10 @@
 
 #include "sysid/analysis/FeedbackAnalysis.h"
 
+#include <cmath>
+
 #include <frc/controller/LinearQuadraticRegulator.h>
 #include <frc/system/LinearSystem.h>
-#include <frc/system/plant/LinearSystemId.h>
 #include <units/acceleration.h>
 #include <units/velocity.h>
 #include <units/voltage.h>
@@ -21,22 +22,30 @@ using Ka_t = decltype(1_V / 1_mps_sq);
 FeedbackGains sysid::CalculatePositionFeedbackGains(
     const FeedbackControllerPreset& preset, const LQRParameters& params,
     double Kv, double Ka) {
+  if (!std::isfinite(Kv) || !std::isfinite(Ka)) {
+    return {0.0, 0.0};
+  }
+
   // If acceleration requires no effort, velocity becomes an input for position
   // control. We choose an appropriate model in this case to avoid numerical
   // instabilities in the LQR.
   if (Ka > 1E-7) {
     // Create a position system from our feedforward gains.
-    auto system = frc::LinearSystemId::IdentifyPositionSystem<units::meter>(
-        Kv_t(Kv), Ka_t(Ka));
+    frc::LinearSystem<2, 1, 1> system{
+        frc::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -Kv / Ka}},
+        frc::Matrixd<2, 1>{0.0, 1.0 / Ka}, frc::Matrixd<1, 2>{1.0, 0.0},
+        frc::Matrixd<1, 1>{0.0}};
     // Create an LQR with 2 states to control -- position and velocity.
     frc::LinearQuadraticRegulator<2, 1> controller{
         system, {params.qp, params.qv}, {params.r}, preset.period};
     // Compensate for any latency from sensor measurements, filtering, etc.
-    controller.LatencyCompensate(system, preset.period, 0.0_s);
+    controller.LatencyCompensate(system, preset.period,
+                                 preset.measurementDelay);
 
-    return {controller.K(0, 0) * preset.outputConversionFactor,
-            controller.K(0, 1) * preset.outputConversionFactor /
-                (preset.normalized ? 1 : preset.period.value())};
+    return {
+        controller.K(0, 0) * preset.outputConversionFactor,
+        controller.K(0, 1) * preset.outputConversionFactor /
+            (preset.normalized ? 1 : units::second_t{preset.period}.value())};
   }
 
   // This is our special model to avoid instabilities in the LQR.
@@ -47,7 +56,7 @@ FeedbackGains sysid::CalculatePositionFeedbackGains(
   frc::LinearQuadraticRegulator<1, 1> controller{
       system, {params.qp}, {params.r}, preset.period};
   // Compensate for any latency from sensor measurements, filtering, etc.
-  controller.LatencyCompensate(system, preset.period, 0.0_s);
+  controller.LatencyCompensate(system, preset.period, preset.measurementDelay);
 
   return {Kv * controller.K(0, 0) * preset.outputConversionFactor, 0.0};
 }
@@ -55,6 +64,10 @@ FeedbackGains sysid::CalculatePositionFeedbackGains(
 FeedbackGains sysid::CalculateVelocityFeedbackGains(
     const FeedbackControllerPreset& preset, const LQRParameters& params,
     double Kv, double Ka, double encFactor) {
+  if (!std::isfinite(Kv) || !std::isfinite(Ka)) {
+    return {0.0, 0.0};
+  }
+
   // If acceleration for velocity control requires no effort, the feedback
   // control gains approach zero. We special-case it here because numerical
   // instabilities arise in LQR otherwise.
@@ -63,8 +76,9 @@ FeedbackGains sysid::CalculateVelocityFeedbackGains(
   }
 
   // Create a velocity system from our feedforward gains.
-  auto system = frc::LinearSystemId::IdentifyVelocitySystem<units::meter>(
-      Kv_t(Kv), Ka_t(Ka));
+  frc::LinearSystem<1, 1, 1> system{
+      frc::Matrixd<1, 1>{-Kv / Ka}, frc::Matrixd<1, 1>{1.0 / Ka},
+      frc::Matrixd<1, 1>{1.0}, frc::Matrixd<1, 1>{0.0}};
   // Create an LQR controller with 1 state -- velocity.
   frc::LinearQuadraticRegulator<1, 1> controller{
       system, {params.qv}, {params.r}, preset.period};

sysid/src/main/native/cpp/analysis/FilteringUtils.cpp

@@ -146,9 +146,18 @@ sysid::TrimStepVoltageData(std::vector<PreparedData>* data,
                wpi::sgn(b.velocity) * b.acceleration;
       });
 
+  // Current limiting can delay onset of the peak acceleration, so we need to
+  // find the first acceleration *near* the max.  Magic number tolerance here
+  // because this whole file is tech debt already
+  auto accelBegins = std::find_if(
+      data->begin(), data->end(), [&maxAccel](const auto& measurement) {
+        return wpi::sgn(measurement.velocity) * measurement.acceleration >
+               0.8 * wpi::sgn(maxAccel->velocity) * maxAccel->acceleration;
+      });
+
   units::second_t velocityDelay;
-  if (maxAccel != data->end()) {
-    velocityDelay = maxAccel->timestamp - firstTimestamp;
+  if (accelBegins != data->end()) {
+    velocityDelay = accelBegins->timestamp - firstTimestamp;
 
     // Trim data before max acceleration
     data->erase(data->begin(), maxAccel);

sysid/src/main/native/cpp/analysis/OLS.cpp

@@ -60,11 +60,11 @@ OLSResult OLS(const Eigen::MatrixXd& X, const Eigen::VectorXd& y) {
   //
   //   SSTO = yᵀy - 1/n yᵀJy
   //
-  // Let J = I.
-  //
-  //   SSTO = yᵀy - 1/n yᵀy
-  //   SSTO = (n − 1)/n yᵀy
-  double SSTO = (n - 1.0) / n * (y.transpose() * y).value();
+  // where J is a matrix of ones.
+  double SSTO =
+      (y.transpose() * y - 1.0 / y.rows() * y.transpose() *
+                               Eigen::MatrixXd::Ones(y.rows(), y.rows()) * y)
+          .value();
 
   // R² or the coefficient of determination, which represents how much of the
   // total variation (variation in y) can be explained by the regression model

sysid/src/main/native/include/sysid/analysis/ArmSim.h

@@ -33,7 +33,7 @@ class ArmSim {
    * forward dt seconds.
    *
    * @param voltage Voltage to apply over the timestep.
-   * @param dt      Sammple period.
+   * @param dt      Sample period.
    */
   void Update(units::volt_t voltage, units::second_t dt);
 

sysid/src/main/native/include/sysid/analysis/ElevatorSim.h

@@ -31,7 +31,7 @@ class ElevatorSim {
    * dt seconds.
    *
    * @param voltage Voltage to apply over the timestep.
-   * @param dt      Sammple period.
+   * @param dt      Sample period.
    */
   void Update(units::volt_t voltage, units::second_t dt);
 

sysid/src/main/native/include/sysid/analysis/SimpleMotorSim.h

@@ -30,7 +30,7 @@ class SimpleMotorSim {
    * seconds.
    *
    * @param voltage Voltage to apply over the timestep.
-   * @param dt      Sammple period.
+   * @param dt      Sample period.
    */
   void Update(units::volt_t voltage, units::second_t dt);
 

sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp

@@ -103,3 +103,43 @@ TEST(FeedbackAnalysisTest, VelocityREVConversion) {
   EXPECT_NEAR(Kp, 0.00241 / 3, 0.005);
   EXPECT_NEAR(Kd, 0.00, 0.05);
 }
+
+TEST(FeedbackAnalysisTest, Position) {
+  auto Kv = 3.060;
+  auto Ka = 0.327;
+
+  sysid::LQRParameters params{1, 1.5, 7};
+
+  auto [Kp, Kd] = sysid::CalculatePositionFeedbackGains(
+      sysid::presets::kDefault, params, Kv, Ka);
+
+  EXPECT_NEAR(Kp, 6.41, 0.05);
+  EXPECT_NEAR(Kd, 2.48, 0.05);
+}
+
+TEST(FeedbackAnalysisTest, PositionWithLatencyCompensation) {
+  auto Kv = 3.060;
+  auto Ka = 0.327;
+
+  sysid::LQRParameters params{1, 1.5, 7};
+  sysid::FeedbackControllerPreset preset{sysid::presets::kDefault};
+
+  preset.measurementDelay = 10_ms;
+  auto [Kp, Kd] = sysid::CalculatePositionFeedbackGains(preset, params, Kv, Ka);
+
+  EXPECT_NEAR(Kp, 5.92, 0.05);
+  EXPECT_NEAR(Kd, 2.12, 0.05);
+}
+
+TEST(FeedbackAnalysisTest, PositionREV) {
+  auto Kv = 3.060;
+  auto Ka = 0.327;
+
+  sysid::LQRParameters params{1, 1.5, 7};
+
+  auto [Kp, Kd] = sysid::CalculatePositionFeedbackGains(
+      sysid::presets::kREVNEOBuiltIn, params, Kv, Ka);
+
+  EXPECT_NEAR(Kp, 0.30202, 0.05);
+  EXPECT_NEAR(Kd, 48.518, 0.05);
+}

sysid/src/test/native/cpp/analysis/OLSTest.cpp

@@ -14,9 +14,9 @@ TEST(OLSTest, TwoVariablesTwoPoints) {
   auto [coeffs, rSquared, rmse] = sysid::OLS(X, y);
   EXPECT_EQ(coeffs.size(), 2u);
 
-  EXPECT_NEAR(coeffs[0], 1.0, 0.05);
-  EXPECT_NEAR(coeffs[1], 2.0, 0.05);
-  EXPECT_NEAR(rSquared, 1.0, 1e-4);
+  EXPECT_DOUBLE_EQ(coeffs[0], 1.0);
+  EXPECT_DOUBLE_EQ(coeffs[1], 2.0);
+  EXPECT_DOUBLE_EQ(rSquared, 1.0);
 }
 
 TEST(OLSTest, TwoVariablesFivePoints) {
@@ -28,9 +28,9 @@ TEST(OLSTest, TwoVariablesFivePoints) {
   auto [coeffs, rSquared, rmse] = sysid::OLS(X, y);
   EXPECT_EQ(coeffs.size(), 2u);
 
-  EXPECT_NEAR(coeffs[0], 0.305, 0.05);
-  EXPECT_NEAR(coeffs[1], 1.518, 0.05);
-  EXPECT_NEAR(rSquared, 0.985, 0.05);
+  EXPECT_DOUBLE_EQ(coeffs[0], 0.30487804878048774);
+  EXPECT_DOUBLE_EQ(coeffs[1], 1.5182926829268293);
+  EXPECT_DOUBLE_EQ(rSquared, 0.91906029466386019);
 }
 
 #ifndef NDEBUG

upstream_utils/update_expected.py

@@ -0,0 +1,45 @@
+#!/usr/bin/env python3
+
+import os
+import re
+import shutil
+
+from upstream_utils import (
+    get_repo_root,
+    clone_repo,
+    comment_out_invalid_includes,
+    walk_cwd_and_copy_if,
+    git_am,
+)
+
+
+def main():
+    upstream_root = clone_repo(
+        "https://github.com/TartanLlama/expected",
+        # master on 2024-01-25
+        "3f0ca7b19253129700a073abfa6d8638d9f7c80c",
+        shallow=False,
+    )
+    wpilib_root = get_repo_root()
+    wpiutil = os.path.join(wpilib_root, "wpiutil")
+
+    # Copy expected header into allwpilib
+    dest_filename = os.path.join(
+        wpiutil, "src/main/native/thirdparty/expected/include/wpi/expected"
+    )
+    shutil.copyfile(
+        os.path.join(upstream_root, "include/tl/expected.hpp"), dest_filename
+    )
+
+    # Rename namespace from tl to wpi
+    with open(dest_filename) as f:
+        content = f.read()
+    content = content.replace("namespace tl", "namespace wpi")
+    content = content.replace("tl::", "wpi::")
+    content = content.replace("TL_", "WPI_")
+    with open(dest_filename, "w") as f:
+        f.write(content)
+
+
+if __name__ == "__main__":
+    main()

vcpkg.json

@@ -3,10 +3,9 @@
   "version-string": "latest",
   "dependencies": [
     "opencv",
-    "eigen3",
     "fmt",
     "libuv",
     "protobuf"
   ],
-  "builtin-baseline": "78b61582c9e093fda56a01ebb654be15a0033897"
+  "builtin-baseline": "37c3e63a1306562f7f59c4c3c8892ddd50fdf992"
 }

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

@@ -63,7 +63,7 @@ public abstract class TrapezoidProfileSubsystem extends SubsystemBase {
 
   @Override
   public void periodic() {
-    m_state = m_profile.calculate(m_period, m_goal, m_state);
+    m_state = m_profile.calculate(m_period, m_state, m_goal);
     if (m_enabled) {
       useState(m_state);
     }

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutine.java

@@ -226,7 +226,7 @@ public class SysIdRoutine extends SysIdRoutineLog {
     Timer timer = new Timer();
     return m_mechanism
         .m_subsystem
-        .runOnce(timer::start)
+        .runOnce(timer::restart)
         .andThen(
             m_mechanism.m_subsystem.run(
                 () -> {

wpilibNewCommands/src/main/native/cpp/frc2/command/sysid/SysIdRoutine.cpp

@@ -18,11 +18,7 @@ frc2::CommandPtr SysIdRoutine::Quasistatic(Direction direction) {
 
   double outputSign = direction == Direction::kForward ? 1.0 : -1.0;
 
-  return m_mechanism.m_subsystem
-      ->RunOnce([this] {
-        timer.Reset();
-        timer.Start();
-      })
+  return m_mechanism.m_subsystem->RunOnce([this] { timer.Restart(); })
       .AndThen(
           m_mechanism.m_subsystem
               ->Run([this, state, outputSign] {

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

@@ -2,7 +2,6 @@
 // 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 <cmath>
 #include <functional>
 #include <memory>
 

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

@@ -9,7 +9,6 @@
 #pragma warning(disable : 4521)
 #endif
 
-#include <concepts>
 #include <memory>
 #include <type_traits>
 #include <utility>
@@ -17,7 +16,6 @@
 
 #include <wpi/DecayedDerivedFrom.h>
 
-#include "frc2/command/CommandBase.h"
 #include "frc2/command/CommandHelper.h"
 
 namespace frc2 {

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

@@ -2,9 +2,7 @@
 // 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 <cmath>
 #include <functional>
-#include <memory>
 
 #include <frc/Timer.h>
 #include <frc/controller/HolonomicDriveController.h>

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

@@ -45,7 +45,7 @@ class TrapezoidProfileSubsystem : public SubsystemBase {
         m_period(period) {}
 
   void Periodic() override {
-    m_state = m_profile.Calculate(m_period, m_goal, m_state);
+    m_state = m_profile.Calculate(m_period, m_state, m_goal);
     if (m_enabled) {
       UseState(m_state);
     }

wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h

@@ -4,9 +4,7 @@
 
 #pragma once
 
-#include <concepts>
 #include <functional>
-#include <memory>
 #include <utility>
 
 #include <frc/event/BooleanEvent.h>

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggersTest.java

@@ -6,7 +6,6 @@ package edu.wpi.first.wpilibj2.command.button;
 
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
-import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.simulation.DriverStationSim;
 import edu.wpi.first.wpilibj2.command.CommandTestBase;
 import org.junit.jupiter.api.Test;
@@ -18,7 +17,7 @@ class RobotModeTriggersTest extends CommandTestBase {
     DriverStationSim.setAutonomous(true);
     DriverStationSim.setTest(false);
     DriverStationSim.setEnabled(true);
-    DriverStation.refreshData();
+    DriverStationSim.notifyNewData();
     Trigger auto = RobotModeTriggers.autonomous();
     assertTrue(auto.getAsBoolean());
   }
@@ -29,7 +28,7 @@ class RobotModeTriggersTest extends CommandTestBase {
     DriverStationSim.setAutonomous(false);
     DriverStationSim.setTest(false);
     DriverStationSim.setEnabled(true);
-    DriverStation.refreshData();
+    DriverStationSim.notifyNewData();
     Trigger teleop = RobotModeTriggers.teleop();
     assertTrue(teleop.getAsBoolean());
   }
@@ -40,7 +39,7 @@ class RobotModeTriggersTest extends CommandTestBase {
     DriverStationSim.setAutonomous(false);
     DriverStationSim.setTest(true);
     DriverStationSim.setEnabled(true);
-    DriverStation.refreshData();
+    DriverStationSim.notifyNewData();
     Trigger test = RobotModeTriggers.test();
     assertTrue(test.getAsBoolean());
   }
@@ -51,7 +50,7 @@ class RobotModeTriggersTest extends CommandTestBase {
     DriverStationSim.setAutonomous(false);
     DriverStationSim.setTest(false);
     DriverStationSim.setEnabled(false);
-    DriverStation.refreshData();
+    DriverStationSim.notifyNewData();
     Trigger disabled = RobotModeTriggers.disabled();
     assertTrue(disabled.getAsBoolean());
   }

wpilibNewCommands/src/test/native/cpp/frc2/command/button/RobotModeTriggersTest.cpp

@@ -18,7 +18,7 @@ TEST(RobotModeTriggersTest, Autonomous) {
   DriverStationSim::SetAutonomous(true);
   DriverStationSim::SetTest(false);
   DriverStationSim::SetEnabled(true);
-  frc::DriverStation::RefreshData();
+  DriverStationSim::NotifyNewData();
   Trigger autonomous = RobotModeTriggers::Autonomous();
   EXPECT_TRUE(autonomous.Get());
 }
@@ -28,7 +28,7 @@ TEST(RobotModeTriggersTest, Teleop) {
   DriverStationSim::SetAutonomous(false);
   DriverStationSim::SetTest(false);
   DriverStationSim::SetEnabled(true);
-  frc::DriverStation::RefreshData();
+  DriverStationSim::NotifyNewData();
   Trigger teleop = RobotModeTriggers::Teleop();
   EXPECT_TRUE(teleop.Get());
 }
@@ -38,7 +38,7 @@ TEST(RobotModeTriggersTest, Disabled) {
   DriverStationSim::SetAutonomous(false);
   DriverStationSim::SetTest(false);
   DriverStationSim::SetEnabled(false);
-  frc::DriverStation::RefreshData();
+  DriverStationSim::NotifyNewData();
   Trigger disabled = RobotModeTriggers::Disabled();
   EXPECT_TRUE(disabled.Get());
 }
@@ -48,7 +48,7 @@ TEST(RobotModeTriggersTest, TestMode) {
   DriverStationSim::SetAutonomous(false);
   DriverStationSim::SetTest(true);
   DriverStationSim::SetEnabled(true);
-  frc::DriverStation::RefreshData();
+  DriverStationSim::NotifyNewData();
   Trigger test = RobotModeTriggers::Test();
   EXPECT_TRUE(test.Get());
 }

wpilibc/src/main/native/cpp/DigitalOutput.cpp

@@ -78,7 +78,7 @@ int DigitalOutput::GetChannel() const {
 
 void DigitalOutput::Pulse(units::second_t pulseLength) {
   int32_t status = 0;
-  HAL_Pulse(m_handle, pulseLength.to<double>(), &status);
+  HAL_Pulse(m_handle, pulseLength.value(), &status);
   FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 }
 

wpilibc/src/main/native/cpp/counter/Tachometer.cpp

@@ -44,7 +44,7 @@ Tachometer::~Tachometer() {
 
 units::hertz_t Tachometer::GetFrequency() const {
   auto period = GetPeriod();
-  if (period.to<double>() == 0) {
+  if (period.value() == 0) {
     return units::hertz_t{0.0};
   }
   return 1 / period;
@@ -66,7 +66,7 @@ void Tachometer::SetEdgesPerRevolution(int edges) {
 
 units::turns_per_second_t Tachometer::GetRevolutionsPerSecond() const {
   auto period = GetPeriod();
-  if (period.to<double>() == 0) {
+  if (period.value() == 0) {
     return units::turns_per_second_t{0.0};
   }
   int edgesPerRevolution = GetEdgesPerRevolution();
@@ -74,7 +74,7 @@ units::turns_per_second_t Tachometer::GetRevolutionsPerSecond() const {
     return units::turns_per_second_t{0.0};
   }
   auto rotationHz = ((1.0 / edgesPerRevolution) / period);
-  return units::turns_per_second_t{rotationHz.to<double>()};
+  return units::turns_per_second_t{rotationHz.value()};
 }
 
 units::revolutions_per_minute_t Tachometer::GetRevolutionsPerMinute() const {
@@ -103,7 +103,7 @@ void Tachometer::SetSamplesToAverage(int samples) {
 
 void Tachometer::SetMaxPeriod(units::second_t maxPeriod) {
   int32_t status = 0;
-  HAL_SetCounterMaxPeriod(m_handle, maxPeriod.to<double>(), &status);
+  HAL_SetCounterMaxPeriod(m_handle, maxPeriod.value(), &status);
   FRC_CheckErrorStatus(status, "Channel {}", m_source->GetChannel());
 }
 
@@ -116,7 +116,7 @@ void Tachometer::SetUpdateWhenEmpty(bool updateWhenEmpty) {
 void Tachometer::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Tachometer");
   builder.AddDoubleProperty(
-      "RPS", [&] { return GetRevolutionsPerSecond().to<double>(); }, nullptr);
+      "RPS", [&] { return GetRevolutionsPerSecond().value(); }, nullptr);
   builder.AddDoubleProperty(
-      "RPM", [&] { return GetRevolutionsPerMinute().to<double>(); }, nullptr);
+      "RPM", [&] { return GetRevolutionsPerMinute().value(); }, nullptr);
 }

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

@@ -58,7 +58,10 @@ class PowerDistribution : public wpi::Sendable,
   double GetVoltage() const;
 
   /**
-   * Query the temperature of the PDP/PDH.
+   * Query the temperature of the PDP.
+   *
+   * Not supported on the Rev PDH and returns 0.
+   *
    *
    * @return The temperature in degrees Celsius
    */
@@ -80,21 +83,27 @@ class PowerDistribution : public wpi::Sendable,
   double GetTotalCurrent() const;
 
   /**
-   * Query the total power drawn from all monitored PDP/PDH channels.
+   * Query the total power drawn from all monitored PDP channels.
+   *
+   * Not supported on the Rev PDH and returns 0.
    *
    * @return The total power drawn in Watts
    */
   double GetTotalPower() const;
 
   /**
-   * Query the total energy drawn from the monitored PDP/PDH channels.
+   * Query the total energy drawn from the monitored PDP channels.
+   *
+   * Not supported on the Rev PDH and returns 0.
    *
    * @return The total energy drawn in Joules
    */
   double GetTotalEnergy() const;
 
   /**
-   * Reset the total energy drawn from the PDP/PDH.
+   * Reset the total energy drawn from the PDP.
+   *
+   * Not supported on the Rev PDH and does nothing.
    *
    * @see PowerDistribution#GetTotalEnergy
    */

wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.h

@@ -36,13 +36,12 @@ class SendableChooser : public SendableChooserBase {
   template <class U>
   static U _unwrap_smart_ptr(const U& value);
 
-  template <class U>
-  static U* _unwrap_smart_ptr(const std::unique_ptr<U>& value);
-
   template <class U>
   static std::weak_ptr<U> _unwrap_smart_ptr(const std::shared_ptr<U>& value);
 
  public:
+  using CopyType = decltype(_unwrap_smart_ptr(m_choices.lookup("")));
+
   SendableChooser() = default;
   ~SendableChooser() override = default;
   SendableChooser(SendableChooser&& rhs) = default;
@@ -71,8 +70,8 @@ class SendableChooser : public SendableChooserBase {
   void SetDefaultOption(std::string_view name, T object);
 
   /**
-   * Returns a copy of the selected option (a raw pointer U* if T =
-   * std::unique_ptr<U> or a std::weak_ptr<U> if T = std::shared_ptr<U>).
+   * Returns a copy of the selected option (a std::weak_ptr<U> if T =
+   * std::shared_ptr<U>).
    *
    * If there is none selected, it will return the default. If there is none
    * selected and no default, then it will return a value-initialized instance.
@@ -81,7 +80,7 @@ class SendableChooser : public SendableChooserBase {
    *
    * @return The option selected
    */
-  auto GetSelected() -> decltype(_unwrap_smart_ptr(m_choices[""]));
+  CopyType GetSelected() const;
 
   /**
    * Bind a listener that's called when the selected value changes.

wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.inc

@@ -33,8 +33,7 @@ void SendableChooser<T>::SetDefaultOption(std::string_view name, T object) {
 
 template <class T>
   requires std::copy_constructible<T> && std::default_initializable<T>
-auto SendableChooser<T>::GetSelected()
-    -> decltype(_unwrap_smart_ptr(m_choices[""])) {
+typename SendableChooser<T>::CopyType SendableChooser<T>::GetSelected() const {
   std::string selected = m_defaultChoice;
   {
     std::scoped_lock lock(m_mutex);
@@ -43,9 +42,9 @@ auto SendableChooser<T>::GetSelected()
     }
   }
   if (selected.empty()) {
-    return decltype(_unwrap_smart_ptr(m_choices[""])){};
+    return CopyType{};
   } else {
-    return _unwrap_smart_ptr(m_choices[selected]);
+    return _unwrap_smart_ptr(m_choices.lookup(selected));
   }
 }
 
@@ -121,13 +120,6 @@ U SendableChooser<T>::_unwrap_smart_ptr(const U& value) {
   return value;
 }
 
-template <class T>
-  requires std::copy_constructible<T> && std::default_initializable<T>
-template <class U>
-U* SendableChooser<T>::_unwrap_smart_ptr(const std::unique_ptr<U>& value) {
-  return value.get();
-}
-
 template <class T>
   requires std::copy_constructible<T> && std::default_initializable<T>
 template <class U>

wpilibc/src/main/native/include/frc/smartdashboard/SendableChooserBase.h

@@ -38,7 +38,7 @@ class SendableChooserBase : public wpi::Sendable,
   std::string m_defaultChoice;
   std::string m_selected;
   bool m_haveSelected = false;
-  wpi::mutex m_mutex;
+  mutable wpi::mutex m_mutex;
   int m_instance;
   std::string m_previousVal;
   static std::atomic_int s_instances;

wpilibc/src/main/native/include/frc/sysid/SysIdRoutineLog.h

@@ -194,7 +194,7 @@ class SysIdRoutineLog {
  private:
   LogEntries m_logEntries;
   std::string m_logName;
-  bool m_stateInitialized;
+  bool m_stateInitialized = false;
   wpi::log::StringLogEntry m_state;
 };
 }  // namespace frc::sysid

wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h

@@ -131,7 +131,7 @@ class Drivetrain {
   nt::DoubleArrayEntry& m_cameraToObjectEntryRef = m_cameraToObjectEntry;
 
   frc::AprilTagFieldLayout m_aprilTagFieldLayout{
-      frc::LoadAprilTagLayoutField(frc::AprilTagField::k2022RapidReact)};
+      frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo)};
   frc::Pose3d m_objectInField{m_aprilTagFieldLayout.GetTagPose(0).value()};
 
   frc::PWMSparkMax m_leftLeader{1};

wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h

@@ -44,7 +44,6 @@ class Elevator : public frc2::PIDSubsystem {
 
  private:
   frc::PWMSparkMax m_motor{5};
-  double m_setpoint = 0;
 
 // Conversion value of potentiometer varies between the real world and
 // simulation

wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h

@@ -41,7 +41,6 @@ class Wrist : public frc2::PIDSubsystem {
 
  private:
   frc::PWMSparkMax m_motor{6};
-  double m_setpoint = 0;
 
 // Conversion value of potentiometer varies between the real world and
 // simulation

wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp

@@ -111,8 +111,8 @@ frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
   // command, then stop at the end.
   return frc2::cmd::Sequence(
       frc2::InstantCommand(
-          [this, &exampleTrajectory]() {
-            m_drive.ResetOdometry(exampleTrajectory.InitialPose());
+          [this, initialPose = exampleTrajectory.InitialPose()]() {
+            m_drive.ResetOdometry(initialPose);
           },
           {})
           .ToPtr(),

wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp

@@ -84,8 +84,8 @@ frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
   // Reset odometry to the initial pose of the trajectory, run path following
   // command, then stop at the end.
   return frc2::cmd::RunOnce(
-             [this, &exampleTrajectory] {
-               m_drive.ResetOdometry(exampleTrajectory.InitialPose());
+             [this, initialPose = exampleTrajectory.InitialPose()] {
+               m_drive.ResetOdometry(initialPose);
              },
              {})
       .AndThen(std::move(ramseteCommand))

wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp

@@ -91,7 +91,6 @@ frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() {
 }
 
 void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
-  ResetEncoders();
   m_odometry.ResetPosition(m_gyro.GetRotation2d(),
                            units::meter_t{m_leftEncoder.GetDistance()},
                            units::meter_t{m_rightEncoder.GetDistance()}, pose);

wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/RapidReactCommandBot.cpp

@@ -12,16 +12,13 @@
 
 void RapidReactCommandBot::ConfigureBindings() {
   // Automatically run the storage motor whenever the ball storage is not full,
-  // and turn it off whenever it fills.
-  frc2::Trigger([this] {
-    return m_storage.IsFull();
-  }).WhileFalse(m_storage.RunCommand());
+  // and turn it off whenever it fills. Uses subsystem-hosted trigger to
+  // improve readability and make inter-subsystem communication easier.
+  m_storage.HasCargo.WhileFalse(m_storage.RunCommand());
 
   // Automatically disable and retract the intake whenever the ball storage is
   // full.
-  frc2::Trigger([this] {
-    return m_storage.IsFull();
-  }).OnTrue(m_intake.RetractCommand());
+  m_storage.HasCargo.OnTrue(m_intake.RetractCommand());
 
   // Control the drive with split-stick arcade controls
   m_drive.SetDefaultCommand(m_drive.ArcadeDriveCommand(

wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Storage.cpp

@@ -12,7 +12,3 @@ Storage::Storage() {
 frc2::CommandPtr Storage::RunCommand() {
   return Run([this] { m_motor.Set(1.0); }).WithName("Run");
 }
-
-bool Storage::IsFull() const {
-  return m_ballSensor.Get();
-}

wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Storage.h

@@ -8,6 +8,7 @@
 #include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc2/command/CommandPtr.h>
 #include <frc2/command/SubsystemBase.h>
+#include <frc2/command/button/Trigger.h>
 
 #include "Constants.h"
 
@@ -19,7 +20,7 @@ class Storage : frc2::SubsystemBase {
   frc2::CommandPtr RunCommand();
 
   /** Whether the ball storage is full. */
-  bool IsFull() const;
+  frc2::Trigger HasCargo{[this] { return m_ballSensor.Get(); }};
 
  private:
   frc::PWMSparkMax m_motor{StorageConstants::kMotorPort};

wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp

@@ -30,8 +30,6 @@ void Drivetrain::UpdateOdometry() {
 }
 
 void Drivetrain::ResetOdometry(const frc::Pose2d& pose) {
-  m_leftEncoder.Reset();
-  m_rightEncoder.Reset();
   m_drivetrainSimulator.SetPose(pose);
   m_odometry.ResetPosition(m_gyro.GetRotation2d(),
                            units::meter_t{m_leftEncoder.GetDistance()},

wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp

@@ -112,7 +112,6 @@ frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() {
 }
 
 void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
-  ResetEncoders();
   m_drivetrainSimulator.SetPose(pose);
   m_odometry.ResetPosition(m_gyro.GetRotation2d(),
                            units::meter_t{m_leftEncoder.GetDistance()},

wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp

@@ -93,8 +93,8 @@ frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
   // command, then stop at the end.
   return frc2::cmd::Sequence(
       frc2::InstantCommand(
-          [this, &exampleTrajectory]() {
-            m_drive.ResetOdometry(exampleTrajectory.InitialPose());
+          [this, initialPose = exampleTrajectory.InitialPose()]() {
+            m_drive.ResetOdometry(initialPose);
           },
           {})
           .ToPtr(),

wpilibcExamples/src/main/cpp/examples/SysId/cpp/SysIdRoutineBot.cpp

@@ -15,14 +15,28 @@ void SysIdRoutineBot::ConfigureBindings() {
       [this] { return -m_driverController.GetLeftY(); },
       [this] { return -m_driverController.GetRightX(); }));
 
-  m_driverController.A().WhileTrue(
-      m_drive.SysIdQuasistatic(frc2::sysid::Direction::kForward));
-  m_driverController.B().WhileTrue(
-      m_drive.SysIdQuasistatic(frc2::sysid::Direction::kReverse));
-  m_driverController.X().WhileTrue(
-      m_drive.SysIdDynamic(frc2::sysid::Direction::kForward));
-  m_driverController.Y().WhileTrue(
-      m_drive.SysIdDynamic(frc2::sysid::Direction::kReverse));
+  // Using bumpers as a modifier and combining it with the buttons so that we
+  // can have both sets of bindings at once
+  (m_driverController.A() && m_driverController.RightBumper())
+      .WhileTrue(m_drive.SysIdQuasistatic(frc2::sysid::Direction::kForward));
+  (m_driverController.B() && m_driverController.RightBumper())
+      .WhileTrue(m_drive.SysIdQuasistatic(frc2::sysid::Direction::kReverse));
+  (m_driverController.X() && m_driverController.RightBumper())
+      .WhileTrue(m_drive.SysIdDynamic(frc2::sysid::Direction::kForward));
+  (m_driverController.Y() && m_driverController.RightBumper())
+      .WhileTrue(m_drive.SysIdDynamic(frc2::sysid::Direction::kReverse));
+
+  m_shooter.SetDefaultCommand(m_shooter.RunShooterCommand(
+      [this] { return m_driverController.GetLeftTriggerAxis(); }));
+
+  (m_driverController.A() && m_driverController.LeftBumper())
+      .WhileTrue(m_shooter.SysIdQuasistatic(frc2::sysid::Direction::kForward));
+  (m_driverController.B() && m_driverController.LeftBumper())
+      .WhileTrue(m_shooter.SysIdQuasistatic(frc2::sysid::Direction::kReverse));
+  (m_driverController.X() && m_driverController.LeftBumper())
+      .WhileTrue(m_shooter.SysIdDynamic(frc2::sysid::Direction::kForward));
+  (m_driverController.Y() && m_driverController.LeftBumper())
+      .WhileTrue(m_shooter.SysIdDynamic(frc2::sysid::Direction::kReverse));
 }
 
 frc2::CommandPtr SysIdRoutineBot::GetAutonomousCommand() {

wpilibcExamples/src/main/cpp/examples/SysId/cpp/subsystems/Shooter.cpp

@@ -0,0 +1,37 @@
+// 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 "subsystems/Shooter.h"
+
+#include <frc2/command/Commands.h>
+#include <units/angle.h>
+#include <units/voltage.h>
+
+Shooter::Shooter() {
+  m_shooterEncoder.SetDistancePerPulse(
+      constants::shooter::kEncoderDistancePerPulse.value());
+}
+
+frc2::CommandPtr Shooter::RunShooterCommand(
+    std::function<double()> shooterSpeed) {
+  return frc2::cmd::Run(
+             [this, shooterSpeed] {
+               m_shooterMotor.SetVoltage(
+                   units::volt_t{m_shooterFeedback.Calculate(
+                       m_shooterEncoder.GetRate(), shooterSpeed())} +
+                   m_shooterFeedforward.Calculate(
+                       units::turns_per_second_t{shooterSpeed()}));
+               m_feederMotor.Set(constants::shooter::kFeederSpeed);
+             },
+             {this})
+      .WithName("Set Shooter Speed");
+}
+
+frc2::CommandPtr Shooter::SysIdQuasistatic(frc2::sysid::Direction direction) {
+  return m_sysIdRoutine.Quasistatic(direction);
+}
+
+frc2::CommandPtr Shooter::SysIdDynamic(frc2::sysid::Direction direction) {
+  return m_sysIdRoutine.Dynamic(direction);
+}

wpilibcExamples/src/main/cpp/examples/SysId/include/Constants.h

@@ -8,6 +8,7 @@
 #include <numbers>
 
 #include <units/angle.h>
+#include <units/angular_acceleration.h>
 #include <units/angular_velocity.h>
 #include <units/length.h>
 #include <units/time.h>
@@ -38,8 +39,13 @@ using kv_unit =
                          units::inverse<units::turns>>;
 using kv_unit_t = units::unit_t<kv_unit>;
 
+using ka_unit =
+    units::compound_unit<units::volts,
+                         units::inverse<units::turns_per_second_squared>>;
+using ka_unit_t = units::unit_t<ka_unit>;
+
 inline constexpr std::array<int, 2> kEncoderPorts = {4, 5};
-inline constexpr bool kLeftEncoderReversed = false;
+inline constexpr bool kEncoderReversed = false;
 inline constexpr int kEncoderCpr = 1024;
 inline constexpr units::turn_t kEncoderDistancePerPulse =
     units::turn_t{1.0} / static_cast<double>(kEncoderCpr);
@@ -55,6 +61,7 @@ inline constexpr double kP = 1.0;
 
 inline constexpr units::volt_t kS = 0.05_V;
 inline constexpr kv_unit_t kV = (12_V) / kShooterFreeSpeed;
+inline constexpr ka_unit_t kA = 0_V * 1_s * 1_s / units::turn_t{1};
 
 inline constexpr double kFeederSpeed = 0.5;
 }  // namespace shooter

wpilibcExamples/src/main/cpp/examples/SysId/include/SysIdRoutineBot.h

@@ -9,6 +9,7 @@
 
 #include "Constants.h"
 #include "subsystems/Drive.h"
+#include "subsystems/Shooter.h"
 
 class SysIdRoutineBot {
  public:
@@ -21,4 +22,5 @@ class SysIdRoutineBot {
   frc2::CommandXboxController m_driverController{
       constants::oi::kDriverControllerPort};
   Drive m_drive;
+  Shooter m_shooter;
 };

wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Shooter.h

@@ -0,0 +1,54 @@
+// 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 <functional>
+
+#include <frc/Encoder.h>
+#include <frc/RobotController.h>
+#include <frc/controller/PIDController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <frc2/command/SubsystemBase.h>
+#include <frc2/command/sysid/SysIdRoutine.h>
+
+#include "Constants.h"
+
+class Shooter : public frc2::SubsystemBase {
+ public:
+  Shooter();
+
+  frc2::CommandPtr RunShooterCommand(std::function<double()> shooterSpeed);
+  frc2::CommandPtr SysIdQuasistatic(frc2::sysid::Direction direction);
+  frc2::CommandPtr SysIdDynamic(frc2::sysid::Direction direction);
+
+ private:
+  frc::PWMSparkMax m_shooterMotor{constants::shooter::kShooterMotorPort};
+  frc::PWMSparkMax m_feederMotor{constants::shooter::kFeederMotorPort};
+
+  frc::Encoder m_shooterEncoder{constants::shooter::kEncoderPorts[0],
+                                constants::shooter::kEncoderPorts[1],
+                                constants::shooter::kEncoderReversed};
+
+  frc2::sysid::SysIdRoutine m_sysIdRoutine{
+      frc2::sysid::Config{std::nullopt, std::nullopt, std::nullopt,
+                          std::nullopt},
+      frc2::sysid::Mechanism{
+          [this](units::volt_t driveVoltage) {
+            m_shooterMotor.SetVoltage(driveVoltage);
+          },
+          [this](frc::sysid::SysIdRoutineLog* log) {
+            log->Motor("shooter-wheel")
+                .voltage(m_shooterMotor.Get() *
+                         frc::RobotController::GetBatteryVoltage())
+                .position(units::turn_t{m_shooterEncoder.GetDistance()})
+                .velocity(
+                    units::turns_per_second_t{m_shooterEncoder.GetRate()});
+          },
+          this}};
+  frc::PIDController m_shooterFeedback{constants::shooter::kP, 0, 0};
+  frc::SimpleMotorFeedforward<units::turns> m_shooterFeedforward{
+      constants::shooter::kS, constants::shooter::kV, constants::shooter::kA};
+};

wpilibcExamples/src/test/cpp/examples/MecanumControllerCommand/cpp/MecanumControllerCommandTest.cpp

@@ -0,0 +1,64 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <thread>
+
+#include <frc/simulation/DriverStationSim.h>
+#include <frc/simulation/SimHooks.h>
+#include <gtest/gtest.h>
+#include <units/time.h>
+
+#include "Robot.h"
+
+class MecanumControllerCommandTest : public testing::Test {
+  Robot m_robot;
+  std::optional<std::thread> m_thread;
+  bool joystickWarning;
+
+ public:
+  void SetUp() override {
+    frc::sim::PauseTiming();
+    joystickWarning = frc::DriverStation::IsJoystickConnectionWarningSilenced();
+    frc::DriverStation::SilenceJoystickConnectionWarning(true);
+
+    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();
+
+    frc::sim::DriverStationSim::ResetData();
+    frc::DriverStation::SilenceJoystickConnectionWarning(joystickWarning);
+  }
+};
+
+TEST_F(MecanumControllerCommandTest, Match) {
+  // auto
+  frc::sim::DriverStationSim::SetAutonomous(true);
+  frc::sim::DriverStationSim::SetEnabled(true);
+  frc::sim::DriverStationSim::NotifyNewData();
+
+  frc::sim::StepTiming(15_s);
+
+  // brief disabled period- exact duration shouldn't matter
+  frc::sim::DriverStationSim::SetAutonomous(false);
+  frc::sim::DriverStationSim::SetEnabled(false);
+  frc::sim::DriverStationSim::NotifyNewData();
+
+  frc::sim::StepTiming(3_s);
+
+  // teleop
+  frc::sim::DriverStationSim::SetAutonomous(false);
+  frc::sim::DriverStationSim::SetEnabled(true);
+  frc::sim::DriverStationSim::NotifyNewData();
+
+  frc::sim::StepTiming(135_s);
+
+  // end of match
+  frc::sim::DriverStationSim::SetAutonomous(false);
+  frc::sim::DriverStationSim::SetEnabled(false);
+  frc::sim::DriverStationSim::NotifyNewData();
+}

wpilibcExamples/src/test/cpp/examples/MecanumControllerCommand/cpp/main.cpp

@@ -0,0 +1,16 @@
+// 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 <hal/HALBase.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/RamseteCommand/cpp/RamseteCommandTest.cpp

@@ -0,0 +1,64 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <thread>
+
+#include <frc/simulation/DriverStationSim.h>
+#include <frc/simulation/SimHooks.h>
+#include <gtest/gtest.h>
+#include <units/time.h>
+
+#include "Robot.h"
+
+class MecanumControllerCommandTest : public testing::Test {
+  Robot m_robot;
+  std::optional<std::thread> m_thread;
+  bool joystickWarning;
+
+ public:
+  void SetUp() override {
+    frc::sim::PauseTiming();
+    joystickWarning = frc::DriverStation::IsJoystickConnectionWarningSilenced();
+    frc::DriverStation::SilenceJoystickConnectionWarning(true);
+
+    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();
+
+    frc::sim::DriverStationSim::ResetData();
+    frc::DriverStation::SilenceJoystickConnectionWarning(joystickWarning);
+  }
+};
+
+TEST_F(MecanumControllerCommandTest, Match) {
+  // auto
+  frc::sim::DriverStationSim::SetAutonomous(true);
+  frc::sim::DriverStationSim::SetEnabled(true);
+  frc::sim::DriverStationSim::NotifyNewData();
+
+  frc::sim::StepTiming(15_s);
+
+  // brief disabled period- exact duration shouldn't matter
+  frc::sim::DriverStationSim::SetAutonomous(false);
+  frc::sim::DriverStationSim::SetEnabled(false);
+  frc::sim::DriverStationSim::NotifyNewData();
+
+  frc::sim::StepTiming(3_s);
+
+  // teleop
+  frc::sim::DriverStationSim::SetAutonomous(false);
+  frc::sim::DriverStationSim::SetEnabled(true);
+  frc::sim::DriverStationSim::NotifyNewData();
+
+  frc::sim::StepTiming(135_s);
+
+  // end of match
+  frc::sim::DriverStationSim::SetAutonomous(false);
+  frc::sim::DriverStationSim::SetEnabled(false);
+  frc::sim::DriverStationSim::NotifyNewData();
+}

wpilibcExamples/src/test/cpp/examples/RamseteCommand/cpp/main.cpp

@@ -0,0 +1,16 @@
+// 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 <hal/HALBase.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/SwerveControllerCommand/cpp/SwerveControllerCommandTest.cpp

@@ -0,0 +1,69 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <iostream>
+#include <thread>
+
+#include <frc/simulation/DriverStationSim.h>
+#include <frc/simulation/SimHooks.h>
+#include <gtest/gtest.h>
+#include <units/time.h>
+
+#include "Robot.h"
+
+class SwerveControllerCommandTest : public testing::Test {
+  Robot m_robot;
+  std::optional<std::thread> m_thread;
+  bool joystickWarning;
+
+ public:
+  void SetUp() override {
+    frc::sim::PauseTiming();
+    joystickWarning = frc::DriverStation::IsJoystickConnectionWarningSilenced();
+    frc::DriverStation::SilenceJoystickConnectionWarning(true);
+
+    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();
+
+    frc::sim::DriverStationSim::ResetData();
+    frc::DriverStation::SilenceJoystickConnectionWarning(joystickWarning);
+  }
+};
+
+TEST_F(SwerveControllerCommandTest, Match) {
+  std::cerr << "autonomous" << std::endl;
+  // auto
+  frc::sim::DriverStationSim::SetAutonomous(true);
+  frc::sim::DriverStationSim::SetEnabled(true);
+  frc::sim::DriverStationSim::NotifyNewData();
+
+  frc::sim::StepTiming(15_s);
+
+  // brief disabled period- exact duration shouldn't matter
+  std::cerr << "mid disabled" << std::endl;
+  frc::sim::DriverStationSim::SetAutonomous(false);
+  frc::sim::DriverStationSim::SetEnabled(false);
+  frc::sim::DriverStationSim::NotifyNewData();
+
+  frc::sim::StepTiming(3_s);
+
+  // teleop
+  std::cerr << "teleop" << std::endl;
+  frc::sim::DriverStationSim::SetAutonomous(false);
+  frc::sim::DriverStationSim::SetEnabled(true);
+  frc::sim::DriverStationSim::NotifyNewData();
+
+  frc::sim::StepTiming(135_s);
+
+  // end of match
+  std::cerr << "end of match" << std::endl;
+  frc::sim::DriverStationSim::SetAutonomous(false);
+  frc::sim::DriverStationSim::SetEnabled(false);
+  frc::sim::DriverStationSim::NotifyNewData();
+}

wpilibcExamples/src/test/cpp/examples/SwerveControllerCommand/cpp/main.cpp

@@ -0,0 +1,16 @@
+// 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 <hal/HALBase.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/AddressableLEDBuffer.java

@@ -131,8 +131,7 @@ public class AddressableLEDBuffer {
    * @return the LED color at the specified index
    */
   public Color8Bit getLED8Bit(int index) {
-    return new Color8Bit(
-        m_buffer[index * 4 + 2] & 0xFF, m_buffer[index * 4 + 1] & 0xFF, m_buffer[index * 4] & 0xFF);
+    return new Color8Bit(getRed(index), getGreen(index), getBlue(index));
   }
 
   /**
@@ -142,9 +141,70 @@ public class AddressableLEDBuffer {
    * @return the LED color at the specified index
    */
   public Color getLED(int index) {
-    return new Color(
-        (m_buffer[index * 4 + 2] & 0xFF) / 255.0,
-        (m_buffer[index * 4 + 1] & 0xFF) / 255.0,
-        (m_buffer[index * 4] & 0xFF) / 255.0);
+    return new Color(getRed(index) / 255.0, getGreen(index) / 255.0, getBlue(index) / 255.0);
+  }
+
+  /**
+   * Gets the red channel of the color at the specified index.
+   *
+   * @param index the index of the LED to read
+   * @return the value of the red channel, from [0, 255]
+   */
+  public int getRed(int index) {
+    return m_buffer[index * 4 + 2] & 0xFF;
+  }
+
+  /**
+   * Gets the green channel of the color at the specified index.
+   *
+   * @param index the index of the LED to read
+   * @return the value of the green channel, from [0, 255]
+   */
+  public int getGreen(int index) {
+    return m_buffer[index * 4 + 1] & 0xFF;
+  }
+
+  /**
+   * Gets the blue channel of the color at the specified index.
+   *
+   * @param index the index of the LED to read
+   * @return the value of the blue channel, from [0, 255]
+   */
+  public int getBlue(int index) {
+    return m_buffer[index * 4] & 0xFF;
+  }
+
+  /**
+   * A functional interface that allows for iteration over an LED buffer without manually writing an
+   * indexed for-loop.
+   */
+  @FunctionalInterface
+  public interface IndexedColorIterator {
+    /**
+     * Accepts an index of an LED in the buffer and the red, green, and blue components of the
+     * currently stored color for that LED.
+     *
+     * @param index the index of the LED in the buffer that the red, green, and blue channels
+     *     corresponds to
+     * @param r the value of the red channel of the color currently in the buffer at index {@code i}
+     * @param g the value of the green channel of the color currently in the buffer at index {@code
+     *     i}
+     * @param b the value of the blue channel of the color currently in the buffer at index {@code
+     *     i}
+     */
+    void accept(int index, int r, int g, int b);
+  }
+
+  /**
+   * Iterates over the LEDs in the buffer, starting from index 0. The iterator function is passed
+   * the current index of iteration, along with the values for the red, green, and blue components
+   * of the color written to the LED at that index.
+   *
+   * @param iterator the iterator function to call for each LED in the buffer.
+   */
+  public void forEach(IndexedColorIterator iterator) {
+    for (int i = 0; i < getLength(); i++) {
+      iterator.accept(i, getRed(i), getGreen(i), getBlue(i));
+    }
   }
 }

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

@@ -93,7 +93,9 @@ public class PowerDistribution implements Sendable, AutoCloseable {
   }
 
   /**
-   * Query the temperature of the PDP/PDH.
+   * Query the temperature of the PDP.
+   *
+   * <p>Not supported on the Rev PDH and returns 0.
    *
    * @return The temperature in degrees Celsius
    */
@@ -121,7 +123,9 @@ public class PowerDistribution implements Sendable, AutoCloseable {
   }
 
   /**
-   * Query the total power drawn from the monitored channels.
+   * Query the total power drawn from the monitored channels of the PDP.
+   *
+   * <p>Not supported on the Rev PDH and returns 0.
    *
    * @return the total power in Watts
    */
@@ -130,7 +134,9 @@ public class PowerDistribution implements Sendable, AutoCloseable {
   }
 
   /**
-   * Query the total energy drawn from the monitored channels.
+   * Query the total energy drawn from the monitored channels of the PDP.
+   *
+   * <p>Not supported on the Rev PDH and returns 0.
    *
    * @return the total energy in Joules
    */
@@ -138,7 +144,11 @@ public class PowerDistribution implements Sendable, AutoCloseable {
     return PowerDistributionJNI.getTotalEnergy(m_handle);
   }
 
-  /** Reset the total energy to 0. */
+  /**
+   * Reset the total energy to 0 of the PDP.
+   *
+   * <p>Not supported on the Rev PDH and does nothing.
+   */
   public void resetTotalEnergy() {
     PowerDistributionJNI.resetTotalEnergy(m_handle);
   }

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

@@ -453,6 +453,11 @@ public abstract class RobotBase implements AutoCloseable {
       runRobot(robotSupplier);
     }
 
+    // On RIO, this will just terminate rather than shutting down cleanly (it's a no-op in sim).
+    // It's not worth the risk of hanging on shutdown when we want the code to restart as quickly
+    // as possible.
+    HAL.terminate();
+
     HAL.shutdown();
 
     System.exit(0);

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

@@ -6,6 +6,7 @@ package edu.wpi.first.wpilibj;
 
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.fail;
 import static org.junit.jupiter.params.provider.Arguments.arguments;
 
 import edu.wpi.first.wpilibj.util.Color;
@@ -70,4 +71,73 @@ class AddressableLEDBufferTest {
     assertEquals(firstRedColor8Bit, buffer.getLED8Bit(2));
     assertEquals(firstBlueColor8Bit, buffer.getLED8Bit(3));
   }
+
+  @Test
+  void getRed() {
+    var buffer = new AddressableLEDBuffer(1);
+    buffer.setRGB(0, 127, 128, 129);
+    assertEquals(127, buffer.getRed(0));
+  }
+
+  @Test
+  void getGreen() {
+    var buffer = new AddressableLEDBuffer(1);
+    buffer.setRGB(0, 127, 128, 129);
+    assertEquals(128, buffer.getGreen(0));
+  }
+
+  @Test
+  void getBlue() {
+    var buffer = new AddressableLEDBuffer(1);
+    buffer.setRGB(0, 127, 128, 129);
+    assertEquals(129, buffer.getBlue(0));
+  }
+
+  @Test
+  void forEach() {
+    var buffer = new AddressableLEDBuffer(3);
+    buffer.setRGB(0, 1, 2, 3);
+    buffer.setRGB(1, 4, 5, 6);
+    buffer.setRGB(2, 7, 8, 9);
+
+    buffer.forEach(
+        (index, r, g, b) -> {
+          switch (index) {
+            case 0:
+              {
+                assertAll(
+                    () -> assertEquals(1, r, "red at index 0"),
+                    () -> assertEquals(2, g, "green at index 0"),
+                    () -> assertEquals(3, b, "blue at index 0"));
+              }
+              break;
+            case 1:
+              {
+                assertAll(
+                    () -> assertEquals(4, r, "red at index 1"),
+                    () -> assertEquals(5, g, "green at index 1"),
+                    () -> assertEquals(6, b, "blue at index 1"));
+              }
+              break;
+            case 2:
+              {
+                assertAll(
+                    () -> assertEquals(7, r, "red at index 2"),
+                    () -> assertEquals(8, g, "green at index 2"),
+                    () -> assertEquals(9, b, "blue at index 2"));
+              }
+              break;
+            default:
+              fail("Unexpected index " + index);
+              break;
+          }
+        });
+  }
+
+  @Test
+  void forEachOnEmptyBuffer() {
+    var buffer = new AddressableLEDBuffer(0);
+
+    buffer.forEach((i, r, g, b) -> fail("Iterator should not be called on an empty buffer"));
+  }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator;
 
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.ComputerVisionUtil;
 import edu.wpi.first.math.VecBuilder;
@@ -125,7 +126,8 @@ public class Drivetrain {
 
     m_cameraToObjectEntry = cameraToObjectTopic.getEntry(m_defaultVal);
 
-    m_objectInField = AprilTagFields.k2022RapidReact.loadAprilTagLayoutField().getTagPose(0).get();
+    m_objectInField =
+        AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo).getTagPose(0).get();
 
     SmartDashboard.putData("Field", m_fieldSim);
     SmartDashboard.putData("FieldEstimation", m_fieldApproximation);

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java

@@ -102,7 +102,6 @@ public class DriveSubsystem extends SubsystemBase {
    * @param pose The pose to which to set the odometry.
    */
   public void resetOdometry(Pose2d pose) {
-    resetEncoders();
     m_odometry.resetPosition(
         m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance(), pose);
   }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/RapidReactCommandBot.java

@@ -46,11 +46,12 @@ public class RapidReactCommandBot {
    */
   public void configureBindings() {
     // Automatically run the storage motor whenever the ball storage is not full,
-    // and turn it off whenever it fills.
-    new Trigger(m_storage::isFull).whileFalse(m_storage.runCommand());
+    // and turn it off whenever it fills. Uses subsystem-hosted trigger to
+    // improve readability and make inter-subsystem communication easier.
+    m_storage.hasCargo.whileFalse(m_storage.runCommand());
 
     // Automatically disable and retract the intake whenever the ball storage is full.
-    new Trigger(m_storage::isFull).onTrue(m_intake.retractCommand());
+    m_storage.hasCargo.onTrue(m_intake.retractCommand());
 
     // Control the drive with split-stick arcade controls
     m_drive.setDefaultCommand(

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Storage.java

@@ -4,28 +4,29 @@
 
 package edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems;
 
-import static edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.StorageConstants;
-
 import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.StorageConstants;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 
 public class Storage extends SubsystemBase {
   private final PWMSparkMax m_motor = new PWMSparkMax(StorageConstants.kMotorPort);
   private final DigitalInput m_ballSensor = new DigitalInput(StorageConstants.kBallSensorPort);
 
+  // Expose trigger from subsystem to improve readability and ease
+  // inter-subsystem communications
+  /** Whether the ball storage is full. */
+  @SuppressWarnings("checkstyle:MemberName")
+  public final Trigger hasCargo = new Trigger(m_ballSensor::get);
+
   /** Create a new Storage subsystem. */
   public Storage() {
     // Set default command to turn off the storage motor and then idle
     setDefaultCommand(runOnce(m_motor::disable).andThen(run(() -> {})).withName("Idle"));
   }
 
-  /** Whether the ball storage is full. */
-  public boolean isFull() {
-    return m_ballSensor.get();
-  }
-
   /** Returns a command that runs the storage motor indefinitely. */
   public Command runCommand() {
     return run(() -> m_motor.set(1)).withName("run");