Merge branch 'main' into 2027

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

@@ -287,6 +287,24 @@ public abstract class Command implements Sendable {
     return group;
   }
 
+  /**
+   * Creates a new command that runs this command and the deadline in parallel, finishing (and
+   * interrupting this command) when the deadline finishes.
+   *
+   * <p>Note: This decorator works by adding this command to a composition. The command the
+   * decorator was called on cannot be scheduled independently or be added to a different
+   * composition (namely, decorators), unless it is manually cleared from the list of composed
+   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
+   * returned from this method can be further decorated without issue.
+   *
+   * @param deadline the deadline of the command group
+   * @return the decorated command
+   * @see Command#deadlineFor
+   */
+  public ParallelDeadlineGroup withDeadline(Command deadline) {
+    return new ParallelDeadlineGroup(deadline, this);
+  }
+
   /**
    * Decorates this command with a set of commands to run parallel to it, ending when the calling
    * command ends and interrupting all the others. Often more convenient/less-verbose than
@@ -321,6 +339,7 @@ public abstract class Command implements Sendable {
    * @param parallel the commands to run in parallel. Note the parallel commands will be interrupted
    *     when the deadline command ends
    * @return the decorated command
+   * @see Command#withDeadline
    */
   public ParallelDeadlineGroup deadlineFor(Command... parallel) {
     return new ParallelDeadlineGroup(this, parallel);

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

@@ -38,6 +38,7 @@ import java.util.function.Supplier;
  *
  * <p>This class is provided by the NewCommands VendorDep
  */
+@SuppressWarnings("removal")
 public class MecanumControllerCommand extends Command {
   private final Timer m_timer = new Timer();
   private final boolean m_usePID;
@@ -53,7 +54,7 @@ public class MecanumControllerCommand extends Command {
   private final PIDController m_frontRightController;
   private final PIDController m_rearRightController;
   private final Supplier<MecanumDriveWheelSpeeds> m_currentWheelSpeeds;
-  private final Consumer<MecanumDriveMotorVoltages> m_outputDriveVoltages;
+  private final MecanumVoltagesConsumer m_outputDriveVoltages;
   private final Consumer<MecanumDriveWheelSpeeds> m_outputWheelSpeeds;
   private double m_prevFrontLeftSpeedSetpoint; // m/s
   private double m_prevRearLeftSpeedSetpoint; // m/s
@@ -84,8 +85,7 @@ public class MecanumControllerCommand extends Command {
    * @param frontRightController The front right wheel velocity PID.
    * @param rearRightController The rear right wheel velocity PID.
    * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing the current wheel speeds.
-   * @param outputDriveVoltages A MecanumDriveMotorVoltages object containing the output motor
-   *     voltages.
+   * @param outputDriveVoltages A MecanumVoltagesConsumer that consumes voltages of mecanum motors.
    * @param requirements The subsystems to require.
    */
   @SuppressWarnings("this-escape")
@@ -104,7 +104,7 @@ public class MecanumControllerCommand extends Command {
       PIDController frontRightController,
       PIDController rearRightController,
       Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
-      Consumer<MecanumDriveMotorVoltages> outputDriveVoltages,
+      MecanumVoltagesConsumer outputDriveVoltages,
       Subsystem... requirements) {
     m_trajectory = requireNonNullParam(trajectory, "trajectory", "MecanumControllerCommand");
     m_pose = requireNonNullParam(pose, "pose", "MecanumControllerCommand");
@@ -145,6 +145,139 @@ public class MecanumControllerCommand extends Command {
     addRequirements(requirements);
   }
 
+  /**
+   * Constructs a new MecanumControllerCommand that when executed will follow the provided
+   * trajectory. PID control and feedforward are handled internally. Outputs are scaled from -12 to
+   * 12 as a voltage output to the motor.
+   *
+   * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path
+   * this is left to the user, since it is not appropriate for paths with nonstationary endstates.
+   *
+   * @param trajectory The trajectory to follow.
+   * @param pose A function that supplies the robot pose - use one of the odometry classes to
+   *     provide this.
+   * @param feedforward The feedforward to use for the drivetrain.
+   * @param kinematics The kinematics for the robot drivetrain.
+   * @param xController The Trajectory Tracker PID controller for the robot's x position.
+   * @param yController The Trajectory Tracker PID controller for the robot's y position.
+   * @param thetaController The Trajectory Tracker PID controller for angle for the robot.
+   * @param desiredRotation The angle that the robot should be facing. This is sampled at each time
+   *     step.
+   * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
+   * @param frontLeftController The front left wheel velocity PID.
+   * @param rearLeftController The rear left wheel velocity PID.
+   * @param frontRightController The front right wheel velocity PID.
+   * @param rearRightController The rear right wheel velocity PID.
+   * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing the current wheel speeds.
+   * @param outputDriveVoltages A MecanumDriveMotorVoltages object containing the output motor
+   *     voltages.
+   * @param requirements The subsystems to require.
+   */
+  @Deprecated(since = "2025", forRemoval = true)
+  public MecanumControllerCommand(
+      Trajectory trajectory,
+      Supplier<Pose2d> pose,
+      SimpleMotorFeedforward feedforward,
+      MecanumDriveKinematics kinematics,
+      PIDController xController,
+      PIDController yController,
+      ProfiledPIDController thetaController,
+      Supplier<Rotation2d> desiredRotation,
+      double maxWheelVelocityMetersPerSecond,
+      PIDController frontLeftController,
+      PIDController rearLeftController,
+      PIDController frontRightController,
+      PIDController rearRightController,
+      Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
+      Consumer<MecanumDriveMotorVoltages> outputDriveVoltages,
+      Subsystem... requirements) {
+    this(
+        trajectory,
+        pose,
+        feedforward,
+        kinematics,
+        xController,
+        yController,
+        thetaController,
+        desiredRotation,
+        maxWheelVelocityMetersPerSecond,
+        frontLeftController,
+        rearLeftController,
+        frontRightController,
+        rearRightController,
+        currentWheelSpeeds,
+        (frontLeft, frontRight, rearLeft, rearRight) ->
+            outputDriveVoltages.accept(
+                new MecanumDriveMotorVoltages(frontLeft, frontRight, rearLeft, rearRight)),
+        requirements);
+  }
+
+  /**
+   * Constructs a new MecanumControllerCommand that when executed will follow the provided
+   * trajectory. PID control and feedforward are handled internally. Outputs are scaled from -12 to
+   * 12 as a voltage output to the motor.
+   *
+   * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path
+   * this is left to the user, since it is not appropriate for paths with nonstationary endstates.
+   *
+   * <p>Note 2: The final rotation of the robot will be set to the rotation of the final pose in the
+   * trajectory. The robot will not follow the rotations from the poses at each timestep. If
+   * alternate rotation behavior is desired, the other constructor with a supplier for rotation
+   * should be used.
+   *
+   * @param trajectory The trajectory to follow.
+   * @param pose A function that supplies the robot pose - use one of the odometry classes to
+   *     provide this.
+   * @param feedforward The feedforward to use for the drivetrain.
+   * @param kinematics The kinematics for the robot drivetrain.
+   * @param xController The Trajectory Tracker PID controller for the robot's x position.
+   * @param yController The Trajectory Tracker PID controller for the robot's y position.
+   * @param thetaController The Trajectory Tracker PID controller for angle for the robot.
+   * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
+   * @param frontLeftController The front left wheel velocity PID.
+   * @param rearLeftController The rear left wheel velocity PID.
+   * @param frontRightController The front right wheel velocity PID.
+   * @param rearRightController The rear right wheel velocity PID.
+   * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing the current wheel speeds.
+   * @param outputDriveVoltages A MecanumVoltagesConsumer that consumes voltages of mecanum motors.
+   * @param requirements The subsystems to require.
+   */
+  public MecanumControllerCommand(
+      Trajectory trajectory,
+      Supplier<Pose2d> pose,
+      SimpleMotorFeedforward feedforward,
+      MecanumDriveKinematics kinematics,
+      PIDController xController,
+      PIDController yController,
+      ProfiledPIDController thetaController,
+      double maxWheelVelocityMetersPerSecond,
+      PIDController frontLeftController,
+      PIDController rearLeftController,
+      PIDController frontRightController,
+      PIDController rearRightController,
+      Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
+      MecanumVoltagesConsumer outputDriveVoltages,
+      Subsystem... requirements) {
+    this(
+        trajectory,
+        pose,
+        feedforward,
+        kinematics,
+        xController,
+        yController,
+        thetaController,
+        () ->
+            trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(),
+        maxWheelVelocityMetersPerSecond,
+        frontLeftController,
+        rearLeftController,
+        frontRightController,
+        rearRightController,
+        currentWheelSpeeds,
+        outputDriveVoltages,
+        requirements);
+  }
+
   /**
    * Constructs a new MecanumControllerCommand that when executed will follow the provided
    * trajectory. PID control and feedforward are handled internally. Outputs are scaled from -12 to
@@ -176,6 +309,7 @@ public class MecanumControllerCommand extends Command {
    *     voltages.
    * @param requirements The subsystems to require.
    */
+  @Deprecated(since = "2025", forRemoval = true)
   public MecanumControllerCommand(
       Trajectory trajectory,
       Supplier<Pose2d> pose,
@@ -200,15 +334,15 @@ public class MecanumControllerCommand extends Command {
         xController,
         yController,
         thetaController,
-        () ->
-            trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(),
         maxWheelVelocityMetersPerSecond,
         frontLeftController,
         rearLeftController,
         frontRightController,
         rearRightController,
         currentWheelSpeeds,
-        outputDriveVoltages,
+        (frontLeft, frontRight, rearLeft, rearRight) ->
+            outputDriveVoltages.accept(
+                new MecanumDriveMotorVoltages(frontLeft, frontRight, rearLeft, rearRight)),
         requirements);
   }
 
@@ -400,8 +534,7 @@ public class MecanumControllerCommand extends Command {
                   m_currentWheelSpeeds.get().rearRightMetersPerSecond, rearRightSpeedSetpoint);
 
       m_outputDriveVoltages.accept(
-          new MecanumDriveMotorVoltages(
-              frontLeftOutput, frontRightOutput, rearLeftOutput, rearRightOutput));
+          frontLeftOutput, frontRightOutput, rearLeftOutput, rearRightOutput);
 
     } else {
       m_outputWheelSpeeds.accept(
@@ -422,4 +555,22 @@ public class MecanumControllerCommand extends Command {
   public boolean isFinished() {
     return m_timer.hasElapsed(m_trajectory.getTotalTimeSeconds());
   }
+
+  /** A consumer to represent an operation on the voltages of a mecanum drive. */
+  @FunctionalInterface
+  public interface MecanumVoltagesConsumer {
+    /**
+     * Accepts the voltages to perform some operation with them.
+     *
+     * @param frontLeftVoltage The voltage of the front left motor.
+     * @param frontRightVoltage The voltage of the front right motor.
+     * @param rearLeftVoltage The voltage of the rear left motor.
+     * @param rearRightVoltage The voltage of the rear left motor.
+     */
+    void accept(
+        double frontLeftVoltage,
+        double frontRightVoltage,
+        double rearLeftVoltage,
+        double rearRightVoltage);
+  }
 }

wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp

@@ -121,6 +121,10 @@ CommandPtr Command::OnlyIf(std::function<bool()> condition) && {
   return std::move(*this).ToPtr().OnlyIf(std::move(condition));
 }
 
+CommandPtr Command::WithDeadline(CommandPtr&& deadline) && {
+  return std::move(*this).ToPtr().WithDeadline(std::move(deadline));
+}
+
 CommandPtr Command::DeadlineFor(CommandPtr&& parallel) && {
   return std::move(*this).ToPtr().DeadlineFor(std::move(parallel));
 }

wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp

@@ -168,6 +168,15 @@ CommandPtr CommandPtr::OnlyIf(std::function<bool()> condition) && {
   return std::move(*this).Unless(std::not_fn(std::move(condition)));
 }
 
+CommandPtr CommandPtr::WithDeadline(CommandPtr&& deadline) && {
+  AssertValid();
+  std::vector<std::unique_ptr<Command>> vec;
+  vec.emplace_back(std::move(m_ptr));
+  m_ptr = std::make_unique<ParallelDeadlineGroup>(std::move(deadline).Unwrap(),
+                                                  std::move(vec));
+  return std::move(*this);
+}
+
 CommandPtr CommandPtr::DeadlineWith(CommandPtr&& parallel) && {
   AssertValid();
   std::vector<std::unique_ptr<Command>> vec;

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

@@ -309,6 +309,16 @@ class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
   [[nodiscard]]
   CommandPtr OnlyIf(std::function<bool()> condition) &&;
 
+  /**
+   * Creates a new command that runs this command and the deadline in parallel,
+   * finishing (and interrupting this command) when the deadline finishes.
+   *
+   * @param deadline the deadline of the command group
+   * @return the decorated command
+   * @see DeadlineFor
+   */
+  CommandPtr WithDeadline(CommandPtr&& deadline) &&;
+
   /**
    * Decorates this command with a set of commands to run parallel to it, ending
    * when the calling command ends and interrupting all the others. Often more
@@ -318,9 +328,11 @@ class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
    * @param parallel the commands to run in parallel. Note the parallel commands
    * will be interupted when the deadline command ends
    * @return the decorated command
+   * @see WithDeadline
    */
   [[nodiscard]]
   CommandPtr DeadlineFor(CommandPtr&& parallel) &&;
+
   /**
    * Decorates this command with a set of commands to run parallel to it, ending
    * when the last command ends. Often more convenient/less-verbose than

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

@@ -182,6 +182,16 @@ class CommandPtr final {
   [[nodiscard]]
   CommandPtr OnlyIf(std::function<bool()> condition) &&;
 
+  /**
+   * Creates a new command that runs this command and the deadline in parallel,
+   * finishing (and interrupting this command) when the deadline finishes.
+   *
+   * @param deadline the deadline of the command group
+   * @return the decorated command
+   * @see DeadlineFor
+   */
+  CommandPtr WithDeadline(CommandPtr&& deadline) &&;
+
   /**
    * Decorates this command with a set of commands to run parallel to it, ending
    * when the calling command ends and interrupting all the others. Often more

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java

@@ -271,6 +271,57 @@ class CommandDecoratorTest extends CommandTestBase {
     }
   }
 
+  @Test
+  void withDeadlineTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean finish = new AtomicBoolean(false);
+
+      Command endsBeforeGroup = Commands.none().withDeadline(Commands.waitUntil(finish::get));
+      scheduler.schedule(endsBeforeGroup);
+      scheduler.run();
+      assertTrue(scheduler.isScheduled(endsBeforeGroup));
+      finish.set(true);
+      scheduler.run();
+      assertFalse(scheduler.isScheduled(endsBeforeGroup));
+      finish.set(false);
+
+      Command endsAfterGroup = Commands.idle().withDeadline(Commands.waitUntil(finish::get));
+      scheduler.schedule(endsAfterGroup);
+      scheduler.run();
+      assertTrue(scheduler.isScheduled(endsAfterGroup));
+      finish.set(true);
+      scheduler.run();
+      assertFalse(scheduler.isScheduled(endsAfterGroup));
+    }
+  }
+
+  @Test
+  void withDeadlineOrderTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean dictatorHasRun = new AtomicBoolean(false);
+      AtomicBoolean dictatorWasPolled = new AtomicBoolean(false);
+      Command dictator =
+          new FunctionalCommand(
+              () -> {},
+              () -> dictatorHasRun.set(true),
+              interrupted -> {},
+              () -> {
+                dictatorWasPolled.set(true);
+                return true;
+              });
+      Command other =
+          Commands.run(
+              () ->
+                  assertAll(
+                      () -> assertTrue(dictatorHasRun.get()),
+                      () -> assertTrue(dictatorWasPolled.get())));
+      Command group = other.withDeadline(dictator);
+      scheduler.schedule(group);
+      scheduler.run();
+      assertAll(() -> assertTrue(dictatorHasRun.get()), () -> assertTrue(dictatorWasPolled.get()));
+    }
+  }
+
   @Test
   void alongWithTest() {
     try (CommandScheduler scheduler = new CommandScheduler()) {

wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp

@@ -221,6 +221,27 @@ TEST_F(CommandDecoratorTest, DeadlineFor) {
   EXPECT_FALSE(scheduler.IsScheduled(group));
 }
 
+TEST_F(CommandDecoratorTest, WithDeadline) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finish = false;
+
+  auto dictator = WaitUntilCommand([&finish] { return finish; });
+  auto endsAfter = WaitUntilCommand([] { return false; });
+
+  auto group = std::move(endsAfter).WithDeadline(std::move(dictator).ToPtr());
+
+  scheduler.Schedule(group);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduler.IsScheduled(group));
+
+  finish = true;
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(group));
+}
+
 TEST_F(CommandDecoratorTest, AlongWith) {
   CommandScheduler scheduler = GetScheduler();
 
@@ -283,6 +304,33 @@ TEST_F(CommandDecoratorTest, DeadlineForOrder) {
   EXPECT_TRUE(dictatorWasPolled);
 }
 
+TEST_F(CommandDecoratorTest, WithDeadlineOrder) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool dictatorHasRun = false;
+  bool dictatorWasPolled = false;
+
+  auto dictator =
+      FunctionalCommand([] {}, [&dictatorHasRun] { dictatorHasRun = true; },
+                        [](bool interrupted) {},
+                        [&dictatorWasPolled] {
+                          dictatorWasPolled = true;
+                          return true;
+                        });
+  auto other = RunCommand([&dictatorHasRun, &dictatorWasPolled] {
+    EXPECT_TRUE(dictatorHasRun);
+    EXPECT_TRUE(dictatorWasPolled);
+  });
+
+  auto group = std::move(other).WithDeadline(std::move(dictator).ToPtr());
+
+  scheduler.Schedule(group);
+  scheduler.Run();
+
+  EXPECT_TRUE(dictatorHasRun);
+  EXPECT_TRUE(dictatorWasPolled);
+}
+
 TEST_F(CommandDecoratorTest, AlongWithOrder) {
   CommandScheduler scheduler = GetScheduler();