diff --git a/.github/workflows/comment-command.yml b/.github/workflows/comment-command.yml
index 6cdcf123bdb..8277499c38f 100644
--- a/.github/workflows/comment-command.yml
+++ b/.github/workflows/comment-command.yml
@@ -43,7 +43,7 @@ jobs:
           distribution: 'zulu'
           java-version: 17
       - name: Install wpiformat
-        run: pip3 install wpiformat==2024.33
+        run: pip3 install wpiformat==2024.34
       - name: Run wpiformat
         run: wpiformat
       - name: Run spotlessApply
diff --git a/.github/workflows/lint-format.yml b/.github/workflows/lint-format.yml
index d32d9f70d00..b749a504920 100644
--- a/.github/workflows/lint-format.yml
+++ b/.github/workflows/lint-format.yml
@@ -27,7 +27,7 @@ jobs:
         with:
           python-version: '3.10'
       - name: Install wpiformat
-        run: pip3 install wpiformat==2024.33
+        run: pip3 install wpiformat==2024.34
       - name: Run
         run: wpiformat
       - name: Check output
@@ -66,7 +66,7 @@ jobs:
         with:
           python-version: '3.10'
       - name: Install wpiformat
-        run: pip3 install wpiformat
+        run: pip3 install wpiformat==2024.34
       - name: Create compile_commands.json
         run: |
           ./gradlew generateCompileCommands -Ptoolchain-optional-roboRio
diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java
index c569a9c55ff..016c982fe7e 100644
--- a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java
+++ b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java
@@ -35,7 +35,9 @@ public enum AprilTagFields {
    *
    * @return AprilTagFieldLayout of the field
    * @throws UncheckedIOException If the layout does not exist
+   * @deprecated Use {@link AprilTagFieldLayout#loadField(AprilTagFields)} instead.
    */
+  @Deprecated(forRemoval = true, since = "2025")
   public AprilTagFieldLayout loadAprilTagLayoutField() {
     return AprilTagFieldLayout.loadField(this);
   }
diff --git a/apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h
index 6ba3824ed66..c2e73c78c4d 100644
--- a/apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h
+++ b/apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h
@@ -166,7 +166,9 @@ void from_json(const wpi::json& json, AprilTagFieldLayout& layout);
  *
  * @param field The predefined field
  * @return AprilTagFieldLayout of the field
+ * @deprecated Use AprilTagFieldLayout::LoadField() instead
  */
+[[deprecated("Use AprilTagFieldLayout::LoadField() instead")]]
 WPILIB_DLLEXPORT AprilTagFieldLayout
 LoadAprilTagLayoutField(AprilTagField field);
 
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
index 0186b6550d6..3a5ee405f63 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
@@ -349,11 +349,16 @@ public RepeatCommand repeatedly() {
   }
 
   /**
-   * Decorates this command to run "by proxy" by wrapping it in a {@link ProxyCommand}. This is
-   * useful for "forking off" from command compositions when the user does not wish to extend the
-   * command's requirements to the entire command composition.
+   * Decorates this command to run "by proxy" by wrapping it in a {@link ProxyCommand}. Use this for
+   * "forking off" from command compositions when the user does not wish to extend the command's
+   * requirements to the entire command composition. ProxyCommand has unique implications and
+   * semantics, see the WPILib docs for a full explanation.
    *
    * @return the decorated command
+   * @see ProxyCommand
+   * @see <a
+   *     href="https://docs.wpilib.org/en/stable/docs/software/commandbased/command-compositions.html#scheduling-other-commands">WPILib
+   *     docs</a>
    */
   public ProxyCommand asProxy() {
     return new ProxyCommand(this);
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/DeferredCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/DeferredCommand.java
index 099eba3b04e..f76cf909238 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/DeferredCommand.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/DeferredCommand.java
@@ -11,9 +11,9 @@
 import java.util.function.Supplier;
 
 /**
- * Defers Command construction to runtime. Runs the command returned by the supplier when this
- * command is initialized, and ends when it ends. Useful for performing runtime tasks before
- * creating a new command. If this command is interrupted, it will cancel the command.
+ * Defers Command construction to runtime. Runs the command returned by a supplier when this command
+ * is initialized, and ends when it ends. Useful for performing runtime tasks before creating a new
+ * command. If this command is interrupted, it will cancel the command.
  *
  * <p>Note that the supplier <i>must</i> create a new Command each call. For selecting one of a
  * preallocated set of commands, use {@link SelectCommand}.
@@ -28,9 +28,10 @@ public class DeferredCommand extends Command {
   private Command m_command = m_nullCommand;
 
   /**
-   * Creates a new DeferredCommand that runs the supplied command when initialized, and ends when it
-   * ends. Useful for lazily creating commands at runtime. The {@link Supplier} will be called each
-   * time this command is initialized. The Supplier <i>must</i> create a new Command each call.
+   * Creates a new DeferredCommand that directly runs the supplied command when initialized, and
+   * ends when it ends. Useful for lazily creating commands when the DeferredCommand is initialized,
+   * such as if the supplied command depends on runtime state. The {@link Supplier} will be called
+   * each time this command is initialized. The Supplier <i>must</i> create a new Command each call.
    *
    * @param supplier The command supplier
    * @param requirements The command requirements. This is a {@link Set} to prevent accidental
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProxyCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProxyCommand.java
index f05b286f4d4..f71c10b2339 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProxyCommand.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProxyCommand.java
@@ -10,8 +10,13 @@
 import java.util.function.Supplier;
 
 /**
- * Schedules the given command when this command is initialized, and ends when it ends. Useful for
- * forking off from CommandGroups. If this command is interrupted, it will cancel the command.
+ * Schedules a given command when this command is initialized and ends when it ends, but does not
+ * directly run it. Use this for including a command in a composition without adding its
+ * requirements, <strong>but only if you know what you are doing. If you are unsure, see <a
+ * href="https://docs.wpilib.org/en/stable/docs/software/commandbased/command-compositions.html#scheduling-other-commands">the
+ * WPILib docs</a> for a complete explanation of proxy semantics.</strong> Do not proxy a command
+ * from a subsystem already required by the composition, or else the composition will cancel itself
+ * when the proxy is reached. If this command is interrupted, it will cancel the command.
  *
  * <p>This class is provided by the NewCommands VendorDep
  */
@@ -21,9 +26,12 @@ public class ProxyCommand extends Command {
 
   /**
    * Creates a new ProxyCommand that schedules the supplied command when initialized, and ends when
-   * it is no longer scheduled. Useful for lazily creating commands at runtime.
+   * it is no longer scheduled. Use this for lazily creating <strong>proxied</strong> commands at
+   * runtime. Proxying should only be done to escape from composition requirement semantics, so if
+   * only initialization time command construction is needed, use {@link DeferredCommand} instead.
    *
    * @param supplier the command supplier
+   * @see DeferredCommand
    */
   public ProxyCommand(Supplier<Command> supplier) {
     m_supplier = requireNonNullParam(supplier, "supplier", "ProxyCommand");
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java
index 3bfdbaebf52..8e49706958c 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java
@@ -22,7 +22,6 @@ public class TrapezoidProfileCommand extends Command {
   private final Consumer<State> m_output;
   private final Supplier<State> m_goal;
   private final Supplier<State> m_currentState;
-  private final boolean m_newAPI; // TODO: Remove
   private final Timer m_timer = new Timer();
 
   /**
@@ -46,29 +45,6 @@ public TrapezoidProfileCommand(
     m_output = requireNonNullParam(output, "output", "TrapezoidProfileCommand");
     m_goal = goal;
     m_currentState = currentState;
-    m_newAPI = true;
-    addRequirements(requirements);
-  }
-
-  /**
-   * Creates a new TrapezoidProfileCommand that will execute the given {@link TrapezoidProfile}.
-   * Output will be piped to the provided consumer function.
-   *
-   * @param profile The motion profile to execute.
-   * @param output The consumer for the profile output.
-   * @param requirements The subsystems required by this command.
-   * @deprecated The new constructor allows you to pass in a supplier for desired and current state.
-   *     This allows you to change goals at runtime.
-   */
-  @Deprecated(since = "2024", forRemoval = true)
-  @SuppressWarnings("this-escape")
-  public TrapezoidProfileCommand(
-      TrapezoidProfile profile, Consumer<State> output, Subsystem... requirements) {
-    m_profile = requireNonNullParam(profile, "profile", "TrapezoidProfileCommand");
-    m_output = requireNonNullParam(output, "output", "TrapezoidProfileCommand");
-    m_newAPI = false;
-    m_goal = null;
-    m_currentState = null;
     addRequirements(requirements);
   }
 
@@ -80,11 +56,7 @@ public void initialize() {
   @Override
   @SuppressWarnings("removal")
   public void execute() {
-    if (m_newAPI) {
-      m_output.accept(m_profile.calculate(m_timer.get(), m_currentState.get(), m_goal.get()));
-    } else {
-      m_output.accept(m_profile.calculate(m_timer.get()));
-    }
+    m_output.accept(m_profile.calculate(m_timer.get(), m_currentState.get(), m_goal.get()));
   }
 
   @Override
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/ProxyCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ProxyCommand.cpp
index eb4fb7cab1f..3851afa53c0 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/ProxyCommand.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/ProxyCommand.cpp
@@ -4,6 +4,7 @@
 
 #include "frc2/command/ProxyCommand.h"
 
+#include <fmt/core.h>
 #include <wpi/sendable/SendableBuilder.h>
 
 using namespace frc2;
@@ -20,11 +21,11 @@ ProxyCommand::ProxyCommand(wpi::unique_function<CommandPtr()> supplier)
 
 ProxyCommand::ProxyCommand(Command* command)
     : ProxyCommand([command] { return command; }) {
-  SetName(std::string{"Proxy("}.append(command->GetName()).append(")"));
+  SetName(fmt::format("Proxy({})", command->GetName()));
 }
 
 ProxyCommand::ProxyCommand(std::unique_ptr<Command> command) {
-  SetName(std::string{"Proxy("}.append(command->GetName()).append(")"));
+  SetName(fmt::format("Proxy({})", command->GetName()));
   m_supplier = [command = std::move(command)] { return command.get(); };
 }
 
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
index ad6aa79040c..27ac7f35828 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
@@ -249,14 +249,17 @@ class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
   CommandPtr Repeatedly() &&;
 
   /**
-   * Decorates this command to run "by proxy" by wrapping it in a
-   * ProxyCommand. This is useful for "forking off" from command groups
-   * when the user does not wish to extend the command's requirements to the
-   * entire command group.
+   * Decorates this command to run "by proxy" by wrapping it in a ProxyCommand.
+   * Use this for "forking off" from command compositions when the user does not
+   * wish to extend the command's requirements to the entire command
+   * composition. ProxyCommand has unique implications and semantics, see <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/commandbased/command-compositions.html#scheduling-other-commands">the
+   * WPILib docs</a> for a full explanation.
    *
    * <p>This overload transfers command ownership to the returned CommandPtr.
    *
    * @return the decorated command
+   * @see ProxyCommand
    */
   [[nodiscard]]
   CommandPtr AsProxy() &&;
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h
index 2610684b09e..f4548e4438f 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h
@@ -7,7 +7,6 @@
 #include <concepts>
 #include <functional>
 #include <memory>
-#include <optional>
 #include <string>
 #include <utility>
 #include <vector>
@@ -51,12 +50,15 @@ class CommandPtr final {
   CommandPtr Repeatedly() &&;
 
   /**
-   * Decorates this command to run "by proxy" by wrapping it in a
-   * ProxyCommand. This is useful for "forking off" from command groups
-   * when the user does not wish to extend the command's requirements to the
-   * entire command group.
+   * Decorates this command to run "by proxy" by wrapping it in a ProxyCommand.
+   * Use this for "forking off" from command compositions when the user does not
+   * wish to extend the command's requirements to the entire command
+   * composition. ProxyCommand has unique implications and semantics, see <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/commandbased/command-compositions.html#scheduling-other-commands">the
+   * WPILib docs</a> for a full explanation.
    *
    * @return the decorated command
+   * @see ProxyCommand
    */
   [[nodiscard]]
   CommandPtr AsProxy() &&;
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/DeferredCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/DeferredCommand.h
index 442076d8f27..28460fdf9b5 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/DeferredCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/DeferredCommand.h
@@ -16,7 +16,7 @@
 
 namespace frc2 {
 /**
- * Defers Command construction to runtime. Runs the command returned by the
+ * Defers Command construction to runtime. Runs the command returned by a
  * supplier when this command is initialized, and ends when it ends. Useful for
  * performing runtime tasks before creating a new command. If this command is
  * interrupted, it will cancel the command.
@@ -29,9 +29,10 @@ namespace frc2 {
 class DeferredCommand : public CommandHelper<Command, DeferredCommand> {
  public:
   /**
-   * Creates a new DeferredCommand that runs the supplied command when
-   * initialized, and ends when it ends. Useful for lazily
-   * creating commands at runtime. The supplier will be called each time this
+   * Creates a new DeferredCommand that directly runs the supplied command when
+   * initialized, and ends when it ends. Useful for lazily creating commands
+   * when the DeferredCommand is initialized, such as if the supplied command
+   * depends on runtime state. The supplier will be called each time this
    * command is initialized. The supplier <i>must</i> create a new Command each
    * call.
    *
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ParallelDeadlineGroup.h b/wpilibNewCommands/src/main/native/include/frc2/command/ParallelDeadlineGroup.h
index 9e253fbffdf..22e9759f2b4 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/ParallelDeadlineGroup.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ParallelDeadlineGroup.h
@@ -9,7 +9,6 @@
 #pragma warning(disable : 4521)
 #endif
 
-#include <concepts>
 #include <memory>
 #include <type_traits>
 #include <utility>
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ParallelRaceGroup.h b/wpilibNewCommands/src/main/native/include/frc2/command/ParallelRaceGroup.h
index caefa48fa35..17db907dd43 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/ParallelRaceGroup.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ParallelRaceGroup.h
@@ -9,7 +9,6 @@
 #pragma warning(disable : 4521)
 #endif
 
-#include <concepts>
 #include <memory>
 #include <type_traits>
 #include <utility>
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ProxyCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/ProxyCommand.h
index 3b7eecc6741..d4d4757cf4f 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/ProxyCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ProxyCommand.h
@@ -13,9 +13,15 @@
 
 namespace frc2 {
 /**
- * Schedules the given command when this command is initialized, and ends when
- * it ends. Useful for forking off from CommandGroups. If this command is
- * interrupted, it will cancel the command.
+ * Schedules a given command when this command is initialized and ends when it
+ * ends, but does not directly run it. Use this for including a command in a
+ * composition without adding its requirements, <strong>but only if you know
+ * what you are doing. If you are unsure, see <a
+ * href="https://docs.wpilib.org/en/stable/docs/software/commandbased/command-compositions.html#scheduling-other-commands">the
+ * WPILib docs</a> for a complete explanation of proxy semantics.</strong> Do
+ * not proxy a command from a subsystem already required by the composition, or
+ * else the composition will cancel itself when the proxy is reached. If this
+ * command is interrupted, it will cancel the command.
  *
  * <p>This class is provided by the NewCommands VendorDep
  */
@@ -23,19 +29,27 @@ class ProxyCommand : public CommandHelper<Command, ProxyCommand> {
  public:
   /**
    * Creates a new ProxyCommand that schedules the supplied command when
-   * initialized, and ends when it is no longer scheduled. Useful for lazily
-   * creating commands at runtime.
+   * initialized, and ends when it is no longer scheduled. Use this for lazily
+   * creating <strong>proxied</strong> commands at runtime. Proxying should only
+   * be done to escape from composition requirement semantics, so if only
+   * initialization time command construction is needed, use {@link
+   * DeferredCommand} instead.
    *
    * @param supplier the command supplier
+   * @see DeferredCommand
    */
   explicit ProxyCommand(wpi::unique_function<Command*()> supplier);
 
   /**
    * Creates a new ProxyCommand that schedules the supplied command when
-   * initialized, and ends when it is no longer scheduled. Useful for lazily
-   * creating commands at runtime.
+   * initialized, and ends when it is no longer scheduled. Use this for lazily
+   * creating <strong>proxied</strong> commands at runtime. Proxying should only
+   * be done to escape from composition requirement semantics, so if only
+   * initialization time command construction is needed, use {@link
+   * DeferredCommand} instead.
    *
    * @param supplier the command supplier
+   * @see DeferredCommand
    */
   explicit ProxyCommand(wpi::unique_function<CommandPtr()> supplier);
 
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h b/wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h
index fdcd1bc22fc..321118880b4 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h
@@ -9,7 +9,6 @@
 #pragma warning(disable : 4521)
 #endif
 
-#include <concepts>
 #include <limits>
 #include <memory>
 #include <type_traits>
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
index 837dda3e098..30ad9b70872 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
@@ -52,28 +52,6 @@ class TrapezoidProfileCommand
         m_goal(goal),
         m_currentState(currentState) {
     this->AddRequirements(requirements);
-    m_newAPI = true;
-  }
-
-  /**
-   * Creates a new TrapezoidProfileCommand that will execute the given
-   * TrapezoidalProfile. Output will be piped to the provided consumer function.
-   *
-   * @param profile      The motion profile to execute.
-   * @param output       The consumer for the profile output.
-   * @param requirements The list of requirements.
-   * @deprecated The new constructor allows you to pass in a supplier for
-   * desired and current state. This allows you to change goals at runtime.
-   */
-  [[deprecated(
-      "The new constructor allows you to pass in a supplier for desired and "
-      "current state. This allows you to change goals at runtime.")]]
-  TrapezoidProfileCommand(frc::TrapezoidProfile<Distance> profile,
-                          std::function<void(State)> output,
-                          Requirements requirements = {})
-      : m_profile(profile), m_output(output) {
-    this->AddRequirements(requirements);
-    m_newAPI = false;
   }
 
   void Initialize() override { m_timer.Restart(); }
@@ -93,7 +71,6 @@ class TrapezoidProfileCommand
   std::function<void(State)> m_output;
   std::function<State()> m_goal;
   std::function<State()> m_currentState;
-  bool m_newAPI;  // TODO: Remove
   frc::Timer m_timer;
 };
 
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp
index 884d5043469..7c342476fb1 100644
--- a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp
@@ -5,12 +5,9 @@
 #include <frc/simulation/SimHooks.h>
 
 #include "CommandTestBase.h"
-#include "frc2/command/ConditionalCommand.h"
 #include "frc2/command/FunctionalCommand.h"
 #include "frc2/command/InstantCommand.h"
-#include "frc2/command/ParallelRaceGroup.h"
 #include "frc2/command/RunCommand.h"
-#include "frc2/command/SequentialCommandGroup.h"
 
 using namespace frc2;
 class CommandDecoratorTest : public CommandTestBase {};
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h
index 586432dff05..16d0cdc8b2c 100644
--- a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h
@@ -5,7 +5,6 @@
 #pragma once
 
 #include <functional>
-#include <memory>
 #include <utility>
 
 #include <frc/simulation/DriverStationSim.h>
@@ -16,14 +15,13 @@
 #include "frc2/command/Requirements.h"
 #include "frc2/command/SubsystemBase.h"
 #include "gmock/gmock.h"
-#include "make_vector.h"
 
 namespace frc2 {
 
 class TestSubsystem : public SubsystemBase {
  public:
   explicit TestSubsystem(std::function<void()> periodic = [] {})
-      : m_periodic{periodic} {}
+      : m_periodic{std::move(periodic)} {}
   void Periodic() override { m_periodic(); }
 
  private:
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
index 14eee8f190a..141844e362e 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
@@ -21,6 +21,7 @@
 import java.util.Map;
 import java.util.NoSuchElementException;
 import java.util.Objects;
+import java.util.Optional;
 
 /**
  * This class wraps {@link Odometry} to fuse latency-compensated vision measurements with encoder
@@ -125,6 +126,16 @@ public Pose2d getEstimatedPosition() {
     return m_odometry.getPoseMeters();
   }
 
+  /**
+   * Return the pose at a given timestamp, if the buffer is not empty.
+   *
+   * @param timestampSeconds The pose's timestamp in seconds.
+   * @return The pose at the given timestamp (or Optional.empty() if the buffer is empty).
+   */
+  public Optional<Pose2d> sampleAt(double timestampSeconds) {
+    return m_poseBuffer.getSample(timestampSeconds).map(record -> record.poseMeters);
+  }
+
   /**
    * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
    * while still accounting for measurement noise.
diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
index 6e38d8c65fb..342ed2529c9 100644
--- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
@@ -4,6 +4,8 @@
 
 #pragma once
 
+#include <optional>
+
 #include <Eigen/Core>
 #include <wpi/SymbolExports.h>
 #include <wpi/array.h>
@@ -89,6 +91,15 @@ class WPILIB_DLLEXPORT PoseEstimator {
    */
   Pose2d GetEstimatedPosition() const;
 
+  /**
+   * Return the pose at a given timestamp, if the buffer is not empty.
+   *
+   * @param timestamp The pose's timestamp.
+   * @return The pose at the given timestamp (or std::nullopt if the buffer is
+   * empty).
+   */
+  std::optional<Pose2d> SampleAt(units::second_t timestamp) const;
+
   /**
    * Adds a vision measurement to the Kalman Filter. This will correct
    * the odometry pose estimate while still accounting for measurement noise.
diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc
index 79d71bcc566..43942cfbae2 100644
--- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc
+++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc
@@ -57,6 +57,19 @@ Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::GetEstimatedPosition()
   return m_odometry.GetPose();
 }
 
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+std::optional<Pose2d> PoseEstimator<WheelSpeeds, WheelPositions>::SampleAt(
+    units::second_t timestamp) const {
+  // TODO Replace with std::optional::transform() in C++23
+  std::optional<PoseEstimator<WheelSpeeds, WheelPositions>::InterpolationRecord>
+      sample = m_poseBuffer.Sample(timestamp);
+  if (sample) {
+    return sample->pose;
+  } else {
+    return std::nullopt;
+  }
+}
+
 template <typename WheelSpeeds, WheelPositions WheelPositions>
 void PoseEstimator<WheelSpeeds, WheelPositions>::AddVisionMeasurement(
     const Pose2d& visionRobotPose, units::second_t timestamp) {
diff --git a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
index 94d28180d8f..bfb1b548976 100644
--- a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
+++ b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
@@ -100,7 +100,7 @@ class TimeInterpolatableBuffer {
    *
    * @param time The time at which to sample the buffer.
    */
-  std::optional<T> Sample(units::second_t time) {
+  std::optional<T> Sample(units::second_t time) const {
     if (m_pastSnapshots.empty()) {
       return {};
     }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
index 08c5a156cf1..b019c608116 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
@@ -18,6 +18,7 @@
 import edu.wpi.first.math.trajectory.TrajectoryConfig;
 import edu.wpi.first.math.trajectory.TrajectoryGenerator;
 import java.util.List;
+import java.util.Optional;
 import java.util.Random;
 import java.util.TreeMap;
 import java.util.function.Function;
@@ -304,4 +305,54 @@ void testDiscardsStaleVisionMeasurements() {
         estimator.getEstimatedPosition().getRotation().getRadians(),
         "Incorrect Final Theta");
   }
+
+  @Test
+  void testSampleAt() {
+    var kinematics = new DifferentialDriveKinematics(1);
+    var estimator =
+        new DifferentialDrivePoseEstimator(
+            kinematics,
+            new Rotation2d(),
+            0,
+            0,
+            new Pose2d(),
+            VecBuilder.fill(1, 1, 1),
+            VecBuilder.fill(1, 1, 1));
+
+    // Returns empty when null
+    assertEquals(Optional.empty(), estimator.sampleAt(1));
+
+    // Add odometry measurements, but don't fill up the buffer
+    // Add a tiny tolerance for the upper bound because of floating point rounding error
+    for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
+      estimator.updateWithTime(time, new Rotation2d(), time, time);
+    }
+
+    // Sample at an added time
+    assertEquals(Optional.of(new Pose2d(1.02, 0, new Rotation2d())), estimator.sampleAt(1.02));
+    // Sample between updates (test interpolation)
+    assertEquals(Optional.of(new Pose2d(1.01, 0, new Rotation2d())), estimator.sampleAt(1.01));
+    // Sampling before the oldest value returns the oldest value
+    assertEquals(Optional.of(new Pose2d(1, 0, new Rotation2d())), estimator.sampleAt(0.5));
+    // Sampling after the newest value returns the newest value
+    assertEquals(Optional.of(new Pose2d(2, 0, new Rotation2d())), estimator.sampleAt(2.5));
+
+    // Add a vision measurement after the odometry measurements (while keeping all of the old
+    // odometry measurements)
+    estimator.addVisionMeasurement(new Pose2d(2, 0, new Rotation2d(1)), 2.2);
+
+    // Make sure nothing changed (except the newest value)
+    assertEquals(Optional.of(new Pose2d(1.02, 0, new Rotation2d())), estimator.sampleAt(1.02));
+    assertEquals(Optional.of(new Pose2d(1.01, 0, new Rotation2d())), estimator.sampleAt(1.01));
+    assertEquals(Optional.of(new Pose2d(1, 0, new Rotation2d())), estimator.sampleAt(0.5));
+
+    // Add a vision measurement before the odometry measurements that's still in the buffer
+    estimator.addVisionMeasurement(new Pose2d(1, 0.2, new Rotation2d()), 0.9);
+
+    // Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
+    assertEquals(Optional.of(new Pose2d(1.02, 0.1, new Rotation2d())), estimator.sampleAt(1.02));
+    assertEquals(Optional.of(new Pose2d(1.01, 0.1, new Rotation2d())), estimator.sampleAt(1.01));
+    assertEquals(Optional.of(new Pose2d(1, 0.1, new Rotation2d())), estimator.sampleAt(0.5));
+    assertEquals(Optional.of(new Pose2d(2, 0.1, new Rotation2d())), estimator.sampleAt(2.5));
+  }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
index ea08a5f1d90..c48012f7f95 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
@@ -19,6 +19,7 @@
 import edu.wpi.first.math.trajectory.TrajectoryConfig;
 import edu.wpi.first.math.trajectory.TrajectoryGenerator;
 import java.util.List;
+import java.util.Optional;
 import java.util.Random;
 import java.util.TreeMap;
 import java.util.function.Function;
@@ -321,4 +322,59 @@ void testDiscardsOldVisionMeasurements() {
         estimator.getEstimatedPosition().getRotation().getRadians(),
         "Incorrect Final Theta");
   }
+
+  @Test
+  void testSampleAt() {
+    var kinematics =
+        new MecanumDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(-1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1));
+    var estimator =
+        new MecanumDrivePoseEstimator(
+            kinematics,
+            new Rotation2d(),
+            new MecanumDriveWheelPositions(),
+            new Pose2d(),
+            VecBuilder.fill(1, 1, 1),
+            VecBuilder.fill(1, 1, 1));
+
+    // Returns empty when null
+    assertEquals(Optional.empty(), estimator.sampleAt(1));
+
+    // Add odometry measurements, but don't fill up the buffer
+    // Add a tiny tolerance for the upper bound because of floating point rounding error
+    for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
+      var wheelPositions = new MecanumDriveWheelPositions(time, time, time, time);
+      estimator.updateWithTime(time, new Rotation2d(), wheelPositions);
+    }
+
+    // Sample at an added time
+    assertEquals(Optional.of(new Pose2d(1.02, 0, new Rotation2d())), estimator.sampleAt(1.02));
+    // Sample between updates (test interpolation)
+    assertEquals(Optional.of(new Pose2d(1.01, 0, new Rotation2d())), estimator.sampleAt(1.01));
+    // Sampling before the oldest value returns the oldest value
+    assertEquals(Optional.of(new Pose2d(1, 0, new Rotation2d())), estimator.sampleAt(0.5));
+    // Sampling after the newest value returns the newest value
+    assertEquals(Optional.of(new Pose2d(2, 0, new Rotation2d())), estimator.sampleAt(2.5));
+
+    // Add a vision measurement after the odometry measurements (while keeping all of the old
+    // odometry measurements)
+    estimator.addVisionMeasurement(new Pose2d(2, 0, new Rotation2d(1)), 2.2);
+
+    // Make sure nothing changed (except the newest value)
+    assertEquals(Optional.of(new Pose2d(1.02, 0, new Rotation2d())), estimator.sampleAt(1.02));
+    assertEquals(Optional.of(new Pose2d(1.01, 0, new Rotation2d())), estimator.sampleAt(1.01));
+    assertEquals(Optional.of(new Pose2d(1, 0, new Rotation2d())), estimator.sampleAt(0.5));
+
+    // Add a vision measurement before the odometry measurements that's still in the buffer
+    estimator.addVisionMeasurement(new Pose2d(1, 0.2, new Rotation2d()), 0.9);
+
+    // Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
+    assertEquals(Optional.of(new Pose2d(1.02, 0.1, new Rotation2d())), estimator.sampleAt(1.02));
+    assertEquals(Optional.of(new Pose2d(1.01, 0.1, new Rotation2d())), estimator.sampleAt(1.01));
+    assertEquals(Optional.of(new Pose2d(1, 0.1, new Rotation2d())), estimator.sampleAt(0.5));
+    assertEquals(Optional.of(new Pose2d(2, 0.1, new Rotation2d())), estimator.sampleAt(2.5));
+  }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
index f827a102527..7101fa554a8 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
@@ -19,6 +19,7 @@
 import edu.wpi.first.math.trajectory.TrajectoryConfig;
 import edu.wpi.first.math.trajectory.TrajectoryGenerator;
 import java.util.List;
+import java.util.Optional;
 import java.util.Random;
 import java.util.TreeMap;
 import java.util.function.Function;
@@ -355,4 +356,70 @@ void testDiscardsOldVisionMeasurements() {
         estimator.getEstimatedPosition().getRotation().getRadians(),
         "Incorrect Final Theta");
   }
+
+  @Test
+  void testSampleAt() {
+    var kinematics =
+        new SwerveDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(-1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1));
+    var estimator =
+        new SwerveDrivePoseEstimator(
+            kinematics,
+            new Rotation2d(),
+            new SwerveModulePosition[] {
+              new SwerveModulePosition(),
+              new SwerveModulePosition(),
+              new SwerveModulePosition(),
+              new SwerveModulePosition()
+            },
+            new Pose2d(),
+            VecBuilder.fill(1, 1, 1),
+            VecBuilder.fill(1, 1, 1));
+
+    // Returns empty when null
+    assertEquals(Optional.empty(), estimator.sampleAt(1));
+
+    // Add odometry measurements, but don't fill up the buffer
+    // Add a tiny tolerance for the upper bound because of floating point rounding error
+    for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
+      var wheelPositions =
+          new SwerveModulePosition[] {
+            new SwerveModulePosition(time, new Rotation2d()),
+            new SwerveModulePosition(time, new Rotation2d()),
+            new SwerveModulePosition(time, new Rotation2d()),
+            new SwerveModulePosition(time, new Rotation2d())
+          };
+      estimator.updateWithTime(time, new Rotation2d(), wheelPositions);
+    }
+
+    // Sample at an added time
+    assertEquals(Optional.of(new Pose2d(1.02, 0, new Rotation2d())), estimator.sampleAt(1.02));
+    // Sample between updates (test interpolation)
+    assertEquals(Optional.of(new Pose2d(1.01, 0, new Rotation2d())), estimator.sampleAt(1.01));
+    // Sampling before the oldest value returns the oldest value
+    assertEquals(Optional.of(new Pose2d(1, 0, new Rotation2d())), estimator.sampleAt(0.5));
+    // Sampling after the newest value returns the newest value
+    assertEquals(Optional.of(new Pose2d(2, 0, new Rotation2d())), estimator.sampleAt(2.5));
+
+    // Add a vision measurement after the odometry measurements (while keeping all of the old
+    // odometry measurements)
+    estimator.addVisionMeasurement(new Pose2d(2, 0, new Rotation2d(1)), 2.2);
+
+    // Make sure nothing changed (except the newest value)
+    assertEquals(Optional.of(new Pose2d(1.02, 0, new Rotation2d())), estimator.sampleAt(1.02));
+    assertEquals(Optional.of(new Pose2d(1.01, 0, new Rotation2d())), estimator.sampleAt(1.01));
+    assertEquals(Optional.of(new Pose2d(1, 0, new Rotation2d())), estimator.sampleAt(0.5));
+
+    // Add a vision measurement before the odometry measurements that's still in the buffer
+    estimator.addVisionMeasurement(new Pose2d(1, 0.2, new Rotation2d()), 0.9);
+
+    // Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
+    assertEquals(Optional.of(new Pose2d(1.02, 0.1, new Rotation2d())), estimator.sampleAt(1.02));
+    assertEquals(Optional.of(new Pose2d(1.01, 0.1, new Rotation2d())), estimator.sampleAt(1.01));
+    assertEquals(Optional.of(new Pose2d(1, 0.1, new Rotation2d())), estimator.sampleAt(0.5));
+    assertEquals(Optional.of(new Pose2d(2, 0.1, new Rotation2d())), estimator.sampleAt(2.5));
+  }
 }
diff --git a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
index 3103068344d..8144700e86b 100644
--- a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
@@ -293,3 +293,62 @@ TEST(DifferentialDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
               estimator.GetEstimatedPosition().Rotation().Radians().value(),
               1e-6);
 }
+
+TEST(DifferentialDrivePoseEstimatorTest, TestSampleAt) {
+  frc::DifferentialDriveKinematics kinematics{1_m};
+  frc::DifferentialDrivePoseEstimator estimator{
+      kinematics,      frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
+      {1.0, 1.0, 1.0}, {1.0, 1.0, 1.0}};
+
+  // Returns empty when null
+  EXPECT_EQ(std::nullopt, estimator.SampleAt(1_s));
+
+  // Add odometry measurements, but don't fill up the buffer
+  // Add a tiny tolerance for the upper bound because of floating point rounding
+  // error
+  for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
+    estimator.UpdateWithTime(units::second_t{time}, frc::Rotation2d{},
+                             units::meter_t{time}, units::meter_t{time});
+  }
+
+  // Sample at an added time
+  EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}),
+            estimator.SampleAt(1.02_s));
+  // Sample between updates (test interpolation)
+  EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}),
+            estimator.SampleAt(1.01_s));
+  // Sampling before the oldest value returns the oldest value
+  EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}),
+            estimator.SampleAt(0.5_s));
+  // Sampling after the newest value returns the newest value
+  EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0_m, frc::Rotation2d{}}),
+            estimator.SampleAt(2.5_s));
+
+  // Add a vision measurement after the odometry measurements (while keeping all
+  // of the old odometry measurements)
+  estimator.AddVisionMeasurement(frc::Pose2d{2_m, 0_m, frc::Rotation2d{1_rad}},
+                                 2.2_s);
+
+  // Make sure nothing changed (except the newest value)
+  EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}),
+            estimator.SampleAt(1.02_s));
+  EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}),
+            estimator.SampleAt(1.01_s));
+  EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}),
+            estimator.SampleAt(0.5_s));
+
+  // Add a vision measurement before the odometry measurements that's still in
+  // the buffer
+  estimator.AddVisionMeasurement(frc::Pose2d{1_m, 0.2_m, frc::Rotation2d{}},
+                                 0.9_s);
+
+  // Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
+  EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0.1_m, frc::Rotation2d{}}),
+            estimator.SampleAt(1.02_s));
+  EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0.1_m, frc::Rotation2d{}}),
+            estimator.SampleAt(1.01_s));
+  EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0.1_m, frc::Rotation2d{}}),
+            estimator.SampleAt(0.5_s));
+  EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0.1_m, frc::Rotation2d{}}),
+            estimator.SampleAt(2.5_s));
+}
diff --git a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
index 8b3a586892d..13d5c3180e5 100644
--- a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
@@ -295,3 +295,67 @@ TEST(MecanumDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
               estimator.GetEstimatedPosition().Rotation().Radians().value(),
               1e-6);
 }
+
+TEST(MecanumDrivePoseEstimatorTest, TestSampleAt) {
+  frc::MecanumDriveKinematics kinematics{
+      frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
+      frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+  frc::MecanumDrivePoseEstimator estimator{
+      kinematics,    frc::Rotation2d{}, frc::MecanumDriveWheelPositions{},
+      frc::Pose2d{}, {1.0, 1.0, 1.0},   {1.0, 1.0, 1.0}};
+
+  // Returns empty when null
+  EXPECT_EQ(std::nullopt, estimator.SampleAt(1_s));
+
+  // Add odometry measurements, but don't fill up the buffer
+  // Add a tiny tolerance for the upper bound because of floating point rounding
+  // error
+  for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
+    frc::MecanumDriveWheelPositions wheelPositions{
+        units::meter_t{time}, units::meter_t{time}, units::meter_t{time},
+        units::meter_t{time}};
+    estimator.UpdateWithTime(units::second_t{time}, frc::Rotation2d{},
+                             wheelPositions);
+  }
+
+  // Sample at an added time
+  EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}),
+            estimator.SampleAt(1.02_s));
+  // Sample between updates (test interpolation)
+  EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}),
+            estimator.SampleAt(1.01_s));
+  // Sampling before the oldest value returns the oldest value
+  EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}),
+            estimator.SampleAt(0.5_s));
+  // Sampling after the newest value returns the newest value
+  EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0_m, frc::Rotation2d{}}),
+            estimator.SampleAt(2.5_s));
+
+  // Add a vision measurement after the odometry measurements (while keeping all
+  // of the old odometry measurements)
+  estimator.AddVisionMeasurement(frc::Pose2d{2_m, 0_m, frc::Rotation2d{1_rad}},
+                                 2.2_s);
+
+  // Make sure nothing changed (except the newest value)
+  EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}),
+            estimator.SampleAt(1.02_s));
+  EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}),
+            estimator.SampleAt(1.01_s));
+  EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}),
+            estimator.SampleAt(0.5_s));
+
+  // Add a vision measurement before the odometry measurements that's still in
+  // the buffer
+  estimator.AddVisionMeasurement(frc::Pose2d{1_m, 0.2_m, frc::Rotation2d{}},
+                                 0.9_s);
+
+  // Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
+  EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0.1_m, frc::Rotation2d{}}),
+            estimator.SampleAt(1.02_s));
+  EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0.1_m, frc::Rotation2d{}}),
+            estimator.SampleAt(1.01_s));
+  EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0.1_m, frc::Rotation2d{}}),
+            estimator.SampleAt(0.5_s));
+  EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0.1_m, frc::Rotation2d{}}),
+            estimator.SampleAt(2.5_s));
+}
diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
index f1024efbc92..8a38279beae 100644
--- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
@@ -311,3 +311,74 @@ TEST(SwerveDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
               estimator.GetEstimatedPosition().Rotation().Radians().value(),
               1e-6);
 }
+
+TEST(SwerveDrivePoseEstimatorTest, TestSampleAt) {
+  frc::SwerveDriveKinematics<4> kinematics{
+      frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
+      frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+  frc::SwerveDrivePoseEstimator estimator{
+      kinematics,
+      frc::Rotation2d{},
+      {frc::SwerveModulePosition{}, frc::SwerveModulePosition{},
+       frc::SwerveModulePosition{}, frc::SwerveModulePosition{}},
+      frc::Pose2d{},
+      {1.0, 1.0, 1.0},
+      {1.0, 1.0, 1.0}};
+
+  // Returns empty when null
+  EXPECT_EQ(std::nullopt, estimator.SampleAt(1_s));
+
+  // Add odometry measurements, but don't fill up the buffer
+  // Add a tiny tolerance for the upper bound because of floating point rounding
+  // error
+  for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
+    wpi::array<frc::SwerveModulePosition, 4> wheelPositions{
+        {frc::SwerveModulePosition{units::meter_t{time}, frc::Rotation2d{}},
+         frc::SwerveModulePosition{units::meter_t{time}, frc::Rotation2d{}},
+         frc::SwerveModulePosition{units::meter_t{time}, frc::Rotation2d{}},
+         frc::SwerveModulePosition{units::meter_t{time}, frc::Rotation2d{}}}};
+    estimator.UpdateWithTime(units::second_t{time}, frc::Rotation2d{},
+                             wheelPositions);
+  }
+
+  // Sample at an added time
+  EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}),
+            estimator.SampleAt(1.02_s));
+  // Sample between updates (test interpolation)
+  EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}),
+            estimator.SampleAt(1.01_s));
+  // Sampling before the oldest value returns the oldest value
+  EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}),
+            estimator.SampleAt(0.5_s));
+  // Sampling after the newest value returns the newest value
+  EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0_m, frc::Rotation2d{}}),
+            estimator.SampleAt(2.5_s));
+
+  // Add a vision measurement after the odometry measurements (while keeping all
+  // of the old odometry measurements)
+  estimator.AddVisionMeasurement(frc::Pose2d{2_m, 0_m, frc::Rotation2d{1_rad}},
+                                 2.2_s);
+
+  // Make sure nothing changed (except the newest value)
+  EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}),
+            estimator.SampleAt(1.02_s));
+  EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}),
+            estimator.SampleAt(1.01_s));
+  EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}),
+            estimator.SampleAt(0.5_s));
+
+  // Add a vision measurement before the odometry measurements that's still in
+  // the buffer
+  estimator.AddVisionMeasurement(frc::Pose2d{1_m, 0.2_m, frc::Rotation2d{}},
+                                 0.9_s);
+
+  // Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
+  EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0.1_m, frc::Rotation2d{}}),
+            estimator.SampleAt(1.02_s));
+  EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0.1_m, frc::Rotation2d{}}),
+            estimator.SampleAt(1.01_s));
+  EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0.1_m, frc::Rotation2d{}}),
+            estimator.SampleAt(0.5_s));
+  EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0.1_m, frc::Rotation2d{}}),
+            estimator.SampleAt(2.5_s));
+}
diff --git a/wpiunits/src/main/java/edu/wpi/first/units/Units.java b/wpiunits/src/main/java/edu/wpi/first/units/Units.java
index 825acf4b5b4..26b6b358cbe 100644
--- a/wpiunits/src/main/java/edu/wpi/first/units/Units.java
+++ b/wpiunits/src/main/java/edu/wpi/first/units/Units.java
@@ -182,6 +182,13 @@ private Units() {
   public static final Velocity<Velocity<Distance>> MetersPerSecondPerSecond =
       MetersPerSecond.per(Second);
 
+  /**
+   * A unit of linear acceleration equivalent to accelerating at a rate of one {@link #Foot Foot}
+   * per {@link #Second} every second.
+   */
+  public static final Velocity<Velocity<Distance>> FeetPerSecondPerSecond =
+      FeetPerSecond.per(Second);
+
   /**
    * A unit of angular acceleration equivalent to accelerating at a rate of one {@link #Rotations
    * Rotation} per {@link #Second} every second.