[hal, wpilib] Usage Reporting: QFRCDashboard -> QDash (#8571)

Fixes #8570.

.github/workflows/cmake-android.yml

@@ -30,7 +30,7 @@ jobs:
       - uses: nttld/setup-ndk@v1
         id: setup-ndk
         with:
-          ndk-version: r27c
+          ndk-version: r27d
           add-to-path: false
 
       - uses: actions/setup-java@v4

.github/workflows/gradle.yml

@@ -52,11 +52,11 @@ jobs:
         run: echo "EXTRA_GRADLE_ARGS=-PreleaseMode" >> $GITHUB_ENV
         if: startsWith(github.ref, 'refs/tags/v') && !contains(github.ref, '2027')
       - name: Build with Gradle
-        uses: addnab/docker-run-action@v3
+        uses: addnab/docker-run-action@3e77f186b7a929ef010f183a9e24c0f9955ea609
         with:
           image: ${{ matrix.container }}
           options: -v ${{ github.workspace }}:/work -w /work -e ARTIFACTORY_PUBLISH_USERNAME -e ARTIFACTORY_PUBLISH_PASSWORD -e GITHUB_REF -e CI
-          run: df . && rm -f semicolon_delimited_script && echo $GITHUB_REF && ./gradlew build --build-cache -PbuildServer -PskipJavaFormat ${{ matrix.build-options }} ${{ env.EXTRA_GRADLE_ARGS }}
+          run: df . && echo $GITHUB_REF && ./gradlew build --build-cache -PbuildServer -PskipJavaFormat ${{ matrix.build-options }} ${{ env.EXTRA_GRADLE_ARGS }}
         env:
           ARTIFACTORY_PUBLISH_USERNAME: ${{ secrets.ARTIFACTORY_USERNAME }}
           ARTIFACTORY_PUBLISH_PASSWORD: ${{ secrets.ARTIFACTORY_PASSWORD }}

.github/workflows/sentinel-build.yml

@@ -53,11 +53,11 @@ jobs:
         with:
           fetch-depth: 0
       - name: Build with Gradle
-        uses: addnab/docker-run-action@v3
+        uses: addnab/docker-run-action@3e77f186b7a929ef010f183a9e24c0f9955ea609
         with:
           image: ${{ matrix.container }}
           options: -v ${{ github.workspace }}:/work -w /work -e GITHUB_REF -e CI
-          run: df . && rm -f semicolon_delimited_script && echo $GITHUB_REF && ./gradlew build -PbuildServer -PskipJavaFormat ${{ matrix.build-options }}
+          run: df . && echo $GITHUB_REF && ./gradlew build -PbuildServer -PskipJavaFormat ${{ matrix.build-options }}
       - name: Check free disk space
         run: df .
       - uses: actions/upload-artifact@v4

CMakeLists.txt

@@ -351,7 +351,9 @@ endif()
 if(WITH_WPILIB)
     set(APRILTAG_DEP_REPLACE "find_dependency(apriltag)")
     set(WPILIBC_DEP_REPLACE "find_dependency(wpilibc)")
-    set(WPILIBJ_DEP_REPLACE "find_dependency(wpilibj)")
+    if(WITH_JAVA)
+        set(WPILIBJ_DEP_REPLACE "find_dependency(wpilibj)")
+    endif()
     set(WPILIBNEWCOMMANDS_DEP_REPLACE "find_dependency(wpilibNewCommands)")
     add_subdirectory(apriltag)
     add_subdirectory(wpilibj)

DevelopmentBuilds.md

@@ -13,7 +13,7 @@ This article contains instructions on building projects using a development buil
 
 Development builds are the per-commit build hosted every time a commit is pushed to the [allwpilib](https://github.com/wpilibsuite/allwpilib/) repository. These builds are then hosted on [artifactory](https://frcmaven.wpi.edu/artifactory/webapp/#/home).
 
-To build a project using a development build, find the build.gradle file and open it. Then, add the following code below the plugin section and replace YEAR with the year of the development version. It is also necessary to use a 2025 GradleRIO version, ie `2025.1.1-beta-1`
+To build a project using a development build, find the build.gradle file and open it. Then, add the following code below the plugin section and replace YEAR with the year of the development version. It is also necessary to use a 2026 GradleRIO version, ie `2026.1.1`
 
 ```groovy
 wpi.maven.useLocal = false
@@ -28,13 +28,13 @@ Java
 ```groovy
 plugins {
   id "java"
-  id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-1"
+  id "edu.wpi.first.GradleRIO" version "2026.1.1"
 }
 
 wpi.maven.useLocal = false
 wpi.maven.useDevelopment = true
-wpi.versions.wpilibVersion = '2025.+'
-wpi.versions.wpimathVersion = '2025.+'
+wpi.versions.wpilibVersion = '2026.+'
+wpi.versions.wpimathVersion = '2026.+'
 ```
 
 C++
@@ -42,13 +42,13 @@ C++
 plugins {
   id "cpp"
   id "google-test-test-suite"
-  id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-1"
+  id "edu.wpi.first.GradleRIO" version "2026.1.1"
 }
 
 wpi.maven.useLocal = false
 wpi.maven.useDevelopment = true
-wpi.versions.wpilibVersion = '2025.+'
-wpi.versions.wpimathVersion = '2025.+'
+wpi.versions.wpilibVersion = '2026.+'
+wpi.versions.wpimathVersion = '2026.+'
 ```
 
 ### Development Build Documentation
@@ -64,7 +64,7 @@ Java
 ```groovy
 plugins {
   id "java"
-  id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-1"
+  id "edu.wpi.first.GradleRIO" version "2026.1.1"
 }
 
 wpi.maven.useLocal = false
@@ -78,7 +78,7 @@ C++
 plugins {
   id "cpp"
   id "google-test-test-suite"
-  id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-1"
+  id "edu.wpi.first.GradleRIO" version "2026.1.1"
 }
 
 wpi.maven.useLocal = false

LICENSE.md

@@ -1,4 +1,4 @@
-Copyright (c) 2009-2025 FIRST and other WPILib contributors
+Copyright (c) 2009-2026 FIRST and other WPILib contributors
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

README.md

@@ -50,7 +50,7 @@ Using Gradle makes building WPILib very straightforward. It only has a few depen
 - C++ compiler
     - On Linux, install GCC 11 or greater
     - On Windows, install [Visual Studio Community 2022](https://visualstudio.microsoft.com/vs/community/) and select the C++ programming language during installation (Gradle can't use the build tools for Visual Studio)
-    - On macOS 13.3 or newer, install Xcode 14 or later (the command-line build tools are insufficient).
+    - On macOS, install the Xcode command-line build tools via `xcode-select --install`. Xcode 14 or later is required.
 - ARM compiler toolchain
     - Run `./gradlew installRoboRioToolchain` after cloning this repository
     - If the WPILib installer was used, this toolchain is already installed

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

@@ -17,13 +17,17 @@ public enum AprilTagFields {
   /** 2025 Reefscape Welded (see TU 12). */
   k2025ReefscapeWelded("2025-reefscape-welded.json"),
   /** 2025 Reefscape AndyMark (see TU 12). */
-  k2025ReefscapeAndyMark("2025-reefscape-andymark.json");
+  k2025ReefscapeAndyMark("2025-reefscape-andymark.json"),
+  /** 2026 Rebuilt Welded. */
+  k2026RebuiltWelded("2026-rebuilt-welded.json"),
+  /** 2026 Rebuilt AndyMark. */
+  k2026RebuiltAndymark("2026-rebuilt-andymark.json");
 
   /** Base resource directory. */
   public static final String kBaseResourceDir = "/edu/wpi/first/apriltag/";
 
   /** Alias to the current game. */
-  public static final AprilTagFields kDefaultField = k2025ReefscapeWelded;
+  public static final AprilTagFields kDefaultField = k2026RebuiltWelded;
 
   /** Resource filename. */
   public final String m_resourceFile;

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

@@ -135,6 +135,8 @@ std::string_view GetResource_2023_chargedup_json();
 std::string_view GetResource_2024_crescendo_json();
 std::string_view GetResource_2025_reefscape_welded_json();
 std::string_view GetResource_2025_reefscape_andymark_json();
+std::string_view GetResource_2026_rebuilt_welded_json();
+std::string_view GetResource_2026_rebuilt_andymark_json();
 
 }  // namespace frc
 
@@ -156,6 +158,12 @@ AprilTagFieldLayout AprilTagFieldLayout::LoadField(AprilTagField field) {
     case AprilTagField::k2025ReefscapeAndyMark:
       fieldString = GetResource_2025_reefscape_andymark_json();
       break;
+    case AprilTagField::k2026RebuiltWelded:
+      fieldString = GetResource_2026_rebuilt_welded_json();
+      break;
+    case AprilTagField::k2026RebuiltAndyMark:
+      fieldString = GetResource_2026_rebuilt_andymark_json();
+      break;
     case AprilTagField::kNumFields:
       throw std::invalid_argument("Invalid Field");
   }

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

@@ -24,8 +24,12 @@ enum class AprilTagField {
   k2025ReefscapeAndyMark,
   /// 2025 Reefscape Welded (see TU12).
   k2025ReefscapeWelded,
+  /// 2026 Rebuilt Andymark.
+  k2026RebuiltAndyMark,
+  /// 2026 Rebuilt Welded.
+  k2026RebuiltWelded,
   /// Alias to the current game.
-  kDefaultField = k2025ReefscapeWelded,
+  kDefaultField = k2026RebuiltWelded,
 
   // This is a placeholder for denoting the last supported field. This should
   // always be the last entry in the enum and should not be used by users

apriltag/src/main/native/resources/edu/wpi/first/apriltag/2026-rebuilt-andymark.csv

@@ -0,0 +1,33 @@
+ID,X,Y,Z,Z-Rotation,X-Rotation
+1,467.085,291.791,35,180,0
+2,468.559,182.077,44.25,90,0
+3,444.797,172.321,44.25,180,0
+4,444.797,158.321,44.25,180,0
+5,468.559,134.565,44.25,270,0
+6,467.085,24.851,35,180,0
+7,470.034,24.851,35,0,0
+8,482.559,134.565,44.25,270,0
+9,492.329,144.321,44.25,0,0
+10,492.329,158.321,44.25,0,0
+11,482.559,182.077,44.25,90,0
+12,470.034,291.791,35,0,0
+13,649.58,291.02,21.75,180,0
+14,649.58,274.02,21.75,180,0
+15,649.566,169.783,21.75,180,0
+16,649.566,152.783,21.75,180,0
+17,183.034,24.851,35,0,0
+18,181.559,134.565,44.25,270,0
+19,205.321,144.321,44.25,0,0
+20,205.321,158.321,44.25,0,0
+21,181.559,182.077,44.25,90,0
+22,183.034,291.791,35,0,0
+23,180.085,291.791,35,180,0
+24,167.559,182.077,44.25,90,0
+25,157.79,172.321,44.25,180,0
+26,157.79,158.321,44.25,180,0
+27,167.559,134.565,44.25,270,0
+28,180.085,24.851,35,180,0
+29,0.539,25.621,21.75,0,0
+30,0.539,42.621,21.75,0,0
+31,0.553,146.858,21.75,0,0
+32,0.553,163.858,21.75,0,0

apriltag/src/main/native/resources/edu/wpi/first/apriltag/2026-rebuilt-andymark.json

@@ -0,0 +1,584 @@
+{
+  "tags": [
+    {
+      "ID": 1,
+      "pose": {
+        "translation": {
+          "x": 11.863959,
+          "y": 7.411491399999999,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 2,
+      "pose": {
+        "translation": {
+          "x": 11.9013986,
+          "y": 4.6247558,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.7071067811865476,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 3,
+      "pose": {
+        "translation": {
+          "x": 11.2978438,
+          "y": 4.3769534,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 4,
+      "pose": {
+        "translation": {
+          "x": 11.2978438,
+          "y": 4.0213534,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 5,
+      "pose": {
+        "translation": {
+          "x": 11.9013986,
+          "y": 3.417951,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 6,
+      "pose": {
+        "translation": {
+          "x": 11.863959,
+          "y": 0.6312154,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 7,
+      "pose": {
+        "translation": {
+          "x": 11.9388636,
+          "y": 0.6312154,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 8,
+      "pose": {
+        "translation": {
+          "x": 12.2569986,
+          "y": 3.417951,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 9,
+      "pose": {
+        "translation": {
+          "x": 12.5051566,
+          "y": 3.6657534,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 10,
+      "pose": {
+        "translation": {
+          "x": 12.5051566,
+          "y": 4.0213534,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 11,
+      "pose": {
+        "translation": {
+          "x": 12.2569986,
+          "y": 4.6247558,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.7071067811865476,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 12,
+      "pose": {
+        "translation": {
+          "x": 11.9388636,
+          "y": 7.411491399999999,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 13,
+      "pose": {
+        "translation": {
+          "x": 16.499332,
+          "y": 7.391907999999999,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 14,
+      "pose": {
+        "translation": {
+          "x": 16.499332,
+          "y": 6.960107999999999,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 15,
+      "pose": {
+        "translation": {
+          "x": 16.4989764,
+          "y": 4.3124882,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 16,
+      "pose": {
+        "translation": {
+          "x": 16.4989764,
+          "y": 3.8806881999999994,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 17,
+      "pose": {
+        "translation": {
+          "x": 4.6490636,
+          "y": 0.6312154,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 18,
+      "pose": {
+        "translation": {
+          "x": 4.6115986,
+          "y": 3.417951,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 19,
+      "pose": {
+        "translation": {
+          "x": 5.2151534,
+          "y": 3.6657534,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 20,
+      "pose": {
+        "translation": {
+          "x": 5.2151534,
+          "y": 4.0213534,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 21,
+      "pose": {
+        "translation": {
+          "x": 4.6115986,
+          "y": 4.6247558,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.7071067811865476,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 22,
+      "pose": {
+        "translation": {
+          "x": 4.6490636,
+          "y": 7.411491399999999,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 23,
+      "pose": {
+        "translation": {
+          "x": 4.574159,
+          "y": 7.411491399999999,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 24,
+      "pose": {
+        "translation": {
+          "x": 4.2559986,
+          "y": 4.6247558,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.7071067811865476,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 25,
+      "pose": {
+        "translation": {
+          "x": 4.007866,
+          "y": 4.3769534,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 26,
+      "pose": {
+        "translation": {
+          "x": 4.007866,
+          "y": 4.0213534,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 27,
+      "pose": {
+        "translation": {
+          "x": 4.2559986,
+          "y": 3.417951,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 28,
+      "pose": {
+        "translation": {
+          "x": 4.574159,
+          "y": 0.6312154,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 29,
+      "pose": {
+        "translation": {
+          "x": 0.0136906,
+          "y": 0.6507734,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 30,
+      "pose": {
+        "translation": {
+          "x": 0.0136906,
+          "y": 1.0825734,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 31,
+      "pose": {
+        "translation": {
+          "x": 0.0140462,
+          "y": 3.7301932,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 32,
+      "pose": {
+        "translation": {
+          "x": 0.0140462,
+          "y": 4.1619931999999995,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    }
+  ],
+  "field": {
+    "length": 16.518,
+    "width": 8.043
+  }
+}

apriltag/src/main/native/resources/edu/wpi/first/apriltag/2026-rebuilt-welded.csv

@@ -0,0 +1,33 @@
+ID,X,Y,Z,Z-Rotation,X-Rotation
+1,467.637,292.314,35,180,0
+2,469.111,182.6,44.25,90,0
+3,445.349,172.844,44.25,180,0
+4,445.349,158.844,44.25,180,0
+5,469.111,135.088,44.25,270,0
+6,467.637,25.374,35,180,0
+7,470.586,25.374,35,0,0
+8,483.111,135.088,44.25,270,0
+9,492.881,144.844,44.25,0,0
+10,492.881,158.844,44.25,0,0
+11,483.111,182.6,44.25,90,0
+12,470.586,292.314,35,0,0
+13,650.918,291.469,21.75,180,0
+14,650.918,274.469,21.75,180,0
+15,650.904,170.219,21.75,180,0
+16,650.904,153.219,21.75,180,0
+17,183.586,25.374,35,0,0
+18,182.111,135.088,44.25,270,0
+19,205.873,144.844,44.25,0,0
+20,205.873,158.844,44.25,0,0
+21,182.111,182.6,44.25,90,0
+22,183.586,292.314,35,0,0
+23,180.637,292.314,35,180,0
+24,168.111,182.6,44.25,90,0
+25,158.341,172.844,44.25,180,0
+26,158.341,158.844,44.25,180,0
+27,168.111,135.088,44.25,270,0
+28,180.637,25.374,35,180,0
+29,0.305,26.219,21.75,0,0
+30,0.305,43.219,21.75,0,0
+31,0.318,147.469,21.75,0,0
+32,0.318,164.469,21.75,0,0

apriltag/src/main/native/resources/edu/wpi/first/apriltag/2026-rebuilt-welded.json

@@ -0,0 +1,584 @@
+{
+  "tags": [
+    {
+      "ID": 1,
+      "pose": {
+        "translation": {
+          "x": 11.8779798,
+          "y": 7.4247756,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 2,
+      "pose": {
+        "translation": {
+          "x": 11.9154194,
+          "y": 4.638039999999999,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.7071067811865476,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 3,
+      "pose": {
+        "translation": {
+          "x": 11.3118646,
+          "y": 4.3902376,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 4,
+      "pose": {
+        "translation": {
+          "x": 11.3118646,
+          "y": 4.0346376,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 5,
+      "pose": {
+        "translation": {
+          "x": 11.9154194,
+          "y": 3.4312351999999997,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 6,
+      "pose": {
+        "translation": {
+          "x": 11.8779798,
+          "y": 0.6444996,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 7,
+      "pose": {
+        "translation": {
+          "x": 11.9528844,
+          "y": 0.6444996,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 8,
+      "pose": {
+        "translation": {
+          "x": 12.2710194,
+          "y": 3.4312351999999997,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 9,
+      "pose": {
+        "translation": {
+          "x": 12.519177399999998,
+          "y": 3.6790375999999996,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 10,
+      "pose": {
+        "translation": {
+          "x": 12.519177399999998,
+          "y": 4.0346376,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 11,
+      "pose": {
+        "translation": {
+          "x": 12.2710194,
+          "y": 4.638039999999999,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.7071067811865476,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 12,
+      "pose": {
+        "translation": {
+          "x": 11.9528844,
+          "y": 7.4247756,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 13,
+      "pose": {
+        "translation": {
+          "x": 16.5333172,
+          "y": 7.4033126,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 14,
+      "pose": {
+        "translation": {
+          "x": 16.5333172,
+          "y": 6.9715126,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 15,
+      "pose": {
+        "translation": {
+          "x": 16.5329616,
+          "y": 4.3235626,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 16,
+      "pose": {
+        "translation": {
+          "x": 16.5329616,
+          "y": 3.8917626,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 17,
+      "pose": {
+        "translation": {
+          "x": 4.6630844,
+          "y": 0.6444996,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 18,
+      "pose": {
+        "translation": {
+          "x": 4.6256194,
+          "y": 3.4312351999999997,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 19,
+      "pose": {
+        "translation": {
+          "x": 5.229174199999999,
+          "y": 3.6790375999999996,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 20,
+      "pose": {
+        "translation": {
+          "x": 5.229174199999999,
+          "y": 4.0346376,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 21,
+      "pose": {
+        "translation": {
+          "x": 4.6256194,
+          "y": 4.638039999999999,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.7071067811865476,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 22,
+      "pose": {
+        "translation": {
+          "x": 4.6630844,
+          "y": 7.4247756,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 23,
+      "pose": {
+        "translation": {
+          "x": 4.5881798,
+          "y": 7.4247756,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 24,
+      "pose": {
+        "translation": {
+          "x": 4.2700194,
+          "y": 4.638039999999999,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.7071067811865476,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 25,
+      "pose": {
+        "translation": {
+          "x": 4.0218614,
+          "y": 4.3902376,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 26,
+      "pose": {
+        "translation": {
+          "x": 4.0218614,
+          "y": 4.0346376,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 27,
+      "pose": {
+        "translation": {
+          "x": 4.2700194,
+          "y": 3.4312351999999997,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 28,
+      "pose": {
+        "translation": {
+          "x": 4.5881798,
+          "y": 0.6444996,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 29,
+      "pose": {
+        "translation": {
+          "x": 0.0077469999999999995,
+          "y": 0.6659626,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 30,
+      "pose": {
+        "translation": {
+          "x": 0.0077469999999999995,
+          "y": 1.0977626,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 31,
+      "pose": {
+        "translation": {
+          "x": 0.0080772,
+          "y": 3.7457125999999996,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 32,
+      "pose": {
+        "translation": {
+          "x": 0.0080772,
+          "y": 4.1775126,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    }
+  ],
+  "field": {
+    "length": 16.541,
+    "width": 8.069
+  }
+}

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

@@ -17,12 +17,13 @@ public enum Fields {
   k2022RapidReact("2022-rapidreact.json"),
   k2023ChargedUp("2023-chargedup.json"),
   k2024Crescendo("2024-crescendo.json"),
-  k2025Reefscape("2025-reefscape.json");
+  k2025Reefscape("2025-reefscape.json"),
+  k2026Rebuilt("2026-rebuilt.json");
 
   public static final String kBaseResourceDir = "/edu/wpi/first/fields/";
 
   /** Alias to the current game. */
-  public static final Fields kDefaultField = k2025Reefscape;
+  public static final Fields kDefaultField = k2026Rebuilt;
 
   public final String m_resourceFile;
 

fieldImages/src/main/native/cpp/fields.cpp

@@ -17,10 +17,12 @@
 #include "fields/2023-chargedup.h"
 #include "fields/2024-crescendo.h"
 #include "fields/2025-reefscape.h"
+#include "fields/2026-rebuilt.h"
 
 using namespace fields;
 
 static const Field kFields[] = {
+    {"2026 Rebuilt", GetResource_2026_rebuilt_json, GetResource_2026_field_png},
     {"2025 Reefscape", GetResource_2025_reefscape_json,
      GetResource_2025_field_png},
     {"2024 Crescendo", GetResource_2024_crescendo_json,

fieldImages/src/main/native/include/fields/2026-rebuilt.h

@@ -0,0 +1,12 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string_view>
+
+namespace fields {
+std::string_view GetResource_2026_rebuilt_json();
+std::string_view GetResource_2026_field_png();
+}  // namespace fields

fieldImages/src/main/native/resources/edu/wpi/first/fields/2026-rebuilt.json

@@ -0,0 +1,19 @@
+{
+  "game": "Rebuilt",
+  "field-image": "2026-field.png",
+  "field-corners": {
+    "top-left": [
+      245,
+      118
+    ],
+    "bottom-right": [
+      3942,
+      1914
+    ]
+  },
+  "field-size": [
+    54.269,
+    26.474
+  ],
+  "field-unit": "foot"
+}

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

@@ -343,7 +343,7 @@ static bool InputPose(frc::Pose2d* pose) {
 }
 
 FieldInfo::FieldInfo(Storage& storage)
-    : m_builtin{storage.GetString("builtin", "2025 Reefscape")},
+    : m_builtin{storage.GetString("builtin", "2026 Rebuilt")},
       m_filename{storage.GetString("image")},
       m_width{storage.GetFloat("width", kDefaultWidth.to<float>())},
       m_height{storage.GetFloat("height", kDefaultHeight.to<float>())},

hal/src/generate/Instances.txt

@@ -17,6 +17,7 @@ kFramework_AdvantageKit = 7
 kFramework_MagicBot = 8
 kFramework_KitBotTraditional = 9
 kFramework_KitBotInline = 10
+kFramework_Everybot = 11
 kRobotDrive_ArcadeStandard = 1
 kRobotDrive_ArcadeButtonSpin = 2
 kRobotDrive_ArcadeRatioCurve = 3
@@ -35,6 +36,7 @@ kRobotDriveSwerve_YAGSL = 15
 kRobotDriveSwerve_CTRE = 16
 kRobotDriveSwerve_MaxSwerve = 17
 kRobotDriveSwerve_AdvantageKit = 18
+kRobotDriveSwerve_EasySwerve = 19
 kDriverStationCIO_Analog = 1
 kDriverStationCIO_DigitalIn = 2
 kDriverStationCIO_DigitalOut = 3
@@ -68,7 +70,7 @@ kDashboard_Shuffleboard = 4
 kDashboard_Elastic = 5
 kDashboard_LabVIEW = 6
 kDashboard_AdvantageScope = 7
-kDashboard_QFRCDashboard = 8
+kDashboard_QDash = 8
 kDashboard_FRCWebComponents = 9
 kDataLogLocation_Onboard = 1
 kDataLogLocation_USB = 2

hal/src/generate/ResourceType.txt

@@ -125,3 +125,13 @@ kResourceType_ThriftyNova = 123
 kResourceType_RevServoHub = 124
 kResourceType_PWFSEN36005 = 125
 kResourceType_LaserShark = 126
+kResourceType_YAMS = 127
+kResourceType_LEDPattern = 128
+kResourceType_LinearQuadraticRegulator = 129
+kResourceType_KalmanFilter = 130
+kResourceType_PoseEstimator = 131
+kResourceType_PoseEstimator3d = 132
+kResourceType_LinearSystemLoop = 133
+kResourceType_LumynLabs_ConnectorX = 134
+kResourceType_LumynLabs_ConnectorXAnimate = 135
+kResourceType_RevMAXSplineEncoder = 136

hal/src/generated/main/java/edu/wpi/first/hal/FRCNetComm.java

@@ -273,6 +273,26 @@ public final class FRCNetComm {
     public static final int kResourceType_PWFSEN36005 = 125;
     /** kResourceType_LaserShark = 126. */
     public static final int kResourceType_LaserShark = 126;
+    /** kResourceType_YAMS = 127. */
+    public static final int kResourceType_YAMS = 127;
+    /** kResourceType_LEDPattern = 128. */
+    public static final int kResourceType_LEDPattern = 128;
+    /** kResourceType_LinearQuadraticRegulator = 129. */
+    public static final int kResourceType_LinearQuadraticRegulator = 129;
+    /** kResourceType_KalmanFilter = 130. */
+    public static final int kResourceType_KalmanFilter = 130;
+    /** kResourceType_PoseEstimator = 131. */
+    public static final int kResourceType_PoseEstimator = 131;
+    /** kResourceType_PoseEstimator3d = 132. */
+    public static final int kResourceType_PoseEstimator3d = 132;
+    /** kResourceType_LinearSystemLoop = 133. */
+    public static final int kResourceType_LinearSystemLoop = 133;
+    /** kResourceType_LumynLabs_ConnectorX = 134. */
+    public static final int kResourceType_LumynLabs_ConnectorX = 134;
+    /** kResourceType_LumynLabs_ConnectorXAnimate = 135. */
+    public static final int kResourceType_LumynLabs_ConnectorXAnimate = 135;
+    /** kResourceType_RevMAXSplineEncoder = 136. */
+    public static final int kResourceType_RevMAXSplineEncoder = 136;
   }
 
   /**
@@ -321,6 +341,8 @@ public final class FRCNetComm {
     public static final int kFramework_KitBotTraditional = 9;
     /** kFramework_KitBotInline = 10. */
     public static final int kFramework_KitBotInline = 10;
+    /** kFramework_Everybot = 11. */
+    public static final int kFramework_Everybot = 11;
     /** kRobotDrive_ArcadeStandard = 1. */
     public static final int kRobotDrive_ArcadeStandard = 1;
     /** kRobotDrive_ArcadeButtonSpin = 2. */
@@ -357,6 +379,8 @@ public final class FRCNetComm {
     public static final int kRobotDriveSwerve_MaxSwerve = 17;
     /** kRobotDriveSwerve_AdvantageKit = 18. */
     public static final int kRobotDriveSwerve_AdvantageKit = 18;
+    /** kRobotDriveSwerve_EasySwerve = 19. */
+    public static final int kRobotDriveSwerve_EasySwerve = 19;
     /** kDriverStationCIO_Analog = 1. */
     public static final int kDriverStationCIO_Analog = 1;
     /** kDriverStationCIO_DigitalIn = 2. */
@@ -423,8 +447,8 @@ public final class FRCNetComm {
     public static final int kDashboard_LabVIEW = 6;
     /** kDashboard_AdvantageScope = 7. */
     public static final int kDashboard_AdvantageScope = 7;
-    /** kDashboard_QFRCDashboard = 8. */
-    public static final int kDashboard_QFRCDashboard = 8;
+    /** kDashboard_QDash = 8. */
+    public static final int kDashboard_QDash = 8;
     /** kDashboard_FRCWebComponents = 9. */
     public static final int kDashboard_FRCWebComponents = 9;
     /** kDataLogLocation_Onboard = 1. */

hal/src/generated/main/native/include/hal/FRCUsageReporting.h

@@ -178,6 +178,16 @@ namespace HALUsageReporting {
     kResourceType_RevServoHub = 124,
     kResourceType_PWFSEN36005 = 125,
     kResourceType_LaserShark = 126,
+    kResourceType_YAMS = 127,
+    kResourceType_LEDPattern = 128,
+    kResourceType_LinearQuadraticRegulator = 129,
+    kResourceType_KalmanFilter = 130,
+    kResourceType_PoseEstimator = 131,
+    kResourceType_PoseEstimator3d = 132,
+    kResourceType_LinearSystemLoop = 133,
+    kResourceType_LumynLabs_ConnectorX = 134,
+    kResourceType_LumynLabs_ConnectorXAnimate = 135,
+    kResourceType_RevMAXSplineEncoder = 136,
   };
   enum tInstances : int32_t {
     kLanguage_LabVIEW = 1,
@@ -199,6 +209,7 @@ namespace HALUsageReporting {
     kFramework_MagicBot = 8,
     kFramework_KitBotTraditional = 9,
     kFramework_KitBotInline = 10,
+    kFramework_Everybot = 11,
     kRobotDrive_ArcadeStandard = 1,
     kRobotDrive_ArcadeButtonSpin = 2,
     kRobotDrive_ArcadeRatioCurve = 3,
@@ -217,6 +228,7 @@ namespace HALUsageReporting {
     kRobotDriveSwerve_CTRE = 16,
     kRobotDriveSwerve_MaxSwerve = 17,
     kRobotDriveSwerve_AdvantageKit = 18,
+    kRobotDriveSwerve_EasySwerve = 19,
     kDriverStationCIO_Analog = 1,
     kDriverStationCIO_DigitalIn = 2,
     kDriverStationCIO_DigitalOut = 3,
@@ -250,7 +262,7 @@ namespace HALUsageReporting {
     kDashboard_Elastic = 5,
     kDashboard_LabVIEW = 6,
     kDashboard_AdvantageScope = 7,
-    kDashboard_QFRCDashboard = 8,
+    kDashboard_QDash = 8,
     kDashboard_FRCWebComponents = 9,
     kDataLogLocation_Onboard = 1,
     kDataLogLocation_USB = 2,

hal/src/generated/main/native/include/hal/UsageReporting.h

@@ -147,6 +147,16 @@ typedef enum
     kResourceType_RevServoHub = 124,
     kResourceType_PWFSEN36005 = 125,
     kResourceType_LaserShark = 126,
+    kResourceType_YAMS = 127,
+    kResourceType_LEDPattern = 128,
+    kResourceType_LinearQuadraticRegulator = 129,
+    kResourceType_KalmanFilter = 130,
+    kResourceType_PoseEstimator = 131,
+    kResourceType_PoseEstimator3d = 132,
+    kResourceType_LinearSystemLoop = 133,
+    kResourceType_LumynLabs_ConnectorX = 134,
+    kResourceType_LumynLabs_ConnectorXAnimate = 135,
+    kResourceType_RevMAXSplineEncoder = 136,
 
 //    kResourceType_MaximumID = 255,
 } tResourceType;
@@ -172,6 +182,7 @@ typedef enum
     kFramework_MagicBot = 8,
     kFramework_KitBotTraditional = 9,
     kFramework_KitBotInline = 10,
+    kFramework_Everybot = 11,
     kRobotDrive_ArcadeStandard = 1,
     kRobotDrive_ArcadeButtonSpin = 2,
     kRobotDrive_ArcadeRatioCurve = 3,
@@ -190,6 +201,7 @@ typedef enum
     kRobotDriveSwerve_CTRE = 16,
     kRobotDriveSwerve_MaxSwerve = 17,
     kRobotDriveSwerve_AdvantageKit = 18,
+    kRobotDriveSwerve_EasySwerve = 19,
     kDriverStationCIO_Analog = 1,
     kDriverStationCIO_DigitalIn = 2,
     kDriverStationCIO_DigitalOut = 3,
@@ -223,7 +235,7 @@ typedef enum
     kDashboard_Elastic = 5,
     kDashboard_LabVIEW = 6,
     kDashboard_AdvantageScope = 7,
-    kDashboard_QFRCDashboard = 8,
+    kDashboard_QDash = 8,
     kDashboard_FRCWebComponents = 9,
     kDataLogLocation_Onboard = 1,
     kDataLogLocation_USB = 2,

romiVendordep/RomiVendordep.json

@@ -3,7 +3,7 @@
   "name": "Romi-Vendordep",
   "version": "1.0.0",
   "uuid": "1010372a-b446-46f4-b229-61e53a26a7dc",
-  "frcYear": "2026beta",
+  "frcYear": "2026",
   "mavenUrls": [],
   "jsonUrl": "",
   "javaDependencies": [

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

@@ -57,7 +57,8 @@ static double Lerp(units::second_t time,
  */
 static std::vector<PreparedData> ConvertToPrepared(const MotorData& data) {
   std::vector<PreparedData> prepared;
-  // assume we've selected down to a single contiguous run by this point
+
+  // Assume we've selected down to a single contiguous run by this point
   auto run = data.runs[0];
 
   for (int i = 0; i < static_cast<int>(run.voltage.size()) - 1; ++i) {
@@ -101,7 +102,7 @@ static void CopyRawData(wpi::StringMap<MotorData>* dataset) {
 }
 
 /**
- * Assigns the combines the various datasets into a single one for analysis.
+ * Combines the various datasets into a single one for analysis.
  *
  * @param slowForward The slow forward dataset
  * @param slowBackward The slow backward dataset
@@ -127,6 +128,28 @@ void AnalysisManager::PrepareGeneralData() {
   // Convert data to PreparedData structs
   for (auto& it : m_data.motorData) {
     auto key = it.first;
+
+    // Assume we've selected down to a single contiguous run by this point
+    auto run = m_data.motorData[key].runs[0];
+
+    // Ensure data has at least two samples in it or linear interpolation within
+    // ConvertToPrepared() will fail
+    if (run.voltage.size() < 2) {
+      throw sysid::InvalidDataError(fmt::format(
+          "{} data has {} voltage samples and at least 2 are required.", key,
+          run.voltage.size()));
+    }
+    if (run.position.size() < 2) {
+      throw sysid::InvalidDataError(fmt::format(
+          "{} data has {} position samples and at least 2 are required.", key,
+          run.position.size()));
+    }
+    if (run.velocity.size() < 2) {
+      throw sysid::InvalidDataError(fmt::format(
+          "{} data has {} velocity samples and at least 2 are required.", key,
+          run.velocity.size()));
+    }
+
     preparedData[key] = ConvertToPrepared(m_data.motorData[key]);
     WPI_INFO(m_logger, "SAMPLES {}", preparedData[key].size());
   }

upstream_utils/llvm_patches/0001-Remove-StringRef-ArrayRef-and-Optional.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sat, 7 May 2022 22:09:18 -0400
-Subject: [PATCH 01/37] Remove StringRef, ArrayRef, and Optional
+Subject: [PATCH 01/36] Remove StringRef, ArrayRef, and Optional
 
 ---
  llvm/include/llvm/ADT/PointerUnion.h          |   1 -

upstream_utils/llvm_patches/0002-Wrap-std-min-max-calls-in-parens-for-Windows-warning.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sat, 7 May 2022 22:12:41 -0400
-Subject: [PATCH 02/37] Wrap std::min/max calls in parens, for Windows warnings
+Subject: [PATCH 02/36] Wrap std::min/max calls in parens, for Windows warnings
 
 ---
  llvm/include/llvm/ADT/DenseMap.h       |  4 ++--

upstream_utils/llvm_patches/0003-Change-unique_function-storage-size.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sat, 7 May 2022 22:13:55 -0400
-Subject: [PATCH 03/37] Change unique_function storage size
+Subject: [PATCH 03/36] Change unique_function storage size
 
 ---
  llvm/include/llvm/ADT/FunctionExtras.h | 4 ++--

upstream_utils/llvm_patches/0004-Threading-updates.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sat, 7 May 2022 22:17:19 -0400
-Subject: [PATCH 04/37] Threading updates
+Subject: [PATCH 04/36] Threading updates
 
 - Remove guards for threads and exception
 - Prefer scope gaurd over lock gaurd

upstream_utils/llvm_patches/0005-ifdef-guard-safety.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sat, 7 May 2022 22:28:13 -0400
-Subject: [PATCH 05/37] \#ifdef guard safety
+Subject: [PATCH 05/36] \#ifdef guard safety
 
 Prevents redefinition if someone is pulling in real LLVM, since the macros are in global namespace
 ---

upstream_utils/llvm_patches/0006-Explicitly-use-std.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sat, 7 May 2022 22:37:34 -0400
-Subject: [PATCH 06/37] Explicitly use std::
+Subject: [PATCH 06/36] Explicitly use std::
 
 ---
  llvm/include/llvm/ADT/SmallSet.h       |  2 +-

upstream_utils/llvm_patches/0007-Remove-format_provider.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sat, 7 May 2022 22:53:50 -0400
-Subject: [PATCH 07/37] Remove format_provider
+Subject: [PATCH 07/36] Remove format_provider
 
 ---
  llvm/include/llvm/Support/Chrono.h      | 114 ------------------------

upstream_utils/llvm_patches/0008-Add-compiler-warning-pragmas.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sun, 8 May 2022 13:34:07 -0400
-Subject: [PATCH 08/37] Add compiler warning pragmas
+Subject: [PATCH 08/36] Add compiler warning pragmas
 
 ---
  llvm/include/llvm/ADT/FunctionExtras.h | 11 +++++++++++

upstream_utils/llvm_patches/0009-Remove-unused-functions.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sun, 8 May 2022 13:43:50 -0400
-Subject: [PATCH 09/37] Remove unused functions
+Subject: [PATCH 09/36] Remove unused functions
 
 ---
  llvm/include/llvm/ADT/SmallString.h      |  77 ------

upstream_utils/llvm_patches/0010-Detemplatize-SmallVectorBase.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Thu, 5 May 2022 23:18:34 -0400
-Subject: [PATCH 10/37] Detemplatize SmallVectorBase
+Subject: [PATCH 10/36] Detemplatize SmallVectorBase
 
 ---
  llvm/include/llvm/ADT/SmallVector.h | 35 ++++++++++-----------------

upstream_utils/llvm_patches/0011-Add-vectors-to-raw_ostream.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sun, 8 May 2022 13:48:59 -0400
-Subject: [PATCH 11/37] Add vectors to raw_ostream
+Subject: [PATCH 11/36] Add vectors to raw_ostream
 
 ---
  llvm/include/llvm/Support/raw_ostream.h | 115 ++++++++++++++++++++++++

upstream_utils/llvm_patches/0012-Extra-collections-features.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Tue, 3 May 2022 22:16:10 -0400
-Subject: [PATCH 12/37] Extra collections features
+Subject: [PATCH 12/36] Extra collections features
 
 ---
  llvm/lib/Support/raw_ostream.cpp | 8 ++++++++

upstream_utils/llvm_patches/0013-Delete-numbers-from-MathExtras.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Thu, 5 May 2022 18:09:45 -0400
-Subject: [PATCH 14/37] Delete numbers from MathExtras
+Subject: [PATCH 13/36] Delete numbers from MathExtras
 
 ---
  llvm/include/llvm/Support/MathExtras.h | 36 --------------------------

upstream_utils/llvm_patches/0013-EpochTracker-ABI-macro.patch

@@ -1,22 +0,0 @@
-From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
-From: PJ Reiniger <pj.reiniger@gmail.com>
-Date: Wed, 4 May 2022 00:01:00 -0400
-Subject: [PATCH 13/37] EpochTracker ABI macro
-
----
- llvm/include/llvm/ADT/EpochTracker.h | 2 +-
- 1 file changed, 1 insertion(+), 1 deletion(-)
-
-diff --git a/llvm/include/llvm/ADT/EpochTracker.h b/llvm/include/llvm/ADT/EpochTracker.h
-index fc41d6f2c92d2f9876c741067b5645a74663db04..56d0acda2c1a0e390cfed086fa298b650c4a561f 100644
---- a/llvm/include/llvm/ADT/EpochTracker.h
-+++ b/llvm/include/llvm/ADT/EpochTracker.h
-@@ -22,7 +22,7 @@
- 
- namespace llvm {
- 
--#if LLVM_ENABLE_ABI_BREAKING_CHECKS
-+#ifndef NDEBUG //ifndef LLVM_ENABLE_ABI_BREAKING_CHECKS
- #define LLVM_DEBUGEPOCHBASE_HANDLEBASE_EMPTYBASE
- 
- /// A base class for data structure classes wishing to make iterators

upstream_utils/llvm_patches/0014-Add-lerp-and-sgn.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Tue, 3 May 2022 22:50:24 -0400
-Subject: [PATCH 15/37] Add lerp and sgn
+Subject: [PATCH 14/36] Add lerp and sgn
 
 ---
  llvm/include/llvm/Support/MathExtras.h | 21 +++++++++++++++++++++

upstream_utils/llvm_patches/0015-Fixup-includes.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sun, 8 May 2022 16:38:11 -0400
-Subject: [PATCH 16/37] Fixup includes
+Subject: [PATCH 15/36] Fixup includes
 
 ---
  llvm/include/llvm/Support/PointerLikeTypeTraits.h |  1 +

upstream_utils/llvm_patches/0016-Use-std-is_trivially_copy_constructible.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sun, 8 May 2022 16:42:09 -0400
-Subject: [PATCH 17/37] Use std::is_trivially_copy_constructible
+Subject: [PATCH 16/36] Use std::is_trivially_copy_constructible
 
 ---
  llvm/include/llvm/Support/type_traits.h | 16 ----------------

upstream_utils/llvm_patches/0017-Windows-support.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Tue, 3 May 2022 20:22:38 -0400
-Subject: [PATCH 18/37] Windows support
+Subject: [PATCH 17/36] Windows support
 
 ---
  .../llvm/Support/Windows/WindowsSupport.h     | 45 +++++----

upstream_utils/llvm_patches/0018-Remove-call-to-RtlGetLastNtStatus.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Tue, 17 Sep 2024 21:19:52 -0700
-Subject: [PATCH 19/37] Remove call to RtlGetLastNtStatus()
+Subject: [PATCH 18/36] Remove call to RtlGetLastNtStatus()
 
 ---
  llvm/lib/Support/ErrorHandling.cpp | 23 -----------------------

upstream_utils/llvm_patches/0019-Prefer-fmtlib.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sun, 8 May 2022 16:46:20 -0400
-Subject: [PATCH 20/37] Prefer fmtlib
+Subject: [PATCH 19/36] Prefer fmtlib
 
 ---
  llvm/lib/Support/ErrorHandling.cpp | 20 ++++++--------------

upstream_utils/llvm_patches/0020-Prefer-wpi-s-fs.h.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sun, 8 May 2022 16:49:36 -0400
-Subject: [PATCH 21/37] Prefer wpi's fs.h
+Subject: [PATCH 20/36] Prefer wpi's fs.h
 
 ---
  llvm/include/llvm/Support/raw_ostream.h | 7 ++-----

upstream_utils/llvm_patches/0021-Remove-unused-functions.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sun, 8 May 2022 19:16:51 -0400
-Subject: [PATCH 22/37] Remove unused functions
+Subject: [PATCH 21/36] Remove unused functions
 
 ---
  llvm/include/llvm/Support/raw_ostream.h |  5 +-

upstream_utils/llvm_patches/0022-OS-specific-changes.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Sun, 8 May 2022 19:30:43 -0400
-Subject: [PATCH 23/37] OS-specific changes
+Subject: [PATCH 22/36] OS-specific changes
 
 ---
  llvm/lib/Support/ErrorHandling.cpp | 16 +++++++---------

upstream_utils/llvm_patches/0023-Use-SmallVector-for-UTF-conversion.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Mon, 9 May 2022 00:04:30 -0400
-Subject: [PATCH 24/37] Use SmallVector for UTF conversion
+Subject: [PATCH 23/36] Use SmallVector for UTF conversion
 
 ---
  llvm/include/llvm/Support/ConvertUTF.h    |  6 +++---

upstream_utils/llvm_patches/0024-Prefer-to-use-static-pointers-in-raw_ostream.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Thu, 19 May 2022 00:58:36 -0400
-Subject: [PATCH 25/37] Prefer to use static pointers in raw_ostream
+Subject: [PATCH 24/36] Prefer to use static pointers in raw_ostream
 
 See #1401
 ---

upstream_utils/llvm_patches/0025-constexpr-endian-byte-swap.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: PJ Reiniger <pj.reiniger@gmail.com>
 Date: Fri, 1 Mar 2024 11:56:17 -0800
-Subject: [PATCH 26/37] constexpr endian byte swap
+Subject: [PATCH 25/36] constexpr endian byte swap
 
 ---
  llvm/include/llvm/Support/Endian.h | 4 +++-

upstream_utils/llvm_patches/0026-Copy-type-traits-from-STLExtras.h-into-PointerUnion..patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Wed, 10 Aug 2022 17:07:52 -0700
-Subject: [PATCH 27/37] Copy type traits from STLExtras.h into PointerUnion.h
+Subject: [PATCH 26/36] Copy type traits from STLExtras.h into PointerUnion.h
 
 ---
  llvm/include/llvm/ADT/PointerUnion.h | 46 ++++++++++++++++++++++++++++

upstream_utils/llvm_patches/0027-Unused-variable-in-release-mode.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Leander Schulten <Leander.Schulten@rwth-aachen.de>
 Date: Mon, 10 Jul 2023 00:53:43 +0200
-Subject: [PATCH 28/37] Unused variable in release mode
+Subject: [PATCH 27/36] Unused variable in release mode
 
 ---
  llvm/include/llvm/ADT/DenseMap.h | 2 +-

upstream_utils/llvm_patches/0028-Use-C-20-bit-header.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Tue, 11 Jul 2023 22:56:09 -0700
-Subject: [PATCH 29/37] Use C++20 <bit> header
+Subject: [PATCH 28/36] Use C++20 <bit> header
 
 ---
  llvm/include/llvm/ADT/DenseMap.h       |   3 +-

upstream_utils/llvm_patches/0029-Remove-DenseMap-GTest-printer-test.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Sun, 30 Jul 2023 14:17:37 -0700
-Subject: [PATCH 30/37] Remove DenseMap GTest printer test
+Subject: [PATCH 29/36] Remove DenseMap GTest printer test
 
 LLVM modifies internal GTest headers to support it, which we can't do.
 ---

upstream_utils/llvm_patches/0030-raw_ostream-Add-SetNumBytesInBuffer.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Peter Johnson <johnson.peter@gmail.com>
 Date: Sun, 29 Oct 2023 23:00:08 -0700
-Subject: [PATCH 31/37] raw_ostream: Add SetNumBytesInBuffer
+Subject: [PATCH 30/36] raw_ostream: Add SetNumBytesInBuffer
 
 ---
  llvm/include/llvm/Support/raw_ostream.h | 5 +++++

upstream_utils/llvm_patches/0031-raw_ostream-Replace-errnoAsErrorCode.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Tue, 17 Sep 2024 15:30:31 -0700
-Subject: [PATCH 32/37] raw_ostream: Replace errnoAsErrorCode()
+Subject: [PATCH 31/36] raw_ostream: Replace errnoAsErrorCode()
 
 ---
  llvm/lib/Support/raw_ostream.cpp | 4 ++--

upstream_utils/llvm_patches/0032-type_traits.h-Add-is_constexpr.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Peter Johnson <johnson.peter@gmail.com>
 Date: Sat, 2 Dec 2023 15:21:32 -0800
-Subject: [PATCH 33/37] type_traits.h: Add is_constexpr()
+Subject: [PATCH 32/36] type_traits.h: Add is_constexpr()
 
 ---
  llvm/include/llvm/Support/type_traits.h | 5 +++++

upstream_utils/llvm_patches/0033-Remove-auto-conversion-from-raw_ostream.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Sun, 17 Mar 2024 14:51:11 -0700
-Subject: [PATCH 34/37] Remove auto-conversion from raw_ostream
+Subject: [PATCH 33/36] Remove auto-conversion from raw_ostream
 
 ---
  llvm/lib/Support/raw_ostream.cpp | 11 +----------

upstream_utils/llvm_patches/0034-Add-SmallVector-erase_if.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Tue, 18 Jun 2024 09:07:33 -0700
-Subject: [PATCH 35/37] Add SmallVector erase_if()
+Subject: [PATCH 34/36] Add SmallVector erase_if()
 
 ---
  llvm/include/llvm/ADT/SmallVector.h | 8 ++++++++

upstream_utils/llvm_patches/0035-Fix-AlignedCharArrayUnion-for-C-23.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Sat, 13 Jul 2024 15:24:30 -0700
-Subject: [PATCH 36/37] Fix AlignedCharArrayUnion for C++23
+Subject: [PATCH 35/36] Fix AlignedCharArrayUnion for C++23
 
 ---
  llvm/include/llvm/Support/AlignOf.h | 14 +++++---------

upstream_utils/llvm_patches/0036-Fix-minIntN-and-maxIntN-assertions.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Mon, 23 Dec 2024 22:56:29 -0800
-Subject: [PATCH 37/37] Fix minIntN() and maxIntN() assertions
+Subject: [PATCH 36/36] Fix minIntN() and maxIntN() assertions
 
 ---
  llvm/include/llvm/Support/MathExtras.h | 4 ++--

wpilibNewCommands/WPILibNewCommands.json

@@ -3,7 +3,7 @@
   "name": "WPILib-New-Commands",
   "version": "1.0.0",
   "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
-  "frcYear": "2026beta",
+  "frcYear": "2026",
   "mavenUrls": [],
   "jsonUrl": "",
   "javaDependencies": [

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java

@@ -7,6 +7,7 @@ package edu.wpi.first.wpilibj2.command.button;
 import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
 
 import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.filter.EdgeCounterFilter;
 import edu.wpi.first.wpilibj.event.EventLoop;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -287,4 +288,31 @@ public class Trigger implements BooleanSupplier {
           }
         });
   }
+
+  /**
+   * Creates a new multi-press trigger from this trigger - it will become active when this trigger
+   * has been activated the required number of times within the specified time window.
+   *
+   * <p>This is useful for implementing "double-click" style functionality.
+   *
+   * <p>Input for this must be stable, consider using a Debouncer before this to avoid counting
+   * noise as multiple presses.
+   *
+   * @param requiredPresses The number of presses required.
+   * @param windowTime The number of seconds in which the presses must occur.
+   * @return The multi-press trigger.
+   */
+  public Trigger multiPress(int requiredPresses, double windowTime) {
+    return new Trigger(
+        m_loop,
+        new BooleanSupplier() {
+          final EdgeCounterFilter m_edgeCounterFilter =
+              new EdgeCounterFilter(requiredPresses, windowTime);
+
+          @Override
+          public boolean getAsBoolean() {
+            return m_edgeCounterFilter.calculate(m_condition.getAsBoolean());
+          }
+        });
+  }
 }

wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp

@@ -7,6 +7,7 @@
 #include <utility>
 
 #include <frc/filter/Debouncer.h>
+#include <frc/filter/EdgeCountFilter.h>
 
 #include "frc2/command/CommandPtr.h"
 
@@ -184,6 +185,14 @@ Trigger Trigger::Debounce(units::second_t debounceTime,
   });
 }
 
+Trigger Trigger::MultiPress(int requiredPresses, units::second_t windowTime) {
+  return Trigger(m_loop,
+                 [filter = frc::EdgeCounterFilter(requiredPresses, windowTime),
+                  condition = m_condition]() mutable {
+                   return filter.Calculate(condition());
+                 });
+}
+
 bool Trigger::Get() const {
   return m_condition();
 }

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

@@ -284,6 +284,22 @@ class Trigger {
                    frc::Debouncer::DebounceType type =
                        frc::Debouncer::DebounceType::kRising);
 
+  /**
+   * Creates a new multi-press trigger from this trigger - it will become active
+   * when this trigger has been activated the required number of times within
+   * the specified time window.
+   *
+   * <p>This is useful for implementing "double-click" style functionality.
+   *
+   * <p>Input for this must be stable, consider using a Debouncer before this to
+   * avoid counting noise as multiple presses.
+   *
+   * @param requiredPresses The number of presses required.
+   * @param windowTime The time in which the presses must occur.
+   * @return The multi-press trigger.
+   */
+  Trigger MultiPress(int requiredPresses, units::second_t windowTime);
+
   /**
    * Returns the current state of this trigger.
    *

wpilibc/src/main/native/cpp/LEDPattern.cpp

@@ -11,6 +11,7 @@
 #include <utility>
 #include <vector>
 
+#include <hal/FRCUsageReporting.h>
 #include <wpi/MathExtras.h>
 #include <wpi/timestamp.h>
 
@@ -21,7 +22,9 @@ using namespace frc;
 LEDPattern::LEDPattern(std::function<void(frc::LEDPattern::LEDReader,
                                           std::function<void(int, frc::Color)>)>
                            impl)
-    : m_impl(std::move(impl)) {}
+    : m_impl(std::move(impl)) {
+  HAL_Report(HALUsageReporting::kResourceType_LEDPattern, 1);
+}
 
 void LEDPattern::ApplyTo(LEDPattern::LEDReader reader,
                          std::function<void(int, frc::Color)> writer) const {

wpilibc/src/main/native/cppcs/RobotBase.cpp

@@ -146,6 +146,22 @@ class WPILibMathShared : public wpi::math::MathShared {
         HAL_Report(HALUsageReporting::kResourceType_PathWeaverTrajectory,
                    count);
         break;
+      case wpi::math::MathUsageId::kController_LinearQuadraticRegulator:
+        HAL_Report(HALUsageReporting::kResourceType_LinearQuadraticRegulator,
+                   count);
+        break;
+      case wpi::math::MathUsageId::kEstimator_KalmanFilter:
+        HAL_Report(HALUsageReporting::kResourceType_KalmanFilter, count);
+        break;
+      case wpi::math::MathUsageId::kEstimator_PoseEstimator:
+        HAL_Report(HALUsageReporting::kResourceType_PoseEstimator, count);
+        break;
+      case wpi::math::MathUsageId::kEstimator_PoseEstimator3d:
+        HAL_Report(HALUsageReporting::kResourceType_PoseEstimator3d, count);
+        break;
+      case wpi::math::MathUsageId::kSystem_LinearSystemLoop:
+        HAL_Report(HALUsageReporting::kResourceType_LinearSystemLoop, count);
+        break;
     }
   }
 
@@ -290,10 +306,11 @@ RobotBase::RobotBase() {
         HAL_Report(HALUsageReporting::kResourceType_Dashboard,
                    HALUsageReporting::kDashboard_AdvantageScope);
         m_dashboardDetected = true;
-      } else if (event.GetConnectionInfo()->remote_id.starts_with(
+      } else if (event.GetConnectionInfo()->remote_id.starts_with("QDash") ||
+                 event.GetConnectionInfo()->remote_id.starts_with(
                      "QFRCDashboard")) {
         HAL_Report(HALUsageReporting::kResourceType_Dashboard,
-                   HALUsageReporting::kDashboard_QFRCDashboard);
+                   HALUsageReporting::kDashboard_QDash);
         m_dashboardDetected = true;
       } else if (event.GetConnectionInfo()->remote_id.starts_with(
                      "FRC Web Components")) {

wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h

@@ -77,7 +77,7 @@ class DifferentialDrive : public RobotDriveBase,
    * Construct a DifferentialDrive.
    *
    * To pass multiple motors per side, use CAN motor controller followers or
-   * PWMSpeedController::AddFollower(). If a motor needs to be inverted, do so
+   * PWMMotorController::AddFollower(). If a motor needs to be inverted, do so
    * before passing it in.
    *
    * @param leftMotor Left motor.
@@ -91,7 +91,7 @@ class DifferentialDrive : public RobotDriveBase,
    * Construct a DifferentialDrive.
    *
    * To pass multiple motors per side, use CAN motor controller followers or
-   * PWMSpeedController::AddFollower(). If a motor needs to be inverted, do so
+   * PWMMotorController::AddFollower(). If a motor needs to be inverted, do so
    * before passing it in.
    *
    * @param leftMotor Left motor setter.

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

@@ -45,8 +45,8 @@ DriveSubsystem::DriveSubsystem()
 void DriveSubsystem::Periodic() {
   // Implementation of subsystem periodic method goes here.
   m_odometry.Update(m_gyro.GetRotation2d(),
-                    {m_frontLeft.GetPosition(), m_rearLeft.GetPosition(),
-                     m_frontRight.GetPosition(), m_rearRight.GetPosition()});
+                    {m_frontLeft.GetPosition(), m_frontRight.GetPosition(),
+                     m_rearLeft.GetPosition(), m_rearRight.GetPosition()});
 }
 
 void DriveSubsystem::Drive(units::meters_per_second_t xSpeed,

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

@@ -9,6 +9,8 @@ import static edu.wpi.first.units.Units.Microsecond;
 import static edu.wpi.first.units.Units.Microseconds;
 import static edu.wpi.first.units.Units.Value;
 
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.units.collections.LongToObjectHashMap;
 import edu.wpi.first.units.measure.Dimensionless;
@@ -149,6 +151,7 @@ public interface LEDPattern {
    * @return the mapped pattern
    */
   default LEDPattern mapIndex(IndexMapper indexMapper) {
+    HAL.report(tResourceType.kResourceType_LEDPattern, 1);
     return (reader, writer) -> {
       int bufLen = reader.getLength();
       applyTo(
@@ -294,6 +297,7 @@ public interface LEDPattern {
     final long totalTimeMicros = (long) (onTime.in(Microseconds) + offTime.in(Microseconds));
     final long onTimeMicros = (long) onTime.in(Microseconds);
 
+    HAL.report(tResourceType.kResourceType_LEDPattern, 1);
     return (reader, writer) -> {
       if (RobotController.getTime() % totalTimeMicros < onTimeMicros) {
         applyTo(reader, writer);
@@ -323,6 +327,7 @@ public interface LEDPattern {
    * @return the blinking pattern
    */
   default LEDPattern synchronizedBlink(BooleanSupplier signal) {
+    HAL.report(tResourceType.kResourceType_LEDPattern, 1);
     return (reader, writer) -> {
       if (signal.getAsBoolean()) {
         applyTo(reader, writer);
@@ -342,6 +347,7 @@ public interface LEDPattern {
   default LEDPattern breathe(Time period) {
     final long periodMicros = (long) period.in(Microseconds);
 
+    HAL.report(tResourceType.kResourceType_LEDPattern, 1);
     return (reader, writer) -> {
       applyTo(
           reader,
@@ -373,6 +379,7 @@ public interface LEDPattern {
    * @return the combined overlay pattern
    */
   default LEDPattern overlayOn(LEDPattern base) {
+    HAL.report(tResourceType.kResourceType_LEDPattern, 1);
     return (reader, writer) -> {
       // write the base pattern down first...
       base.applyTo(reader, writer);
@@ -400,6 +407,7 @@ public interface LEDPattern {
    * @return the blended pattern
    */
   default LEDPattern blend(LEDPattern other) {
+    HAL.report(tResourceType.kResourceType_LEDPattern, 1);
     return (reader, writer) -> {
       applyTo(reader, writer);
 
@@ -431,6 +439,7 @@ public interface LEDPattern {
    * @return the masked pattern
    */
   default LEDPattern mask(LEDPattern mask) {
+    HAL.report(tResourceType.kResourceType_LEDPattern, 1);
     return (reader, writer) -> {
       // Apply the current pattern down as normal...
       applyTo(reader, writer);
@@ -470,6 +479,7 @@ public interface LEDPattern {
   default LEDPattern atBrightness(Dimensionless relativeBrightness) {
     double multiplier = relativeBrightness.in(Value);
 
+    HAL.report(tResourceType.kResourceType_LEDPattern, 1);
     return (reader, writer) -> {
       applyTo(
           reader,
@@ -496,6 +506,7 @@ public interface LEDPattern {
    * @return the pattern
    */
   static LEDPattern solid(Color color) {
+    HAL.report(tResourceType.kResourceType_LEDPattern, 1);
     return (reader, writer) -> {
       int bufLen = reader.getLength();
       for (int led = 0; led < bufLen; led++) {
@@ -525,6 +536,7 @@ public interface LEDPattern {
    * @return the mask pattern
    */
   static LEDPattern progressMaskLayer(DoubleSupplier progressSupplier) {
+    HAL.report(tResourceType.kResourceType_LEDPattern, 1);
     return (reader, writer) -> {
       double progress = MathUtil.clamp(progressSupplier.getAsDouble(), 0, 1);
 
@@ -561,6 +573,7 @@ public interface LEDPattern {
    * @return a motionless step pattern
    */
   static LEDPattern steps(Map<? extends Number, Color> steps) {
+    HAL.report(tResourceType.kResourceType_LEDPattern, 1);
     if (steps.isEmpty()) {
       // no colors specified
       DriverStation.reportWarning("Creating LED steps with no colors!", false);
@@ -622,6 +635,7 @@ public interface LEDPattern {
    * @return a motionless gradient pattern
    */
   static LEDPattern gradient(GradientType type, Color... colors) {
+    HAL.report(tResourceType.kResourceType_LEDPattern, 1);
     if (colors.length == 0) {
       // Nothing to display
       DriverStation.reportWarning("Creating a gradient with no colors!", false);
@@ -679,6 +693,7 @@ public interface LEDPattern {
    * @return the rainbow pattern
    */
   static LEDPattern rainbow(int saturation, int value) {
+    HAL.report(tResourceType.kResourceType_LEDPattern, 1);
     return (reader, writer) -> {
       int bufLen = reader.getLength();
       for (int i = 0; i < bufLen; i++) {

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

@@ -126,6 +126,16 @@ public abstract class RobotBase implements AutoCloseable {
                   HAL.report(tResourceType.kResourceType_BangBangController, count);
               case kTrajectory_PathWeaver ->
                   HAL.report(tResourceType.kResourceType_PathWeaverTrajectory, count);
+              case kController_LinearQuadraticRegulator ->
+                  HAL.report(tResourceType.kResourceType_LinearQuadraticRegulator, count);
+              case kEstimator_KalmanFilter ->
+                  HAL.report(tResourceType.kResourceType_KalmanFilter, count);
+              case kEstimator_PoseEstimator ->
+                  HAL.report(tResourceType.kResourceType_PoseEstimator, count);
+              case kEstimator_PoseEstimator3d ->
+                  HAL.report(tResourceType.kResourceType_PoseEstimator3d, count);
+              case kSystem_LinearSystemLoop ->
+                  HAL.report(tResourceType.kResourceType_LinearSystemLoop, count);
               default -> {
                 // NOP
               }
@@ -201,9 +211,9 @@ public abstract class RobotBase implements AutoCloseable {
                   HAL.report(
                       tResourceType.kResourceType_Dashboard, tInstances.kDashboard_AdvantageScope);
                   m_dashboardDetected = true;
-                } else if (event.connInfo.remote_id.startsWith("QFRCDashboard")) {
-                  HAL.report(
-                      tResourceType.kResourceType_Dashboard, tInstances.kDashboard_QFRCDashboard);
+                } else if (event.connInfo.remote_id.startsWith("QDash")
+                    || event.connInfo.remote_id.startsWith("QFRCDashboard")) {
+                  HAL.report(tResourceType.kResourceType_Dashboard, tInstances.kDashboard_QDash);
                   m_dashboardDetected = true;
                 } else if (event.connInfo.remote_id.startsWith("FRC Web Components")) {
                   HAL.report(

wpimath/src/main/java/edu/wpi/first/math/MathUsageId.java

@@ -41,4 +41,19 @@ public enum MathUsageId {
 
   /** PathWeaver Trajectory. */
   kTrajectory_PathWeaver,
+
+  /** Linear Quadratic Regulator. */
+  kController_LinearQuadraticRegulator,
+
+  /** Kalman Filter. */
+  kEstimator_KalmanFilter,
+
+  /** Pose Estimator. */
+  kEstimator_PoseEstimator,
+
+  /** 3D Pose Estimator. */
+  kEstimator_PoseEstimator3d,
+
+  /** Linear System Loop. */
+  kSystem_LinearSystemLoop,
 }

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

@@ -5,6 +5,8 @@
 package edu.wpi.first.math.controller;
 
 import edu.wpi.first.math.DARE;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Num;
 import edu.wpi.first.math.StateSpaceUtil;
@@ -124,6 +126,8 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
     m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
 
     reset();
+    MathSharedStore.getMathShared()
+        .reportUsage(MathUsageId.kController_LinearQuadraticRegulator, 1);
   }
 
   /**
@@ -163,6 +167,8 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
     m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
 
     reset();
+    MathSharedStore.getMathShared()
+        .reportUsage(MathUsageId.kController_LinearQuadraticRegulator, 1);
   }
 
   /**

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

@@ -5,6 +5,8 @@
 package edu.wpi.first.math.estimator;
 
 import edu.wpi.first.math.DARE;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.Num;
@@ -163,6 +165,8 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
     }
 
     m_P = m_initP;
+
+    MathSharedStore.getMathShared().reportUsage(MathUsageId.kEstimator_KalmanFilter, 2);
   }
 
   /**

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

@@ -5,6 +5,8 @@
 package edu.wpi.first.math.estimator;
 
 import edu.wpi.first.math.DARE;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.Num;
@@ -87,6 +89,8 @@ public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extend
     m_initP = new Matrix<>(DARE.dare(discA.transpose(), C.transpose(), discQ, discR));
 
     reset();
+
+    MathSharedStore.getMathShared().reportUsage(MathUsageId.kEstimator_KalmanFilter, 1);
   }
 
   /**

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

@@ -5,6 +5,7 @@
 package edu.wpi.first.math.estimator;
 
 import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
@@ -48,7 +49,9 @@ public class PoseEstimator<T> {
       TimeInterpolatableBuffer.createBuffer(kBufferDuration);
   // Maps timestamps to vision updates
   // Always contains one entry before the oldest entry in m_odometryPoseBuffer, unless there have
-  // been no vision measurements after the last reset
+  // been no vision measurements after the last reset. May contain one entry while
+  // m_odometryPoseBuffer is empty to correct for translation/rotation after a call to
+  // ResetRotation/ResetTranslation.
   private final NavigableMap<Double, VisionUpdate> m_visionUpdates = new TreeMap<>();
 
   private Pose2d m_poseEstimate;
@@ -79,6 +82,7 @@ public class PoseEstimator<T> {
       m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
     }
     setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    MathSharedStore.getMathShared().reportUsage(MathUsageId.kEstimator_PoseEstimator, 1);
   }
 
   /**
@@ -145,9 +149,22 @@ public class PoseEstimator<T> {
    */
   public void resetTranslation(Translation2d translation) {
     m_odometry.resetTranslation(translation);
+
+    final var latestVisionUpdate = m_visionUpdates.lastEntry();
     m_odometryPoseBuffer.clear();
     m_visionUpdates.clear();
-    m_poseEstimate = m_odometry.getPoseMeters();
+
+    if (latestVisionUpdate != null) {
+      // apply vision compensation to the pose rotation
+      final var visionUpdate =
+          new VisionUpdate(
+              new Pose2d(translation, latestVisionUpdate.getValue().visionPose.getRotation()),
+              new Pose2d(translation, latestVisionUpdate.getValue().odometryPose.getRotation()));
+      m_visionUpdates.put(latestVisionUpdate.getKey(), visionUpdate);
+      m_poseEstimate = visionUpdate.compensate(m_odometry.getPoseMeters());
+    } else {
+      m_poseEstimate = m_odometry.getPoseMeters();
+    }
   }
 
   /**
@@ -157,9 +174,22 @@ public class PoseEstimator<T> {
    */
   public void resetRotation(Rotation2d rotation) {
     m_odometry.resetRotation(rotation);
+
+    final var latestVisionUpdate = m_visionUpdates.lastEntry();
     m_odometryPoseBuffer.clear();
     m_visionUpdates.clear();
-    m_poseEstimate = m_odometry.getPoseMeters();
+
+    if (latestVisionUpdate != null) {
+      // apply vision compensation to the pose translation
+      final var visionUpdate =
+          new VisionUpdate(
+              new Pose2d(latestVisionUpdate.getValue().visionPose.getTranslation(), rotation),
+              new Pose2d(latestVisionUpdate.getValue().odometryPose.getTranslation(), rotation));
+      m_visionUpdates.put(latestVisionUpdate.getKey(), visionUpdate);
+      m_poseEstimate = visionUpdate.compensate(m_odometry.getPoseMeters());
+    } else {
+      m_poseEstimate = m_odometry.getPoseMeters();
+    }
   }
 
   /**

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

@@ -5,6 +5,7 @@
 package edu.wpi.first.math.estimator;
 
 import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
@@ -56,7 +57,9 @@ public class PoseEstimator3d<T> {
       TimeInterpolatableBuffer.createBuffer(kBufferDuration);
   // Maps timestamps to vision updates
   // Always contains one entry before the oldest entry in m_odometryPoseBuffer, unless there have
-  // been no vision measurements after the last reset
+  // been no vision measurements after the last reset. May contain one entry while
+  // m_odometryPoseBuffer is empty to correct for translation/rotation after a call to
+  // ResetRotation/ResetTranslation.
   private final NavigableMap<Double, VisionUpdate> m_visionUpdates = new TreeMap<>();
 
   private Pose3d m_poseEstimate;
@@ -87,6 +90,7 @@ public class PoseEstimator3d<T> {
       m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
     }
     setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    MathSharedStore.getMathShared().reportUsage(MathUsageId.kEstimator_PoseEstimator3d, 1);
   }
 
   /**
@@ -157,9 +161,22 @@ public class PoseEstimator3d<T> {
    */
   public void resetTranslation(Translation3d translation) {
     m_odometry.resetTranslation(translation);
+
+    final var latestVisionUpdate = m_visionUpdates.lastEntry();
     m_odometryPoseBuffer.clear();
     m_visionUpdates.clear();
-    m_poseEstimate = m_odometry.getPoseMeters();
+
+    if (latestVisionUpdate != null) {
+      // apply vision compensation to the pose rotation
+      final var visionUpdate =
+          new VisionUpdate(
+              new Pose3d(translation, latestVisionUpdate.getValue().visionPose.getRotation()),
+              new Pose3d(translation, latestVisionUpdate.getValue().odometryPose.getRotation()));
+      m_visionUpdates.put(latestVisionUpdate.getKey(), visionUpdate);
+      m_poseEstimate = visionUpdate.compensate(m_odometry.getPoseMeters());
+    } else {
+      m_poseEstimate = m_odometry.getPoseMeters();
+    }
   }
 
   /**
@@ -169,9 +186,22 @@ public class PoseEstimator3d<T> {
    */
   public void resetRotation(Rotation3d rotation) {
     m_odometry.resetRotation(rotation);
+
+    final var latestVisionUpdate = m_visionUpdates.lastEntry();
     m_odometryPoseBuffer.clear();
     m_visionUpdates.clear();
-    m_poseEstimate = m_odometry.getPoseMeters();
+
+    if (latestVisionUpdate != null) {
+      // apply vision compensation to the pose translation
+      final var visionUpdate =
+          new VisionUpdate(
+              new Pose3d(latestVisionUpdate.getValue().visionPose.getTranslation(), rotation),
+              new Pose3d(latestVisionUpdate.getValue().odometryPose.getTranslation(), rotation));
+      m_visionUpdates.put(latestVisionUpdate.getKey(), visionUpdate);
+      m_poseEstimate = visionUpdate.compensate(m_odometry.getPoseMeters());
+    } else {
+      m_poseEstimate = m_odometry.getPoseMeters();
+    }
   }
 
   /**

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

@@ -5,6 +5,8 @@
 package edu.wpi.first.math.estimator;
 
 import edu.wpi.first.math.DARE;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.Num;
@@ -107,6 +109,7 @@ public class SteadyStateKalmanFilter<States extends Num, Inputs extends Num, Out
     m_K = new Matrix<>(S.getStorage().solve(C.times(P).getStorage()).transpose());
 
     reset();
+    MathSharedStore.getMathShared().reportUsage(MathUsageId.kEstimator_KalmanFilter, 4);
   }
 
   /** Resets the observer. */

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

@@ -4,6 +4,8 @@
 
 package edu.wpi.first.math.estimator;
 
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.Num;
@@ -163,6 +165,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
     m_pts = new MerweScaledSigmaPoints<>(states);
 
     reset();
+    MathSharedStore.getMathShared().reportUsage(MathUsageId.kEstimator_KalmanFilter, 3);
   }
 
   static <S extends Num, C extends Num>

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

@@ -0,0 +1,120 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.filter;
+
+import edu.wpi.first.math.MathSharedStore;
+
+/**
+ * A rising edge counter for boolean streams. Requires that the boolean change value to true for a
+ * specified number of times within a specified time window after the first rising edge before the
+ * filtered value changes.
+ *
+ * <p>The filter activates when the input has risen (transitioned from false to true) the required
+ * number of times within the time window. Once activated, the output remains true as long as the
+ * input is true. The edge count resets when the time window expires or when the input goes false
+ * after activation.
+ *
+ * <p>Input must be stable; consider using a Debouncer before this filter to avoid counting noise as
+ * multiple edges.
+ */
+public class EdgeCounterFilter {
+  private int m_requiredEdges;
+  private double m_windowTimeSeconds;
+
+  private double m_firstEdgeTimeSeconds;
+  private int m_currentCount;
+
+  private boolean m_lastInput;
+
+  /**
+   * Creates a new EdgeCounterFilter.
+   *
+   * @param requiredEdges The number of rising edges required before the output goes true.
+   * @param windowTime The maximum number of seconds in which all required edges must occur after
+   *     the first rising edge.
+   */
+  public EdgeCounterFilter(int requiredEdges, double windowTime) {
+    m_requiredEdges = requiredEdges;
+    m_windowTimeSeconds = windowTime;
+
+    resetTimer();
+  }
+
+  private void resetTimer() {
+    m_firstEdgeTimeSeconds = MathSharedStore.getTimestamp();
+  }
+
+  private boolean hasElapsed() {
+    return MathSharedStore.getTimestamp() - m_firstEdgeTimeSeconds >= m_windowTimeSeconds;
+  }
+
+  /**
+   * Applies the edge counter filter to the input stream.
+   *
+   * @param input The current value of the input stream.
+   * @return True if the required number of edges have occurred within the time window and the input
+   *     is currently true; false otherwise.
+   */
+  public boolean calculate(boolean input) {
+    boolean enoughEdges = m_currentCount >= m_requiredEdges;
+
+    boolean expired = hasElapsed() && !enoughEdges;
+    boolean activationEnded = !input && enoughEdges;
+
+    if (expired || activationEnded) {
+      m_currentCount = 0;
+    }
+
+    if (input && !m_lastInput) {
+      if (m_currentCount == 0) {
+        resetTimer(); // Start timer on first rising edge
+      }
+
+      m_currentCount++;
+    }
+
+    m_lastInput = input;
+
+    return input && m_currentCount >= m_requiredEdges;
+  }
+
+  /**
+   * Sets the time window duration.
+   *
+   * @param windowTime The maximum window of seconds in which all required edges must occur after
+   *     the first rising edge.
+   */
+  public void setWindowTime(double windowTime) {
+    m_windowTimeSeconds = windowTime;
+  }
+
+  /**
+   * Gets the time window duration.
+   *
+   * @return The maximum window of seconds in which all required edges must occur after the first
+   *     rising edge.
+   */
+  public double getWindowTime() {
+    return m_windowTimeSeconds;
+  }
+
+  /**
+   * Sets the required number of edges.
+   *
+   * @param requiredEdges The number of rising edges required before the output goes true.
+   */
+  public void setRequiredEdges(int requiredEdges) {
+    m_requiredEdges = requiredEdges;
+  }
+
+  /**
+   * Gets the required number of edges.
+   *
+   * @return The number of rising edges required before the output goes true.
+   */
+  public int getRequiredEdges() {
+    return m_requiredEdges;
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java

@@ -16,7 +16,7 @@ public class CoordinateSystem {
   private static final CoordinateSystem m_ned =
       new CoordinateSystem(CoordinateAxis.N(), CoordinateAxis.E(), CoordinateAxis.D());
 
-  // Rotation from this coordinate system to NWU coordinate system
+  // Rotation from this coordinate system to NWU coordinate system when applied extrinsically
   private final Rotation3d m_rotation;
 
   /**
@@ -85,7 +85,8 @@ public class CoordinateSystem {
    */
   public static Translation3d convert(
       Translation3d translation, CoordinateSystem from, CoordinateSystem to) {
-    return translation.rotateBy(from.m_rotation.minus(to.m_rotation));
+    // Convert to NWU, then convert to the new coordinate system
+    return translation.rotateBy(from.m_rotation).rotateBy(to.m_rotation.unaryMinus());
   }
 
   /**
@@ -98,7 +99,8 @@ public class CoordinateSystem {
    */
   public static Rotation3d convert(
       Rotation3d rotation, CoordinateSystem from, CoordinateSystem to) {
-    return rotation.rotateBy(from.m_rotation.minus(to.m_rotation));
+    // Convert to NWU, then convert to the new coordinate system
+    return rotation.rotateBy(from.m_rotation).rotateBy(to.m_rotation.unaryMinus());
   }
 
   /**
@@ -124,9 +126,23 @@ public class CoordinateSystem {
    */
   public static Transform3d convert(
       Transform3d transform, CoordinateSystem from, CoordinateSystem to) {
-    var coordRot = from.m_rotation.minus(to.m_rotation);
+    // coordRot is the rotation that converts between the coordinate systems when applied
+    // extrinsically. It first converts to NWU, then converts to the new coordinate system.
+    var coordRot = from.m_rotation.rotateBy(to.m_rotation.unaryMinus());
+    // The new rotation is the extrinsic rotation from convert(zero) to
+    // convert(transformRot). That is, applying convertedRot extrinsically to
+    // convert(zero) produces convert(transformRot). In the below snippet, we
+    // use matrix notation, so rotA rotB applies rotA extrinsically to rotB.
+    //
+    //   convertedRot convert(zero) = convert(transformRot)
+    //   convertedRot = convert(transformRot) convert(zero)⁻¹
+    //                = (coordRot transformRot) (coordRot zero)⁻¹
+    //                = (coordRot transformRot) coordRot⁻¹
+    //
+    // In code, the equivalent for rotA rotB is rotB.rotateBy(rotA) (note the
+    // change in order), and the equivalent for rot⁻¹ is rot.unaryMinus().
     return new Transform3d(
         convert(transform.getTranslation(), from, to),
-        coordRot.unaryMinus().plus(transform.getRotation().rotateBy(coordRot)));
+        coordRot.unaryMinus().rotateBy(transform.getRotation().rotateBy(coordRot)));
   }
 }

wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java

@@ -220,7 +220,7 @@ public class Pose2d implements Interpolatable<Pose2d>, ProtobufSerializable, Str
   public Pose2d transformBy(Transform2d other) {
     return new Pose2d(
         m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
-        other.getRotation().plus(m_rotation));
+        other.getRotation().rotateBy(m_rotation));
   }
 
   /**

wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java

@@ -254,9 +254,12 @@ public class Pose3d implements Interpolatable<Pose3d>, ProtobufSerializable, Str
    * @return The transformed pose.
    */
   public Pose3d transformBy(Transform3d other) {
+    // Rotating the transform's rotation by the pose's rotation extrinsically is equivalent to
+    // rotating the pose's rotation by the transform's rotation intrinsically. (We define transforms
+    // as being applied intrinsically.)
     return new Pose3d(
         m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
-        other.getRotation().plus(m_rotation));
+        other.getRotation().rotateBy(m_rotation));
   }
 
   /**

wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java

@@ -275,6 +275,17 @@ public class Rotation2d
         m_cos * other.m_cos - m_sin * other.m_sin, m_cos * other.m_sin + m_sin * other.m_cos);
   }
 
+  /**
+   * Returns the current rotation relative to the given rotation.
+   *
+   * @param other The rotation describing the orientation of the new coordinate frame that the
+   *     current rotation will be converted into.
+   * @return The current rotation relative to the new orientation of the coordinate frame.
+   */
+  public Rotation2d relativeTo(Rotation2d other) {
+    return rotateBy(other.unaryMinus());
+  }
+
   /**
    * Returns matrix representation of this rotation.
    *

wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java

@@ -26,7 +26,49 @@ import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Objects;
 
-/** A rotation in a 3D coordinate frame represented by a quaternion. */
+/**
+ * A rotation in a 3D coordinate frame, represented by a quaternion. Note that unlike 2D rotations,
+ * 3D rotations are not always commutative, so changing the order of rotations changes the result.
+ *
+ * <p>As an example of the order of rotations mattering, suppose we have a card that looks like
+ * this:
+ *
+ * <pre>
+ *          ┌───┐        ┌───┐
+ *          │ X │        │ x │
+ *   Front: │ | │  Back: │ · │
+ *          │ | │        │ · │
+ *          └───┘        └───┘
+ * </pre>
+ *
+ * <p>If we rotate 90º clockwise around the axis into the page, then flip around the left/right
+ * axis, we get one result:
+ *
+ * <pre>
+ *   ┌───┐
+ *   │ X │   ┌───────┐   ┌───────┐
+ *   │ | │ → │------X│ → │······x│
+ *   │ | │   └───────┘   └───────┘
+ *   └───┘
+ * </pre>
+ *
+ * <p>If we flip around the left/right axis, then rotate 90º clockwise around the axis into the
+ * page, we get a different result:
+ *
+ * <pre>
+ *   ┌───┐   ┌───┐
+ *   │ X │   │ · │   ┌───────┐
+ *   │ | │ → │ · │ → │x······│
+ *   │ | │   │ x │   └───────┘
+ *   └───┘   └───┘
+ * </pre>
+ *
+ * <p>Because order matters for 3D rotations, we need to distinguish between <em>extrinsic</em>
+ * rotations and <em>intrinsic</em> rotations. Rotating extrinsically means rotating around the
+ * global axes, while rotating intrinsically means rotating from the perspective of the other
+ * rotation. A neat property is that applying a series of rotations extrinsically is the same as
+ * applying the same series in the opposite order intrinsically.
+ */
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Rotation3d
@@ -286,9 +328,17 @@ public class Rotation3d
   }
 
   /**
-   * Adds two rotations together.
+   * Adds two rotations together. The other rotation is applied extrinsically to this rotation,
+   * which is equivalent to this rotation being applied intrinsically to the other rotation. See the
+   * class comment for definitions of extrinsic and intrinsic rotations.
    *
-   * @param other The rotation to add.
+   * <p>Note that {@code a.minus(b).plus(b)} always equals {@code a}, but {@code b.plus(a.minus(b))}
+   * sometimes doesn't. To apply a rotation offset, use either {@code offset =
+   * measurement.unaryMinus().plus(actual); newAngle = angle.plus(offset);} or {@code offset =
+   * actual.minus(measurement); newAngle = offset.plus(angle);}, depending on how the corrected
+   * angle needs to change as the input angle changes.
+   *
+   * @param other The rotation to add (applied extrinsically).
    * @return The sum of the two rotations.
    */
   public Rotation3d plus(Rotation3d other) {
@@ -296,10 +346,19 @@ public class Rotation3d
   }
 
   /**
-   * Subtracts the new rotation from the current rotation and returns the new rotation.
+   * Subtracts the other rotation from the current rotation and returns the new rotation. The new
+   * rotation is from the perspective of the other rotation (like {@link Pose3d#minus}), so it needs
+   * to be applied intrinsically. See the class comment for definitions of extrinsic and intrinsic
+   * rotations.
+   *
+   * <p>Note that {@code a.minus(b).plus(b)} always equals {@code a}, but {@code b.plus(a.minus(b))}
+   * sometimes doesn't. To apply a rotation offset, use either {@code offset =
+   * measurement.unaryMinus().plus(actual); newAngle = angle.plus(offset);} or {@code offset =
+   * actual.minus(measurement); newAngle = offset.plus(angle);}, depending on how the corrected
+   * angle needs to change as the input angle changes.
    *
    * @param other The rotation to subtract.
-   * @return The difference between the two rotations.
+   * @return The difference between the two rotations, from the perspective of the other rotation.
    */
   public Rotation3d minus(Rotation3d other) {
     return rotateBy(other.unaryMinus());
@@ -358,6 +417,24 @@ public class Rotation3d
     return new Rotation3d(other.m_q.times(m_q));
   }
 
+  /**
+   * Returns the current rotation relative to the given rotation. Because the result is in the
+   * perspective of the given rotation, it must be applied intrinsically. See the class comment for
+   * definitions of extrinsic and intrinsic rotations.
+   *
+   * @param other The rotation describing the orientation of the new coordinate frame that the
+   *     current rotation will be converted into.
+   * @return The current rotation relative to the new orientation of the coordinate frame.
+   */
+  public Rotation3d relativeTo(Rotation3d other) {
+    // To apply a quaternion intrinsically, we must right-multiply by that quaternion.
+    // Therefore, "this_q relative to other_q" is the q such that other_q q = this_q:
+    //
+    //   other_q q = this_q
+    //   q = other_q⁻¹ this_q
+    return new Rotation3d(other.m_q.inverse().times(m_q));
+  }
+
   /**
    * Returns the quaternion representation of the Rotation3d.
    *
@@ -562,7 +639,7 @@ public class Rotation3d
 
   @Override
   public Rotation3d interpolate(Rotation3d endValue, double t) {
-    return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
+    return endValue.minus(this).times(MathUtil.clamp(t, 0, 1)).plus(this);
   }
 
   /** Rotation3d protobuf for serialization. */

wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java

@@ -36,15 +36,14 @@ public class Transform2d implements ProtobufSerializable, StructSerializable {
    * @param last The final pose for the transformation.
    */
   public Transform2d(Pose2d initial, Pose2d last) {
-    // We are rotating the difference between the translations
-    // using a clockwise rotation matrix. This transforms the global
-    // delta into a local delta (relative to the initial pose).
+    // To transform the global translation delta to be relative to the initial
+    // pose, rotate by the inverse of the initial pose's orientation.
     m_translation =
         last.getTranslation()
             .minus(initial.getTranslation())
             .rotateBy(initial.getRotation().unaryMinus());
 
-    m_rotation = last.getRotation().minus(initial.getRotation());
+    m_rotation = last.getRotation().relativeTo(initial.getRotation());
   }
 
   /**

wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java

@@ -17,7 +17,10 @@ import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Objects;
 
-/** Represents a transformation for a Pose3d in the pose's frame. */
+/**
+ * Represents a transformation for a Pose3d in the pose's frame. Translation is applied before
+ * rotation. (The translation is applied in the pose's original frame, not the transformed frame.)
+ */
 public class Transform3d implements ProtobufSerializable, StructSerializable {
   /**
    * A preallocated Transform3d representing no transformation.
@@ -36,15 +39,14 @@ public class Transform3d implements ProtobufSerializable, StructSerializable {
    * @param last The final pose for the transformation.
    */
   public Transform3d(Pose3d initial, Pose3d last) {
-    // We are rotating the difference between the translations
-    // using a clockwise rotation matrix. This transforms the global
-    // delta into a local delta (relative to the initial pose).
+    // To transform the global translation delta to be relative to the initial
+    // pose, rotate by the inverse of the initial pose's orientation.
     m_translation =
         last.getTranslation()
             .minus(initial.getTranslation())
             .rotateBy(initial.getRotation().unaryMinus());
 
-    m_rotation = last.getRotation().minus(initial.getRotation());
+    m_rotation = last.getRotation().relativeTo(initial.getRotation());
   }
 
   /**

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

@@ -24,7 +24,10 @@ public class Odometry<T> {
   private Pose2d m_poseMeters;
 
   private Rotation2d m_gyroOffset;
+
+  // Always equal to m_poseMeters.getRotation()
   private Rotation2d m_previousAngle;
+
   private final T m_previousWheelPositions;
 
   /**
@@ -42,7 +45,7 @@ public class Odometry<T> {
       Pose2d initialPoseMeters) {
     m_kinematics = kinematics;
     m_poseMeters = initialPoseMeters;
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+    m_gyroOffset = gyroAngle.unaryMinus().rotateBy(m_poseMeters.getRotation());
     m_previousAngle = m_poseMeters.getRotation();
     m_previousWheelPositions = m_kinematics.copy(wheelPositions);
   }
@@ -60,7 +63,7 @@ public class Odometry<T> {
   public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMeters) {
     m_poseMeters = poseMeters;
     m_previousAngle = m_poseMeters.getRotation();
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+    m_gyroOffset = gyroAngle.unaryMinus().rotateBy(m_poseMeters.getRotation());
     m_kinematics.copyInto(wheelPositions, m_previousWheelPositions);
   }
 
@@ -70,7 +73,10 @@ public class Odometry<T> {
    * @param poseMeters The pose to reset to.
    */
   public void resetPose(Pose2d poseMeters) {
-    m_gyroOffset = m_gyroOffset.plus(poseMeters.getRotation().minus(m_poseMeters.getRotation()));
+    m_gyroOffset =
+        m_gyroOffset
+            .rotateBy(m_poseMeters.getRotation().unaryMinus())
+            .rotateBy(poseMeters.getRotation());
     m_poseMeters = poseMeters;
     m_previousAngle = m_poseMeters.getRotation();
   }
@@ -90,7 +96,8 @@ public class Odometry<T> {
    * @param rotation The rotation to reset to.
    */
   public void resetRotation(Rotation2d rotation) {
-    m_gyroOffset = m_gyroOffset.plus(rotation.minus(m_poseMeters.getRotation()));
+    m_gyroOffset =
+        m_gyroOffset.rotateBy(m_poseMeters.getRotation().unaryMinus()).rotateBy(rotation);
     m_poseMeters = new Pose2d(m_poseMeters.getTranslation(), rotation);
     m_previousAngle = m_poseMeters.getRotation();
   }
@@ -115,7 +122,7 @@ public class Odometry<T> {
    * @return The new pose of the robot.
    */
   public Pose2d update(Rotation2d gyroAngle, T wheelPositions) {
-    var angle = gyroAngle.plus(m_gyroOffset);
+    var angle = gyroAngle.rotateBy(m_gyroOffset);
 
     var twist = m_kinematics.toTwist2d(m_previousWheelPositions, wheelPositions);
     twist.dtheta = angle.minus(m_previousAngle).getRadians();

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

@@ -32,8 +32,15 @@ public class Odometry3d<T> {
   private final Kinematics<?, T> m_kinematics;
   private Pose3d m_poseMeters;
 
+  // Applying a rotation intrinsically to the measured gyro angle should cause the corrected angle
+  // to be rotated intrinsically in the same way, so the measured gyro angle must be applied
+  // intrinsically. This is equivalent to applying the offset extrinsically to the measured gyro
+  // angle.
   private Rotation3d m_gyroOffset;
+
+  // Always equal to m_poseMeters.getRotation()
   private Rotation3d m_previousAngle;
+
   private final T m_previousWheelPositions;
 
   /**
@@ -51,7 +58,9 @@ public class Odometry3d<T> {
       Pose3d initialPoseMeters) {
     m_kinematics = kinematics;
     m_poseMeters = initialPoseMeters;
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+    // When applied extrinsically, m_gyroOffset cancels the current gyroAngle and
+    // then rotates to m_poseMeters.getRotation()
+    m_gyroOffset = gyroAngle.unaryMinus().rotateBy(m_poseMeters.getRotation());
     m_previousAngle = m_poseMeters.getRotation();
     m_previousWheelPositions = m_kinematics.copy(wheelPositions);
   }
@@ -69,7 +78,9 @@ public class Odometry3d<T> {
   public void resetPosition(Rotation3d gyroAngle, T wheelPositions, Pose3d poseMeters) {
     m_poseMeters = poseMeters;
     m_previousAngle = m_poseMeters.getRotation();
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+    // When applied extrinsically, m_gyroOffset cancels the current gyroAngle and
+    // then rotates to m_poseMeters.getRotation()
+    m_gyroOffset = gyroAngle.unaryMinus().rotateBy(m_poseMeters.getRotation());
     m_kinematics.copyInto(wheelPositions, m_previousWheelPositions);
   }
 
@@ -79,7 +90,11 @@ public class Odometry3d<T> {
    * @param poseMeters The pose to reset to.
    */
   public void resetPose(Pose3d poseMeters) {
-    m_gyroOffset = m_gyroOffset.plus(poseMeters.getRotation().minus(m_poseMeters.getRotation()));
+    // Cancel the previous m_pose.Rotation() and then rotate to the new angle
+    m_gyroOffset =
+        m_gyroOffset
+            .rotateBy(m_poseMeters.getRotation().unaryMinus())
+            .rotateBy(poseMeters.getRotation());
     m_poseMeters = poseMeters;
     m_previousAngle = m_poseMeters.getRotation();
   }
@@ -99,7 +114,9 @@ public class Odometry3d<T> {
    * @param rotation The rotation to reset to.
    */
   public void resetRotation(Rotation3d rotation) {
-    m_gyroOffset = m_gyroOffset.plus(rotation.minus(m_poseMeters.getRotation()));
+    // Cancel the previous m_pose.Rotation() and then rotate to the new angle
+    m_gyroOffset =
+        m_gyroOffset.rotateBy(m_poseMeters.getRotation().unaryMinus()).rotateBy(rotation);
     m_poseMeters = new Pose3d(m_poseMeters.getTranslation(), rotation);
     m_previousAngle = m_poseMeters.getRotation();
   }
@@ -124,7 +141,7 @@ public class Odometry3d<T> {
    * @return The new pose of the robot.
    */
   public Pose3d update(Rotation3d gyroAngle, T wheelPositions) {
-    var angle = gyroAngle.plus(m_gyroOffset);
+    var angle = gyroAngle.rotateBy(m_gyroOffset);
     var angle_difference = angle.minus(m_previousAngle).toVector();
 
     var twist2d = m_kinematics.toTwist2d(m_previousWheelPositions, wheelPositions);

wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java

@@ -4,6 +4,8 @@
 
 package edu.wpi.first.math.system;
 
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Num;
 import edu.wpi.first.math.StateSpaceUtil;
@@ -132,6 +134,7 @@ public class LinearSystemLoop<States extends Num, Inputs extends Num, Outputs ex
 
     m_nextR = new Matrix<>(new SimpleMatrix(controller.getK().getNumCols(), 1));
     reset(m_nextR);
+    MathSharedStore.getMathShared().reportUsage(MathUsageId.kSystem_LinearSystemLoop, 1);
   }
 
   /**

wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java

@@ -7,6 +7,7 @@ package edu.wpi.first.math.trajectory;
 import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.trajectory.struct.TrapezoidProfileStateStruct;
+import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Objects;
 
 /**
@@ -76,7 +77,7 @@ public class TrapezoidProfile {
   }
 
   /** Profile state. */
-  public static class State {
+  public static class State implements StructSerializable {
     /** The struct used to serialize this class. */
     public static final TrapezoidProfileStateStruct struct = new TrapezoidProfileStateStruct();
 

wpimath/src/main/native/cpp/filter/EdgeCountFilter.cpp

@@ -0,0 +1,46 @@
+// 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/filter/EdgeCountFilter.h"
+
+#include "wpimath/MathShared.h"
+
+using namespace frc;
+
+EdgeCounterFilter::EdgeCounterFilter(int requiredEdges, units::second_t window)
+    : m_requiredEdges(requiredEdges), m_windowTime(window) {
+  ResetTimer();
+}
+
+void EdgeCounterFilter::ResetTimer() {
+  m_firstEdgeTime = wpi::math::MathSharedStore::GetTimestamp();
+}
+
+bool EdgeCounterFilter::HasElapsed() const {
+  return wpi::math::MathSharedStore::GetTimestamp() - m_firstEdgeTime >=
+         m_windowTime;
+}
+
+bool EdgeCounterFilter::Calculate(bool input) {
+  bool enoughEdges = m_currentCount >= m_requiredEdges;
+
+  bool expired = HasElapsed() && !enoughEdges;
+  bool activationEnded = !input && enoughEdges;
+
+  if (expired || activationEnded) {
+    m_currentCount = 0;
+  }
+
+  if (input && !m_lastInput) {
+    if (m_currentCount == 0) {
+      ResetTimer();  // Start timer on first rising edge
+    }
+
+    m_currentCount++;
+  }
+
+  m_lastInput = input;
+
+  return input && m_currentCount >= m_requiredEdges;
+}

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

@@ -135,6 +135,8 @@ class LinearQuadraticRegulator {
     }
 
     Reset();
+    wpi::math::MathSharedStore::ReportUsage(
+        wpi::math::MathUsageId::kController_LinearQuadraticRegulator, 1);
   }
 
   /**
@@ -194,6 +196,8 @@ class LinearQuadraticRegulator {
     }
 
     Reset();
+    wpi::math::MathSharedStore::ReportUsage(
+        wpi::math::MathUsageId::kController_LinearQuadraticRegulator, 1);
   }
 
   LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;

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

@@ -137,6 +137,8 @@ class ExtendedKalmanFilter {
       m_initP = StateMatrix::Zero();
     }
     m_P = m_initP;
+    wpi::math::MathSharedStore::ReportUsage(
+        wpi::math::MathUsageId::kEstimator_KalmanFilter, 2);
   }
 
   /**
@@ -221,6 +223,8 @@ class ExtendedKalmanFilter {
       m_initP = StateMatrix::Zero();
     }
     m_P = m_initP;
+    wpi::math::MathSharedStore::ReportUsage(
+        wpi::math::MathUsageId::kEstimator_KalmanFilter, 2);
   }
 
   /**

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

@@ -118,6 +118,8 @@ class KalmanFilter {
     }
 
     Reset();
+    wpi::math::MathSharedStore::ReportUsage(
+        wpi::math::MathUsageId::kEstimator_KalmanFilter, 1);
   }
 
   /**

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

@@ -70,6 +70,8 @@ class WPILIB_DLLEXPORT PoseEstimator {
     }
 
     SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    wpi::math::MathSharedStore::ReportUsage(
+        wpi::math::MathUsageId::kEstimator_PoseEstimator, 1);
   }
 
   /**
@@ -137,24 +139,70 @@ class WPILIB_DLLEXPORT PoseEstimator {
    *
    * @param translation The pose to translation to.
    */
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
+#endif  // defined(__GNUC__) && !defined(__clang__)
   void ResetTranslation(const Translation2d& translation) {
     m_odometry.ResetTranslation(translation);
+
+    const std::optional<std::pair<units::second_t, VisionUpdate>>
+        latestVisionUpdate =
+            m_visionUpdates.empty() ? std::nullopt
+                                    : std::optional{*m_visionUpdates.crbegin()};
     m_odometryPoseBuffer.Clear();
     m_visionUpdates.clear();
-    m_poseEstimate = m_odometry.GetPose();
+
+    if (latestVisionUpdate) {
+      // apply vision compensation to the pose rotation
+      const VisionUpdate visionUpdate{
+          Pose2d{translation, latestVisionUpdate->second.visionPose.Rotation()},
+          Pose2d{translation,
+                 latestVisionUpdate->second.odometryPose.Rotation()}};
+      m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
+      m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
+    } else {
+      m_poseEstimate = m_odometry.GetPose();
+    }
   }
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic pop
+#endif  // defined(__GNUC__) && !defined(__clang__)
 
   /**
    * Resets the robot's rotation.
    *
    * @param rotation The rotation to reset to.
    */
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
+#endif  // defined(__GNUC__) && !defined(__clang__)
   void ResetRotation(const Rotation2d& rotation) {
     m_odometry.ResetRotation(rotation);
+
+    const std::optional<std::pair<units::second_t, VisionUpdate>>
+        latestVisionUpdate =
+            m_visionUpdates.empty() ? std::nullopt
+                                    : std::optional{*m_visionUpdates.crbegin()};
     m_odometryPoseBuffer.Clear();
     m_visionUpdates.clear();
-    m_poseEstimate = m_odometry.GetPose();
+
+    if (latestVisionUpdate) {
+      // apply vision compensation to the pose translation
+      const VisionUpdate visionUpdate{
+          Pose2d{latestVisionUpdate->second.visionPose.Translation(), rotation},
+          Pose2d{latestVisionUpdate->second.odometryPose.Translation(),
+                 rotation}};
+      m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
+      m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
+    } else {
+      m_poseEstimate = m_odometry.GetPose();
+    }
   }
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic pop
+#endif  // defined(__GNUC__) && !defined(__clang__)
 
   /**
    * Gets the estimated robot pose.
@@ -434,7 +482,9 @@ class WPILIB_DLLEXPORT PoseEstimator {
   TimeInterpolatableBuffer<Pose2d> m_odometryPoseBuffer{kBufferDuration};
   // Maps timestamps to vision updates
   // Always contains one entry before the oldest entry in m_odometryPoseBuffer,
-  // unless there have been no vision measurements after the last reset
+  // unless there have been no vision measurements after the last reset. May
+  // contain one entry while m_odometryPoseBuffer is empty to correct for
+  // translation/rotation after a call to ResetRotation/ResetTranslation.
   std::map<units::second_t, VisionUpdate> m_visionUpdates;
 
   Pose2d m_poseEstimate;

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

@@ -76,6 +76,8 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
     }
 
     SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    wpi::math::MathSharedStore::ReportUsage(
+        wpi::math::MathUsageId::kEstimator_PoseEstimator3d, 1);
   }
 
   /**
@@ -146,24 +148,70 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
    *
    * @param translation The pose to translation to.
    */
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
+#endif  // defined(__GNUC__) && !defined(__clang__)
   void ResetTranslation(const Translation3d& translation) {
     m_odometry.ResetTranslation(translation);
+
+    const std::optional<std::pair<units::second_t, VisionUpdate>>
+        latestVisionUpdate =
+            m_visionUpdates.empty() ? std::nullopt
+                                    : std::optional{*m_visionUpdates.crbegin()};
     m_odometryPoseBuffer.Clear();
     m_visionUpdates.clear();
-    m_poseEstimate = m_odometry.GetPose();
+
+    if (latestVisionUpdate) {
+      // apply vision compensation to the pose rotation
+      const VisionUpdate visionUpdate{
+          Pose3d{translation, latestVisionUpdate->second.visionPose.Rotation()},
+          Pose3d{translation,
+                 latestVisionUpdate->second.odometryPose.Rotation()}};
+      m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
+      m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
+    } else {
+      m_poseEstimate = m_odometry.GetPose();
+    }
   }
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic pop
+#endif  // defined(__GNUC__) && !defined(__clang__)
 
   /**
    * Resets the robot's rotation.
    *
    * @param rotation The rotation to reset to.
    */
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
+#endif  // defined(__GNUC__) && !defined(__clang__)
   void ResetRotation(const Rotation3d& rotation) {
     m_odometry.ResetRotation(rotation);
+
+    const std::optional<std::pair<units::second_t, VisionUpdate>>
+        latestVisionUpdate =
+            m_visionUpdates.empty() ? std::nullopt
+                                    : std::optional{*m_visionUpdates.crbegin()};
     m_odometryPoseBuffer.Clear();
     m_visionUpdates.clear();
-    m_poseEstimate = m_odometry.GetPose();
+
+    if (latestVisionUpdate) {
+      // apply vision compensation to the pose translation
+      const VisionUpdate visionUpdate{
+          Pose3d{latestVisionUpdate->second.visionPose.Translation(), rotation},
+          Pose3d{latestVisionUpdate->second.odometryPose.Translation(),
+                 rotation}};
+      m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
+      m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
+    } else {
+      m_poseEstimate = m_odometry.GetPose();
+    }
   }
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic pop
+#endif  // defined(__GNUC__) && !defined(__clang__)
 
   /**
    * Gets the estimated robot pose.
@@ -449,7 +497,9 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
   TimeInterpolatableBuffer<Pose3d> m_odometryPoseBuffer{kBufferDuration};
   // Maps timestamps to vision updates
   // Always contains one entry before the oldest entry in m_odometryPoseBuffer,
-  // unless there have been no vision measurements after the last reset
+  // unless there have been no vision measurements after the last reset. May
+  // contain one entry while m_odometryPoseBuffer is empty to correct for
+  // translation/rotation after a call to ResetRotation/ResetTranslation.
   std::map<units::second_t, VisionUpdate> m_visionUpdates;
 
   Pose3d m_poseEstimate;