Fix and deprecate TrapezoidProfileCommand

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

@@ -8,7 +8,6 @@
 import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
 
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
-import edu.wpi.first.wpilibj.Timer;
 import java.util.function.Consumer;
 import java.util.function.Supplier;
 
@@ -17,12 +16,12 @@
  *
  * <p>This class is provided by the NewCommands VendorDep
  */
+@Deprecated(since = "2025", forRemoval = true)
 public class TrapezoidProfileCommand extends Command {
   private final TrapezoidProfile m_profile;
   private final Consumer<State> m_output;
   private final Supplier<State> m_goal;
   private final Supplier<State> m_currentState;
-  private final Timer m_timer = new Timer();
 
   /**
    * Creates a new TrapezoidProfileCommand that will execute the given {@link TrapezoidProfile}.
@@ -50,22 +49,20 @@ public TrapezoidProfileCommand(
 
   @Override
   public void initialize() {
-    m_timer.restart();
   }
 
   @Override
   @SuppressWarnings("removal")
   public void execute() {
-    m_output.accept(m_profile.calculate(m_timer.get(), m_currentState.get(), m_goal.get()));
+    m_output.accept(m_profile.calculate(0.02, m_currentState.get(), m_goal.get()));
   }
 
   @Override
   public void end(boolean interrupted) {
-    m_timer.stop();
   }
 
   @Override
   public boolean isFinished() {
-    return m_timer.hasElapsed(m_profile.totalTime());
+    return m_profile.isFinished(0);
   }
 }

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

@@ -6,7 +6,6 @@
 
 #include <functional>
 
-#include <frc/Timer.h>
 #include <frc/trajectory/TrapezoidProfile.h>
 
 #include "frc2/command/Command.h"
@@ -54,24 +53,24 @@ class TrapezoidProfileCommand
     this->AddRequirements(requirements);
   }
 
-  void Initialize() override { m_timer.Restart(); }
+  void Initialize() override {}
 
   void Execute() override {
-    m_output(m_profile.Calculate(m_timer.Get(), m_currentState(), m_goal()));
+    m_output(
+        m_profile.Calculate(units::second_t{0.02}, m_currentState(), m_goal()));
   }
 
-  void End(bool interrupted) override { m_timer.Stop(); }
+  void End(bool interrupted) override {}
 
   bool IsFinished() override {
-    return m_timer.HasElapsed(m_profile.TotalTime());
+    return m_profile.IsFinished(units::second_t{0});
   }
 
  private:
   frc::TrapezoidProfile<Distance> m_profile;
   std::function<void(State)> m_output;
   std::function<State()> m_goal;
   std::function<State()> m_currentState;
-  frc::Timer m_timer;
 };
 
 }  // namespace frc2

wpilibcExamples/src/main/cpp/commands/commands.json

@@ -191,22 +191,6 @@
     "replacename": "ReplaceMeSubsystem2",
     "commandversion": 2
   },
-  {
-    "name": "TrapezoidProfileCommand",
-    "description": "A command that executes a trapezoidal motion profile.",
-    "tags": [
-      "trapezoidprofilecommand"
-    ],
-    "foldername": "trapezoidprofilecommand",
-    "headers": [
-      "ReplaceMeTrapezoidProfileCommand.h"
-    ],
-    "source": [
-      "ReplaceMeTrapezoidProfileCommand.cpp"
-    ],
-    "replacename": "ReplaceMeTrapezoidProfileCommand",
-    "commandversion": 2
-  },
   {
     "name": "TrapezoidProfileSubsystem",
     "description": "A subsystem that executes a trapezoidal motion profile.",

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

@@ -34,32 +34,12 @@ void RobotContainer::ConfigureButtonBindings() {
   // Drive forward by 3 meters when the 'A' button is pressed, with a timeout of
   // 10 seconds
   m_driverController.A().OnTrue(
-      DriveDistanceProfiled(3_m, &m_drive).WithTimeout(10_s));
+      m_drive.ProfiledDriveDistance(3_m).WithTimeout(10_s));
 
-  // Do the same thing as above when the 'B' button is pressed, but defined
-  // inline
+  // Do the same thing as above when the 'B' button is pressed, but without
+  // resetting the encoders
   m_driverController.B().OnTrue(
-      frc2::TrapezoidProfileCommand<units::meters>(
-          frc::TrapezoidProfile<units::meters>(
-              // Limit the max acceleration and velocity
-              {DriveConstants::kMaxSpeed, DriveConstants::kMaxAcceleration}),
-          // Pipe the profile state to the drive
-          [this](auto setpointState) {
-            m_drive.SetDriveStates(setpointState, setpointState);
-          },
-          // End at desired position in meters; implicitly starts at 0
-          [] {
-            return frc::TrapezoidProfile<units::meters>::State{3_m, 0_mps};
-          },
-          // Current position
-          [] { return frc::TrapezoidProfile<units::meters>::State{}; },
-          // Require the drive
-          {&m_drive})
-          // Convert to CommandPtr
-          .ToPtr()
-          .BeforeStarting(
-              frc2::cmd::RunOnce([this]() { m_drive.ResetEncoders(); }, {}))
-          .WithTimeout(10_s));
+      m_drive.DynamicProfiledDriveDistance(3_m).WithTimeout(10_s));
 }
 
 frc2::CommandPtr RobotContainer::GetAutonomousCommand() {

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

@@ -2,8 +2,12 @@
 // 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.
 
+// the WPILib BSD license file in the root directory of this project.
+
 #include "subsystems/DriveSubsystem.h"
 
+#include <frc/Timer.h>
+
 using namespace DriveConstants;
 
 DriveSubsystem::DriveSubsystem()
@@ -62,3 +66,48 @@ units::meter_t DriveSubsystem::GetRightEncoderDistance() {
 void DriveSubsystem::SetMaxOutput(double maxOutput) {
   m_drive.SetMaxOutput(maxOutput);
 }
+
+frc2::CommandPtr DriveSubsystem::ProfiledDriveDistance(
+    units::meter_t distance) {
+  auto profile =
+      frc::TrapezoidProfile<units::meters>{{kMaxSpeed, kMaxAcceleration}};
+  frc::Timer timer{};
+  return StartRun(
+             [&] {
+               timer.Restart();
+               ResetEncoders();
+             },
+             [&] {
+               auto setpoint = profile.Calculate(
+                   timer.Get(), {}, {distance, units::meters_per_second_t{0}});
+               SetDriveStates(setpoint, setpoint);
+             })
+      .Until([&] { return profile.IsFinished(units::second_t{0}); });
+}
+
+frc2::CommandPtr DriveSubsystem::DynamicProfiledDriveDistance(
+    units::meter_t distance) {
+  auto profile =
+      frc::TrapezoidProfile<units::meters>{{kMaxSpeed, kMaxAcceleration}};
+  return StartRun(
+             [&] {
+               m_initialLeftDistance = GetLeftEncoderDistance();
+               m_initialRightDistance = GetRightEncoderDistance();
+             },
+             [&] {
+               auto leftSetpoint = profile.Calculate(
+                   kDt,
+                   {GetLeftEncoderDistance(),
+                    units::meters_per_second_t{m_leftLeader.GetEncoderRate()}},
+                   {m_initialLeftDistance + distance,
+                    units::meters_per_second_t{0}});
+               auto rightSetpoint = profile.Calculate(
+                   kDt,
+                   {GetLeftEncoderDistance(),
+                    units::meters_per_second_t{m_rightLeader.GetEncoderRate()}},
+                   {m_initialRightDistance + distance,
+                    units::meters_per_second_t{0}});
+               SetDriveStates(leftSetpoint, rightSetpoint);
+             })
+      .Until([&] { return profile.IsFinished(units::second_t{0}); });
+}

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

@@ -22,6 +22,7 @@
  */
 
 namespace DriveConstants {
+inline constexpr units::second_t kDt{0.02};
 inline constexpr int kLeftMotor1Port = 0;
 inline constexpr int kLeftMotor2Port = 1;
 inline constexpr int kRightMotor1Port = 2;

wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h

@@ -8,6 +8,7 @@
 #include <frc/controller/SimpleMotorFeedforward.h>
 #include <frc/drive/DifferentialDrive.h>
 #include <frc/trajectory/TrapezoidProfile.h>
+#include <frc2/command/CommandPtr.h>
 #include <frc2/command/SubsystemBase.h>
 #include <units/length.h>
 
@@ -69,7 +70,29 @@ class DriveSubsystem : public frc2::SubsystemBase {
    */
   void SetMaxOutput(double maxOutput);
 
+  /**
+   * Creates a command to drive forward a specified distance using a motion
+   * profile.
+   *
+   * @param distance The distance to drive forward.
+   * @return A command.
+   */
+  [[nodiscard]]
+  frc2::CommandPtr ProfiledDriveDistance(units::meter_t distance);
+
+  /**
+   * Creates a command to drive forward a specified distance using a motion
+   * profile without resetting the encoders.
+   *
+   * @param distance The distance to drive forward.
+   * @return A command.
+   */
+  [[nodiscard]]
+  frc2::CommandPtr DynamicProfiledDriveDistance(units::meter_t distance);
+
  private:
+  units::meter_t m_initialLeftDistance;
+  units::meter_t m_initialRightDistance;
   // Components (e.g. motor controllers and sensors) should generally be
   // declared private and exposed only through public methods.
 

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

@@ -119,16 +119,6 @@
     "replacename": "ReplaceMeSubsystem",
     "commandversion": 2
   },
-  {
-    "name": "TrapezoidProfileCommand",
-    "description": "A command that executes a trapezoidal motion profile.",
-    "tags": [
-      "trapezoidprofilecommand"
-    ],
-    "foldername": "trapezoidprofilecommand",
-    "replacename": "ReplaceMeTrapezoidProfileCommand",
-    "commandversion": 2
-  },
   {
     "name": "TrapezoidProfileSubsystem",
     "description": "A subsystem that executes a trapezoidal motion profile.",