[wpimath] Add limit setters to SlewRateLimiter (#8581)

This is just #7793 with requested changes applied.

.github/workflows/gradle.yml

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

.github/workflows/sentinel-build.yml

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

.github/workflows/tools.yml

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

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

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

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

docs/build.gradle

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

docs/opencv/element-list

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

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/FMS.cpp

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

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

@@ -36,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
@@ -69,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

@@ -134,3 +134,4 @@ 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

@@ -291,6 +291,8 @@ public final class FRCNetComm {
     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;
   }
 
   /**
@@ -377,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. */
@@ -443,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

@@ -187,6 +187,7 @@ namespace HALUsageReporting {
     kResourceType_LinearSystemLoop = 133,
     kResourceType_LumynLabs_ConnectorX = 134,
     kResourceType_LumynLabs_ConnectorXAnimate = 135,
+    kResourceType_RevMAXSplineEncoder = 136,
   };
   enum tInstances : int32_t {
     kLanguage_LabVIEW = 1,
@@ -227,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,
@@ -260,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

@@ -156,6 +156,7 @@ typedef enum
     kResourceType_LinearSystemLoop = 133,
     kResourceType_LumynLabs_ConnectorX = 134,
     kResourceType_LumynLabs_ConnectorXAnimate = 135,
+    kResourceType_RevMAXSplineEncoder = 136,
 
 //    kResourceType_MaximumID = 255,
 } tResourceType;
@@ -200,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,
@@ -233,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,

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

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

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

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

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

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

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

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

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

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

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

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

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

wpilibNewCommands/src/main/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/WaitUntilCommand.cpp

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

wpilibNewCommands/src/main/native/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/WaitUntilCommand.h

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

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/cppcs/RobotBase.cpp

@@ -306,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/DriverStation.h

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

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

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

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

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

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

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

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

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

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

@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc/TimedRobot.h>
+#include <frc/Timer.h>
+#include <frc/XboxController.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc/xrp/XRPMotor.h>
+
+class Robot : public frc::TimedRobot {
+ public:
+  Robot();
+  void RobotPeriodic() override;
+  void AutonomousInit() override;
+  void AutonomousPeriodic() override;
+  void TeleopInit() override;
+  void TeleopPeriodic() override;
+
+ private:
+  frc::XRPMotor m_leftMotor{0};
+  frc::XRPMotor m_rightMotor{1};
+  frc::XboxController m_XboxController{0};
+  frc::Timer m_timer;
+
+  frc::DifferentialDrive m_drive{
+      [&](double output) { m_leftMotor.Set(output); },
+      [&](double output) { m_rightMotor.Set(output); }};
+};

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

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

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

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

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

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

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

@@ -211,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(

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

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

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

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

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

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

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

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

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/xrptimed/Main.java

@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.xrptimed;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

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

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

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

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

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

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

wpimath/src/main/java/edu/wpi/first/math/filter/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/filter/SlewRateLimiter.java

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

wpimath/src/main/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/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/estimator/PoseEstimator.h

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

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

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

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

@@ -0,0 +1,94 @@
+// 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 <wpi/SymbolExports.h>
+
+#include "units/time.h"
+
+namespace frc {
+/**
+ * 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.
+ *
+ * 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.
+ *
+ * Input must be stable; consider using a Debouncer before this filter to avoid
+ * counting noise as multiple edges.
+ */
+class WPILIB_DLLEXPORT EdgeCounterFilter {
+ public:
+  /**
+   * Creates a new EdgeCounterFilter.
+   *
+   * @param requiredEdges The number of rising edges required before the output
+   *                      goes true.
+   * @param windowTime The maximum time window in which all required edges must
+   *               occur after the first rising edge.
+   */
+  explicit EdgeCounterFilter(int requiredEdges, units::second_t windowTime);
+
+  /**
+   * 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.
+   */
+  bool Calculate(bool input);
+
+  /**
+   * Sets the time window duration.
+   *
+   * @param windowTime The maximum time window in which all required edges must
+   *               occur after the first rising edge.
+   */
+  constexpr void SetWindowTime(units::second_t windowTime) {
+    m_windowTime = windowTime;
+  }
+
+  /**
+   * Gets the time window duration.
+   *
+   * @return The maximum time window in which all required edges must occur
+   *         after the first rising edge.
+   */
+  constexpr units::second_t GetWindowTime() const { return m_windowTime; }
+
+  /**
+   * Sets the required number of edges.
+   *
+   * @param requiredEdges The number of rising edges required before the output
+   *                      goes true.
+   */
+  constexpr 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.
+   */
+  constexpr int GetRequiredEdges() const { return m_requiredEdges; }
+
+ private:
+  int m_requiredEdges;
+  units::second_t m_windowTime;
+
+  units::second_t m_firstEdgeTime;
+  int m_currentCount = 0;
+
+  bool m_lastInput = false;
+
+  void ResetTimer();
+
+  bool HasElapsed() const;
+};
+}  // namespace frc

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

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

wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h

@@ -86,7 +86,8 @@ class WPILIB_DLLEXPORT CoordinateSystem {
   constexpr static Translation3d Convert(const Translation3d& translation,
                                          const CoordinateSystem& from,
                                          const CoordinateSystem& to) {
-    return translation.RotateBy(from.m_rotation - to.m_rotation);
+    // Convert to NWU, then convert to the new coordinate system
+    return translation.RotateBy(from.m_rotation).RotateBy(-to.m_rotation);
   }
 
   /**
@@ -100,7 +101,8 @@ class WPILIB_DLLEXPORT CoordinateSystem {
   constexpr static Rotation3d Convert(const Rotation3d& rotation,
                                       const CoordinateSystem& from,
                                       const CoordinateSystem& to) {
-    return rotation.RotateBy(from.m_rotation - to.m_rotation);
+    // Convert to NWU, then convert to the new coordinate system
+    return rotation.RotateBy(from.m_rotation).RotateBy(-to.m_rotation);
   }
 
   /**
@@ -129,14 +131,30 @@ class WPILIB_DLLEXPORT CoordinateSystem {
   constexpr static Transform3d Convert(const Transform3d& transform,
                                        const CoordinateSystem& from,
                                        const CoordinateSystem& to) {
-    const auto coordRot = from.m_rotation - 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.
+    const auto coordRot = from.m_rotation.RotateBy(-to.m_rotation);
+    // 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.
     return Transform3d{
         Convert(transform.Translation(), from, to),
         (-coordRot).RotateBy(transform.Rotation().RotateBy(coordRot))};
   }
 
  private:
-  // Rotation from this coordinate system to NWU coordinate system
+  // Rotation from this coordinate system to NWU coordinate system when applied
+  // extrinsically
   Rotation3d m_rotation;
 };
 

wpimath/src/main/native/include/frc/geometry/Pose2d.h

@@ -320,7 +320,7 @@ constexpr Transform2d Pose2d::operator-(const Pose2d& other) const {
 
 constexpr Pose2d Pose2d::TransformBy(const frc::Transform2d& other) const {
   return {m_translation + (other.Translation().RotateBy(m_rotation)),
-          other.Rotation() + m_rotation};
+          other.Rotation().RotateBy(m_rotation)};
 }
 
 constexpr Pose2d Pose2d::RelativeTo(const Pose2d& other) const {

wpimath/src/main/native/include/frc/geometry/Pose3d.h

@@ -392,8 +392,11 @@ constexpr Transform3d Pose3d::operator-(const Pose3d& other) const {
 }
 
 constexpr Pose3d Pose3d::TransformBy(const Transform3d& other) const {
+  // 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 {m_translation + (other.Translation().RotateBy(m_rotation)),
-          other.Rotation() + m_rotation};
+          other.Rotation().RotateBy(m_rotation)};
 }
 
 constexpr Pose3d Pose3d::RelativeTo(const Pose3d& other) const {

wpimath/src/main/native/include/frc/geometry/Rotation2d.h

@@ -198,6 +198,19 @@ class WPILIB_DLLEXPORT Rotation2d {
             Cos() * other.Sin() + Sin() * other.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.
+   */
+  constexpr Rotation2d RelativeTo(const Rotation2d& other) const {
+    return RotateBy(-other);
+  }
+
   /**
    * Returns matrix representation of this rotation.
    */

wpimath/src/main/native/include/frc/geometry/Rotation3d.h

@@ -10,6 +10,7 @@
 #include <Eigen/Core>
 #include <fmt/format.h>
 #include <gcem.hpp>
+#include <wpi/MathExtras.h>
 #include <wpi/SymbolExports.h>
 #include <wpi/json_fwd.h>
 
@@ -24,7 +25,49 @@
 namespace frc {
 
 /**
- * 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.
+ *
+ * As an example of the order of rotations mattering, suppose we have a card
+ * that looks like this:
+ *
+ * <pre>
+ *          ┌───┐        ┌───┐
+ *          │ X │        │ x │
+ *   Front: │ | │  Back: │ · │
+ *          │ | │        │ · │
+ *          └───┘        └───┘
+ * </pre>
+ *
+ * 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>
+ *
+ * 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>
+ *
+ * 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.
  */
 class WPILIB_DLLEXPORT Rotation3d {
  public:
@@ -242,9 +285,18 @@ class WPILIB_DLLEXPORT Rotation3d {
       : Rotation3d{0_rad, 0_rad, rotation.Radians()} {}
 
   /**
-   * 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.
+   * Note that `a - b + b` always equals `a`, but `b + (a - b)`
+   * sometimes doesn't. To apply a rotation offset, use either `offset =
+   * -measurement + actual; newAngle = angle + offset;` or `offset = actual -
+   * measurement; newAngle = offset + 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.
    */
@@ -254,11 +306,20 @@ class WPILIB_DLLEXPORT Rotation3d {
 
   /**
    * Subtracts the new rotation from the current rotation and returns the new
-   * rotation.
+   * rotation. The new rotation is from the perspective of the other rotation
+   * (like Pose3d::operator-), so it needs to be applied intrinsically. See the
+   * class comment for definitions of extrinsic and intrinsic rotations.
+   *
+   * Note that `a - b + b` always equals `a`, but `b + (a - b)` sometimes
+   * doesn't. To apply a rotation offset, use either `offset = -measurement +
+   * actual; newAngle = angle + offset;` or `offset = actual - measurement;
+   * newAngle = offset + 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.
    */
   constexpr Rotation3d operator-(const Rotation3d& other) const {
     return *this + -other;
@@ -323,6 +384,28 @@ class WPILIB_DLLEXPORT Rotation3d {
     return Rotation3d{other.m_q * 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.
+   */
+  constexpr Rotation3d RelativeTo(const Rotation3d& other) const {
+    // 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 Rotation3d{other.m_q.Inverse() * m_q};
+  }
+
   /**
    * Returns the quaternion representation of the Rotation3d.
    */
@@ -449,5 +532,11 @@ void from_json(const wpi::json& json, Rotation3d& rotation);
 
 }  // namespace frc
 
+template <>
+constexpr frc::Rotation3d wpi::Lerp(const frc::Rotation3d& startValue,
+                                    const frc::Rotation3d& endValue, double t) {
+  return (endValue - startValue) * t + startValue;
+}
+
 #include "frc/geometry/proto/Rotation3dProto.h"
 #include "frc/geometry/struct/Rotation3dStruct.h"

wpimath/src/main/native/include/frc/geometry/Transform2d.h

@@ -167,13 +167,12 @@ class WPILIB_DLLEXPORT Transform2d {
 namespace frc {
 
 constexpr Transform2d::Transform2d(const Pose2d& initial, const Pose2d& final) {
-  // 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 = (final.Translation() - initial.Translation())
                       .RotateBy(-initial.Rotation());
 
-  m_rotation = final.Rotation() - initial.Rotation();
+  m_rotation = final.Rotation().RelativeTo(initial.Rotation());
 }
 
 constexpr Transform2d Transform2d::operator+(const Transform2d& other) const {

wpimath/src/main/native/include/frc/geometry/Transform3d.h

@@ -16,7 +16,9 @@ namespace frc {
 class Pose3d;
 
 /**
- * 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.)
  */
 class WPILIB_DLLEXPORT Transform3d {
  public:
@@ -192,13 +194,12 @@ class WPILIB_DLLEXPORT Transform3d {
 namespace frc {
 
 constexpr Transform3d::Transform3d(const Pose3d& initial, const Pose3d& final) {
-  // 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 = (final.Translation() - initial.Translation())
                       .RotateBy(-initial.Rotation());
 
-  m_rotation = final.Rotation() - initial.Rotation();
+  m_rotation = final.Rotation().RelativeTo(initial.Rotation());
 }
 
 constexpr Transform3d Transform3d::operator+(const Transform3d& other) const {

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

@@ -14,6 +14,8 @@
 #include <wpi/SymbolExports.h>
 
 #include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Pose3d.h"
+#include "frc/geometry/Rotation3d.h"
 #include "units/time.h"
 
 namespace frc {
@@ -155,7 +157,8 @@ class TimeInterpolatableBuffer {
   std::function<T(const T&, const T&, double)> m_interpolatingFunc;
 };
 
-// Template specialization to ensure that Pose2d uses pose exponential
+// Template specializations to ensure that Pose2d and Pose3d use pose
+// exponential
 template <>
 inline TimeInterpolatableBuffer<Pose2d>::TimeInterpolatableBuffer(
     units::second_t historySize)
@@ -172,4 +175,20 @@ inline TimeInterpolatableBuffer<Pose2d>::TimeInterpolatableBuffer(
         }
       }) {}
 
+template <>
+inline TimeInterpolatableBuffer<Pose3d>::TimeInterpolatableBuffer(
+    units::second_t historySize)
+    : m_historySize(historySize),
+      m_interpolatingFunc([](const Pose3d& start, const Pose3d& end, double t) {
+        if (t < 0) {
+          return start;
+        } else if (t >= 1) {
+          return end;
+        } else {
+          Twist3d twist = start.Log(end);
+          Twist3d scaledTwist = twist * t;
+          return start.Exp(scaledTwist);
+        }
+      }) {}
+
 }  // namespace frc

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

@@ -45,7 +45,7 @@ class WPILIB_DLLEXPORT Odometry {
         m_pose(initialPose),
         m_previousWheelPositions(wheelPositions) {
     m_previousAngle = m_pose.Rotation();
-    m_gyroOffset = m_pose.Rotation() - gyroAngle;
+    m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation());
   }
 
   /**
@@ -62,7 +62,7 @@ class WPILIB_DLLEXPORT Odometry {
                      const WheelPositions& wheelPositions, const Pose2d& pose) {
     m_pose = pose;
     m_previousAngle = pose.Rotation();
-    m_gyroOffset = m_pose.Rotation() - gyroAngle;
+    m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation());
     m_previousWheelPositions = wheelPositions;
   }
 
@@ -72,7 +72,8 @@ class WPILIB_DLLEXPORT Odometry {
    * @param pose The pose to reset to.
    */
   void ResetPose(const Pose2d& pose) {
-    m_gyroOffset = m_gyroOffset + (pose.Rotation() - m_pose.Rotation());
+    m_gyroOffset =
+        m_gyroOffset.RotateBy(-m_pose.Rotation()).RotateBy(pose.Rotation());
     m_pose = pose;
     m_previousAngle = pose.Rotation();
   }
@@ -92,7 +93,7 @@ class WPILIB_DLLEXPORT Odometry {
    * @param rotation The rotation to reset to.
    */
   void ResetRotation(const Rotation2d& rotation) {
-    m_gyroOffset = m_gyroOffset + (rotation - m_pose.Rotation());
+    m_gyroOffset = m_gyroOffset.RotateBy(m_pose.Rotation()).RotateBy(rotation);
     m_pose = Pose2d{m_pose.Translation(), rotation};
     m_previousAngle = rotation;
   }
@@ -116,7 +117,7 @@ class WPILIB_DLLEXPORT Odometry {
    */
   const Pose2d& Update(const Rotation2d& gyroAngle,
                        const WheelPositions& wheelPositions) {
-    auto angle = gyroAngle + m_gyroOffset;
+    auto angle = gyroAngle.RotateBy(m_gyroOffset);
 
     auto twist =
         m_kinematics.ToTwist2d(m_previousWheelPositions, wheelPositions);
@@ -136,7 +137,10 @@ class WPILIB_DLLEXPORT Odometry {
   Pose2d m_pose;
 
   WheelPositions m_previousWheelPositions;
+
+  // Always equal to m_pose.Rotation()
   Rotation2d m_previousAngle;
+
   Rotation2d m_gyroOffset;
 };
 

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

@@ -48,7 +48,9 @@ class WPILIB_DLLEXPORT Odometry3d {
         m_pose(initialPose),
         m_previousWheelPositions(wheelPositions) {
     m_previousAngle = m_pose.Rotation();
-    m_gyroOffset = m_pose.Rotation() - gyroAngle;
+    // When applied extrinsically, m_gyroOffset cancels the
+    // current gyroAngle and then rotates to m_pose.Rotation()
+    m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation());
   }
 
   /**
@@ -65,7 +67,9 @@ class WPILIB_DLLEXPORT Odometry3d {
                      const WheelPositions& wheelPositions, const Pose3d& pose) {
     m_pose = pose;
     m_previousAngle = pose.Rotation();
-    m_gyroOffset = m_pose.Rotation() - gyroAngle;
+    // When applied extrinsically, m_gyroOffset cancels the
+    // current gyroAngle and then rotates to m_pose.Rotation()
+    m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation());
     m_previousWheelPositions = wheelPositions;
   }
 
@@ -75,7 +79,9 @@ class WPILIB_DLLEXPORT Odometry3d {
    * @param pose The pose to reset to.
    */
   void ResetPose(const Pose3d& pose) {
-    m_gyroOffset = m_gyroOffset + (pose.Rotation() - m_pose.Rotation());
+    // Cancel the previous m_pose.Rotation() and then rotate to the new angle
+    m_gyroOffset =
+        m_gyroOffset.RotateBy(-m_pose.Rotation()).RotateBy(pose.Rotation());
     m_pose = pose;
     m_previousAngle = pose.Rotation();
   }
@@ -95,7 +101,8 @@ class WPILIB_DLLEXPORT Odometry3d {
    * @param rotation The rotation to reset to.
    */
   void ResetRotation(const Rotation3d& rotation) {
-    m_gyroOffset = m_gyroOffset + (rotation - m_pose.Rotation());
+    // Cancel the previous m_pose.Rotation() and then rotate to the new angle
+    m_gyroOffset = m_gyroOffset.RotateBy(-m_pose.Rotation()).RotateBy(rotation);
     m_pose = Pose3d{m_pose.Translation(), rotation};
     m_previousAngle = rotation;
   }
@@ -119,7 +126,7 @@ class WPILIB_DLLEXPORT Odometry3d {
    */
   const Pose3d& Update(const Rotation3d& gyroAngle,
                        const WheelPositions& wheelPositions) {
-    auto angle = gyroAngle + m_gyroOffset;
+    auto angle = gyroAngle.RotateBy(m_gyroOffset);
     auto angle_difference = (angle - m_previousAngle).ToVector();
 
     auto twist2d =
@@ -145,7 +152,14 @@ class WPILIB_DLLEXPORT Odometry3d {
   Pose3d m_pose;
 
   WheelPositions m_previousWheelPositions;
+
+  // Always equal to m_pose.Rotation()
   Rotation3d m_previousAngle;
+
+  // 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.
   Rotation3d m_gyroOffset;
 };
 

wpimath/src/test/java/edu/wpi/first/math/filter/EdgeCounterFilterTest.java

@@ -0,0 +1,82 @@
+// 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 static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.util.WPIUtilJNI;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+class EdgeCounterFilterTest {
+  @BeforeEach
+  void setUp() {
+    WPIUtilJNI.enableMockTime();
+    WPIUtilJNI.setMockTime(0L);
+  }
+
+  @AfterEach
+  void tearDown() {
+    WPIUtilJNI.setMockTime(0L);
+    WPIUtilJNI.disableMockTime();
+  }
+
+  @Test
+  void edgeCounterFilterActivatedTest() {
+    var filter = new EdgeCounterFilter(2, 0.2);
+
+    assertFalse(filter.calculate(true)); // First edge
+
+    WPIUtilJNI.setMockTime(50_000L);
+    assertFalse(filter.calculate(false)); // First edge ended
+
+    WPIUtilJNI.setMockTime(100_000L);
+    assertTrue(filter.calculate(true)); // Second edge
+
+    WPIUtilJNI.setMockTime(150_000L);
+    assertTrue(filter.calculate(true)); // Still true
+
+    WPIUtilJNI.setMockTime(250_000L);
+    assertTrue(filter.calculate(true)); // Still true
+
+    WPIUtilJNI.setMockTime(300_000L);
+    assertFalse(filter.calculate(false)); // Input false, should reset
+  }
+
+  @Test
+  void edgeCounterFilterExpiredTest() {
+    var filter = new EdgeCounterFilter(2, 0.2);
+
+    assertFalse(filter.calculate(true)); // First edge
+
+    WPIUtilJNI.setMockTime(50_000L);
+    filter.calculate(false); // First edge ended
+
+    WPIUtilJNI.setMockTime(250_000L);
+    assertFalse(filter.calculate(true)); // Second edge after window expired
+
+    WPIUtilJNI.setMockTime(300_000L);
+    assertFalse(filter.calculate(true)); // Still false
+  }
+
+  @Test
+  void edgeCounterFilterParamsTest() {
+    var filter = new EdgeCounterFilter(2, 0.2);
+
+    assertEquals(filter.getRequiredEdges(), 2);
+    assertEquals(filter.getWindowTime(), 0.2);
+
+    filter.setRequiredEdges(3);
+
+    assertEquals(filter.getRequiredEdges(), 3);
+
+    filter.setWindowTime(0.5);
+
+    assertEquals(filter.getWindowTime(), 0.5);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java

@@ -59,6 +59,16 @@ class Rotation2dTest {
     assertEquals(120.0, rot.getDegrees(), kEpsilon);
   }
 
+  @Test
+  void testRelativeTo() {
+    var start = Rotation2d.fromDegrees(30.0);
+    var end = Rotation2d.kCCW_Pi_2;
+
+    var result = end.relativeTo(start);
+
+    assertEquals(60.0, result.getDegrees(), kEpsilon);
+  }
+
   @Test
   void testMinus() {
     var rot1 = Rotation2d.fromDegrees(70.0);

wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java

@@ -281,6 +281,22 @@ class Rotation3dTest {
     assertEquals(expected, rot);
   }
 
+  @Test
+  void testRelativeTo() {
+    final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
+    final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var start = new Rotation3d(yAxis, Units.degreesToRadians(-90.0));
+    var end = new Rotation3d(zAxis, Units.degreesToRadians(90.0));
+
+    final var intrinsicAxis = VecBuilder.fill(1.0, 1.0, 1.0);
+    var expected = new Rotation3d(intrinsicAxis, Units.degreesToRadians(120.0));
+
+    var result = end.relativeTo(start);
+
+    assertEquals(expected, result);
+  }
+
   @Test
   void testMinus() {
     final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
@@ -409,5 +425,32 @@ class Rotation3dTest {
     assertEquals(Units.degreesToRadians(0.0), interpolated.getX(), kEpsilon);
     assertEquals(Units.degreesToRadians(0.0), interpolated.getY(), kEpsilon);
     assertEquals(Units.degreesToRadians(-175.0), interpolated.getZ(), kEpsilon);
+
+    // t value of 0 should always produce the start
+    rot1 = new Rotation3d(yAxis, -Units.degreesToRadians(90));
+    rot2 = new Rotation3d(zAxis, Units.degreesToRadians(90));
+    interpolated = rot1.interpolate(rot2, 0.0);
+    assertEquals(rot1.getX(), interpolated.getX(), kEpsilon);
+    assertEquals(rot1.getY(), interpolated.getY(), kEpsilon);
+    assertEquals(rot1.getZ(), interpolated.getZ(), kEpsilon);
+
+    // The full rotation from rot1 to rot2 to 120 degrees around extrinsic <-1.0, 1.0, 1.0>
+    var extrinsicAxis = VecBuilder.fill(-1.0, 1.0, 1.0);
+    rot1 = new Rotation3d(yAxis, -Units.degreesToRadians(90));
+    rot2 = new Rotation3d(zAxis, Units.degreesToRadians(90));
+    assertEquals(rot2, rot1.rotateBy(new Rotation3d(extrinsicAxis, Units.degreesToRadians(120))));
+    interpolated = rot1.interpolate(rot2, 0.5);
+    var expected = rot1.rotateBy(new Rotation3d(extrinsicAxis, Units.degreesToRadians(60)));
+    assertEquals(expected.getX(), interpolated.getX(), kEpsilon);
+    assertEquals(expected.getY(), interpolated.getY(), kEpsilon);
+    assertEquals(expected.getZ(), interpolated.getZ(), kEpsilon);
+
+    // t value of 1 should always produce the end
+    rot1 = new Rotation3d(yAxis, -Units.degreesToRadians(90));
+    rot2 = new Rotation3d(zAxis, Units.degreesToRadians(90));
+    interpolated = rot1.interpolate(rot2, 1.0);
+    assertEquals(rot2.getX(), interpolated.getX(), kEpsilon);
+    assertEquals(rot2.getY(), interpolated.getY(), kEpsilon);
+    assertEquals(rot2.getZ(), interpolated.getZ(), kEpsilon);
   }
 }

wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3dTest.java

@@ -9,6 +9,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
 
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
 import org.junit.jupiter.api.Test;
 
@@ -45,4 +46,22 @@ class DifferentialDriveOdometry3dTest {
         () -> assertEquals(pose.getZ(), 0.0, kEpsilon),
         () -> assertEquals(pose.getRotation().toRotation2d().getDegrees(), 90.0, kEpsilon));
   }
+
+  @Test
+  void testGyroOffset() {
+    m_odometry.resetPosition(
+        new Rotation3d(0, Units.degreesToRadians(5), 0),
+        0,
+        0,
+        new Pose3d(Translation3d.kZero, new Rotation3d(0, 0, Units.degreesToRadians(90))));
+    var pose = m_odometry.update(new Rotation3d(0, Units.degreesToRadians(10), 0), 0, 0);
+
+    assertAll(
+        () -> assertEquals(pose.getX(), 0.0, kEpsilon),
+        () -> assertEquals(pose.getY(), 0.0, kEpsilon),
+        () -> assertEquals(pose.getZ(), 0.0, kEpsilon),
+        () -> assertEquals(pose.getRotation().getX(), Units.degreesToRadians(0), kEpsilon),
+        () -> assertEquals(pose.getRotation().getY(), Units.degreesToRadians(5), kEpsilon),
+        () -> assertEquals(pose.getRotation().getZ(), Units.degreesToRadians(90), kEpsilon));
+  }
 }

wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3dTest.java

@@ -295,4 +295,22 @@ class MecanumDriveOdometry3dTest {
         0.05,
         "Incorrect distance travelled");
   }
+
+  @Test
+  void testGyroOffset() {
+    var wheelPositions = new MecanumDriveWheelPositions();
+    m_odometry.resetPosition(
+        new Rotation3d(0, Units.degreesToRadians(5), 0),
+        wheelPositions,
+        new Pose3d(Translation3d.kZero, new Rotation3d(0, 0, Units.degreesToRadians(90))));
+    var pose = m_odometry.update(new Rotation3d(0, Units.degreesToRadians(10), 0), wheelPositions);
+
+    assertAll(
+        () -> assertEquals(pose.getX(), 0.0, 1e-9),
+        () -> assertEquals(pose.getY(), 0.0, 1e-9),
+        () -> assertEquals(pose.getZ(), 0.0, 1e-9),
+        () -> assertEquals(pose.getRotation().getX(), Units.degreesToRadians(0), 1e-9),
+        () -> assertEquals(pose.getRotation().getY(), Units.degreesToRadians(5), 1e-9),
+        () -> assertEquals(pose.getRotation().getZ(), Units.degreesToRadians(90), 1e-9));
+  }
 }

wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry3dTest.java

@@ -305,4 +305,22 @@ class SwerveDriveOdometry3dTest {
         0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.06, "Incorrect mean error");
     assertEquals(0.0, maxError, 0.125, "Incorrect max error");
   }
+
+  @Test
+  void testGyroOffset() {
+    SwerveModulePosition[] modulePositions = {zero, zero, zero, zero};
+    m_odometry.resetPosition(
+        new Rotation3d(0, Units.degreesToRadians(5), 0),
+        modulePositions,
+        new Pose3d(Translation3d.kZero, new Rotation3d(0, 0, Units.degreesToRadians(90))));
+    var pose = m_odometry.update(new Rotation3d(0, Units.degreesToRadians(10), 0), modulePositions);
+
+    assertAll(
+        () -> assertEquals(pose.getX(), 0.0, 1e-9),
+        () -> assertEquals(pose.getY(), 0.0, 1e-9),
+        () -> assertEquals(pose.getZ(), 0.0, 1e-9),
+        () -> assertEquals(pose.getRotation().getX(), Units.degreesToRadians(0), 1e-9),
+        () -> assertEquals(pose.getRotation().getY(), Units.degreesToRadians(5), 1e-9),
+        () -> assertEquals(pose.getRotation().getZ(), Units.degreesToRadians(90), 1e-9));
+  }
 }

wpimath/src/test/native/cpp/filter/EdgeCountFilterTest.cpp

@@ -0,0 +1,75 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+#include <wpi/timestamp.h>
+
+#include "frc/filter/EdgeCountFilter.h"
+#include "units/time.h"
+
+static units::second_t now = 0_s;
+
+class EdgeCounterFilterTest : public ::testing::Test {
+ protected:
+  void SetUp() override {
+    WPI_SetNowImpl([] { return units::microsecond_t{now}.to<uint64_t>(); });
+    now = 0_ms;
+  }
+
+  void TearDown() override {
+    now = 0_ms;
+    WPI_SetNowImpl(nullptr);
+  }
+};
+
+TEST_F(EdgeCounterFilterTest, EdgeCounterFilterActivated) {
+  frc::EdgeCounterFilter filter{2, 0.2_s};
+
+  EXPECT_FALSE(filter.Calculate(true));  // First edge
+
+  now = 50_ms;
+  EXPECT_FALSE(filter.Calculate(false));  // First edge ended
+
+  now = 100_ms;
+  EXPECT_TRUE(filter.Calculate(true));  // Second edge
+
+  now = 150_ms;
+  EXPECT_TRUE(filter.Calculate(true));  // Still true
+
+  now = 250_ms;
+  EXPECT_TRUE(filter.Calculate(true));  // Still true
+
+  now = 300_ms;
+  EXPECT_FALSE(filter.Calculate(false));  // Input false, should reset
+}
+
+TEST_F(EdgeCounterFilterTest, EdgeCounterFilterExpired) {
+  frc::EdgeCounterFilter filter{2, 0.2_s};
+
+  EXPECT_FALSE(filter.Calculate(true));  // First edge
+
+  now = 50_ms;
+  filter.Calculate(false);  // First edge ended
+
+  now = 250_ms;
+  EXPECT_FALSE(filter.Calculate(true));  // Second edge after window expired
+
+  now = 300_ms;
+  EXPECT_FALSE(filter.Calculate(true));  // Still false
+}
+
+TEST_F(EdgeCounterFilterTest, EdgeCounterFilterParams) {
+  frc::EdgeCounterFilter filter{2, 0.2_s};
+
+  EXPECT_EQ(filter.GetRequiredEdges(), 2);
+  EXPECT_EQ(filter.GetWindowTime(), 0.2_s);
+
+  filter.SetRequiredEdges(3);
+
+  EXPECT_EQ(filter.GetRequiredEdges(), 3);
+
+  filter.SetWindowTime(0.5_s);
+
+  EXPECT_EQ(filter.GetWindowTime(), 0.5_s);
+}

wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp

@@ -42,6 +42,15 @@ TEST(Rotation2dTest, RotateByNonZero) {
   EXPECT_DOUBLE_EQ(120.0, rot.Degrees().value());
 }
 
+TEST(Rotation2dTest, RelativeTo) {
+  auto start = Rotation2d{30_deg};
+  auto end = Rotation2d{90_deg};
+
+  auto result = end.RelativeTo(start);
+
+  EXPECT_DOUBLE_EQ(60.0, result.Degrees().value());
+}
+
 TEST(Rotation2dTest, Minus) {
   const auto rot1 = Rotation2d{70_deg};
   const auto rot2 = Rotation2d{30_deg};

wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp

@@ -253,6 +253,21 @@ TEST(Rotation3dTest, RotateByNonZeroZ) {
   EXPECT_EQ(expected, rot);
 }
 
+TEST(Rotation3dTest, RelativeTo) {
+  const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
+  const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+  auto start = Rotation3d{yAxis, -90_deg};
+  auto end = Rotation3d{zAxis, 90_deg};
+
+  const Eigen::Vector3d intrinsicAxis{1.0, 1.0, 1.0};
+  auto expected = Rotation3d{intrinsicAxis, 120_deg};
+
+  auto result = end.RelativeTo(start);
+
+  EXPECT_EQ(expected, result);
+}
+
 TEST(Rotation3dTest, Minus) {
   const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
 
@@ -371,4 +386,32 @@ TEST(Rotation3dTest, Interpolate) {
   EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.X()}.value());
   EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Y()}.value());
   EXPECT_DOUBLE_EQ(-175.0, units::degree_t{interpolated.Z()}.value());
+
+  // t value of 0 should always produce the start
+  rot1 = Rotation3d{yAxis, -90_deg};
+  rot2 = Rotation3d{zAxis, 90_deg};
+  interpolated = wpi::Lerp(rot1, rot2, 0.0);
+  EXPECT_DOUBLE_EQ(rot1.X().value(), interpolated.X().value());
+  EXPECT_DOUBLE_EQ(rot1.Y().value(), interpolated.Y().value());
+  EXPECT_DOUBLE_EQ(rot1.Z().value(), interpolated.Z().value());
+
+  // The full rotation from rot1 to rot2 is 120 degrees around extrinsic
+  // <-1.0, 1.0, 1.0>
+  const Eigen::Vector3d extrinsicAxis{-1.0, 1.0, 1.0};
+  rot1 = Rotation3d{yAxis, -90_deg};
+  rot2 = Rotation3d{zAxis, 90_deg};
+  EXPECT_EQ(rot2, rot1.RotateBy(Rotation3d{extrinsicAxis, 120_deg}));
+  interpolated = wpi::Lerp(rot1, rot2, 0.5);
+  auto expected = rot1.RotateBy(Rotation3d{extrinsicAxis, 60_deg});
+  EXPECT_DOUBLE_EQ(expected.X().value(), interpolated.X().value());
+  EXPECT_DOUBLE_EQ(expected.Y().value(), interpolated.Y().value());
+  EXPECT_DOUBLE_EQ(expected.Z().value(), interpolated.Z().value());
+
+  // t value of 1 should always produce the end
+  rot1 = Rotation3d{yAxis, -90_deg};
+  rot2 = Rotation3d{zAxis, 90_deg};
+  interpolated = wpi::Lerp(rot1, rot2, 1.0);
+  EXPECT_NEAR(rot2.X().value(), interpolated.X().value(), 1e-9);
+  EXPECT_NEAR(rot2.Y().value(), interpolated.Y().value(), 1e-9);
+  EXPECT_NEAR(rot2.Z().value(), interpolated.Z().value(), 1e-9);
 }

wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometry3dTest.cpp

@@ -38,3 +38,18 @@ TEST(DifferentialDriveOdometry3dTest, EncoderDistances) {
   EXPECT_NEAR(pose.Z().value(), 0.0, kEpsilon);
   EXPECT_NEAR(pose.Rotation().ToRotation2d().Degrees().value(), 90.0, kEpsilon);
 }
+
+TEST(DifferentialDriveOdometry3dTest, GyroOffset) {
+  DifferentialDriveOdometry3d odometry{
+      frc::Rotation3d{0_deg, 5_deg, 0_deg}, 0_m, 0_m,
+      frc::Pose3d{frc::Translation3d{}, frc::Rotation3d{0_deg, 0_deg, 90_deg}}};
+  const auto& pose =
+      odometry.Update(frc::Rotation3d{0_deg, 10_deg, 0_deg}, 0_m, 0_m);
+
+  EXPECT_NEAR(pose.X().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(pose.Y().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(pose.Z().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(units::degree_t{pose.Rotation().X()}.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(units::degree_t{pose.Rotation().Y()}.value(), 5.0, kEpsilon);
+  EXPECT_NEAR(units::degree_t{pose.Rotation().Z()}.value(), 90.0, kEpsilon);
+}

wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometry3dTest.cpp

@@ -221,3 +221,19 @@ TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingXAxis) {
   EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.06);
   EXPECT_LT(maxError, 0.125);
 }
+
+TEST_F(MecanumDriveOdometry3dTest, GyroOffset) {
+  frc::MecanumDriveWheelPositions wheelPositions;
+  odometry.ResetPosition(
+      frc::Rotation3d{0_deg, 5_deg, 0_deg}, wheelPositions,
+      frc::Pose3d{frc::Translation3d{}, frc::Rotation3d{0_deg, 0_deg, 90_deg}});
+  auto pose =
+      odometry.Update(frc::Rotation3d{0_deg, 10_deg, 0_deg}, wheelPositions);
+
+  EXPECT_NEAR(pose.X().value(), 0.0, 1e-9);
+  EXPECT_NEAR(pose.Y().value(), 0.0, 1e-9);
+  EXPECT_NEAR(pose.Z().value(), 0.0, 1e-9);
+  EXPECT_NEAR(units::degree_t{pose.Rotation().X()}.value(), 0.0, 1e-9);
+  EXPECT_NEAR(units::degree_t{pose.Rotation().Y()}.value(), 5.0, 1e-9);
+  EXPECT_NEAR(units::degree_t{pose.Rotation().Z()}.value(), 90.0, 1e-9);
+}

wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometry3dTest.cpp

@@ -222,3 +222,18 @@ TEST_F(SwerveDriveOdometry3dTest, AccuracyFacingXAxis) {
   EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.06);
   EXPECT_LT(maxError, 0.125);
 }
+
+TEST_F(SwerveDriveOdometry3dTest, GyroOffset) {
+  m_odometry.ResetPosition(
+      frc::Rotation3d{0_deg, 5_deg, 0_deg}, {zero, zero, zero, zero},
+      frc::Pose3d{frc::Translation3d{}, frc::Rotation3d{0_deg, 0_deg, 90_deg}});
+  auto pose = m_odometry.Update(frc::Rotation3d{0_deg, 10_deg, 0_deg},
+                                {zero, zero, zero, zero});
+
+  EXPECT_NEAR(pose.X().value(), 0.0, 1e-9);
+  EXPECT_NEAR(pose.Y().value(), 0.0, 1e-9);
+  EXPECT_NEAR(pose.Z().value(), 0.0, 1e-9);
+  EXPECT_NEAR(units::degree_t{pose.Rotation().X()}.value(), 0.0, 1e-9);
+  EXPECT_NEAR(units::degree_t{pose.Rotation().Y()}.value(), 5.0, 1e-9);
+  EXPECT_NEAR(units::degree_t{pose.Rotation().Z()}.value(), 90.0, 1e-9);
+}

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

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

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

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

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

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