[commands] Fix and deprecate TrapezoidProfileCommand (wpilibsuite#6722)

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}.
@@ -49,23 +48,19 @@ public TrapezoidProfileCommand(
   }
 
   @Override
-  public void initialize() {
-    m_timer.restart();
-  }
+  public void initialize() {}
 
   @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();
-  }
+  public void end(boolean interrupted) {}
 
   @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

@@ -6,8 +6,6 @@
 
 #include <frc2/command/Commands.h>
 
-#include "commands/DriveDistanceProfiled.h"
-
 RobotContainer::RobotContainer() {
   // Initialize all of your commands and subsystems here
 
@@ -34,32 +32,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

@@ -4,6 +4,8 @@
 
 #include "subsystems/DriveSubsystem.h"
 
+#include <frc/RobotController.h>
+
 using namespace DriveConstants;
 
 DriveSubsystem::DriveSubsystem()
@@ -32,14 +34,21 @@ void DriveSubsystem::Periodic() {
 }
 
 void DriveSubsystem::SetDriveStates(
-    frc::TrapezoidProfile<units::meters>::State left,
-    frc::TrapezoidProfile<units::meters>::State right) {
-  m_leftLeader.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
-                           left.position.value(),
-                           m_feedforward.Calculate(left.velocity) / 12_V);
-  m_rightLeader.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
-                            right.position.value(),
-                            m_feedforward.Calculate(right.velocity) / 12_V);
+    frc::TrapezoidProfile<units::meters>::State currentLeft,
+    frc::TrapezoidProfile<units::meters>::State currentRight,
+    frc::TrapezoidProfile<units::meters>::State nextLeft,
+    frc::TrapezoidProfile<units::meters>::State nextRight) {
+  // Feedforward is divided by battery voltage to normalize it to [-1, 1]
+  m_leftLeader.SetSetpoint(
+      ExampleSmartMotorController::PIDMode::kPosition,
+      currentLeft.position.value(),
+      m_feedforward.Calculate(currentLeft.velocity, nextLeft.velocity) /
+          frc::RobotController::GetBatteryVoltage());
+  m_rightLeader.SetSetpoint(
+      ExampleSmartMotorController::PIDMode::kPosition,
+      currentRight.position.value(),
+      m_feedforward.Calculate(currentRight.velocity, nextRight.velocity) /
+          frc::RobotController::GetBatteryVoltage());
 }
 
 void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
@@ -62,3 +71,60 @@ units::meter_t DriveSubsystem::GetRightEncoderDistance() {
 void DriveSubsystem::SetMaxOutput(double maxOutput) {
   m_drive.SetMaxOutput(maxOutput);
 }
+
+frc2::CommandPtr DriveSubsystem::ProfiledDriveDistance(
+    units::meter_t distance) {
+  return StartRun(
+             [&] {
+               // Restart timer so profile setpoints start at the beginning
+               m_timer.Restart();
+               ResetEncoders();
+             },
+             [&] {
+               // Current state never changes, so we need to use a timer to get
+               // the setpoints we need to be at
+               auto currentTime = m_timer.Get();
+               auto currentSetpoint =
+                   m_profile.Calculate(currentTime, {}, {distance, 0_mps});
+               auto nextSetpoint = m_profile.Calculate(currentTime + kDt, {},
+                                                       {distance, 0_mps});
+               SetDriveStates(currentSetpoint, currentSetpoint, nextSetpoint,
+                              nextSetpoint);
+             })
+      .Until([&] { return m_profile.IsFinished(0_s); });
+}
+
+frc2::CommandPtr DriveSubsystem::DynamicProfiledDriveDistance(
+    units::meter_t distance) {
+  return StartRun(
+             [&] {
+               // Restart timer so profile setpoints start at the beginning
+               m_timer.Restart();
+               // Store distance so we know the target distance for each encoder
+               m_initialLeftDistance = GetLeftEncoderDistance();
+               m_initialRightDistance = GetRightEncoderDistance();
+             },
+             [&] {
+               // Current state never changes for the duration of the command,
+               // so we need to use a timer to get the setpoints we need to be
+               // at
+               auto currentTime = m_timer.Get();
+
+               auto currentLeftSetpoint = m_profile.Calculate(
+                   currentTime, {m_initialLeftDistance, 0_mps},
+                   {m_initialLeftDistance + distance, 0_mps});
+               auto currentRightSetpoint = m_profile.Calculate(
+                   currentTime, {m_initialRightDistance, 0_mps},
+                   {m_initialRightDistance + distance, 0_mps});
+
+               auto nextLeftSetpoint = m_profile.Calculate(
+                   currentTime + kDt, {m_initialLeftDistance, 0_mps},
+                   {m_initialLeftDistance + distance, 0_mps});
+               auto nextRightSetpoint = m_profile.Calculate(
+                   currentTime + kDt, {m_initialRightDistance, 0_mps},
+                   {m_initialRightDistance + distance, 0_mps});
+               SetDriveStates(currentLeftSetpoint, currentRightSetpoint,
+                              nextLeftSetpoint, nextRightSetpoint);
+             })
+      .Until([&] { return m_profile.IsFinished(0_s); });
+}

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;