From d7dfa63ae9c754bb7e413b762a8509f56a30a0b5 Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Mon, 22 Apr 2024 13:34:52 +0800
Subject: [PATCH] [commands] WaitCommand: add Measure<Time> overload (#6386)

Also add waitTime() factory.
---
 .../wpi/first/wpilibj2/command/Command.java   | 20 +++++++++++++++++++
 .../wpi/first/wpilibj2/command/Commands.java  | 13 ++++++++++++
 .../first/wpilibj2/command/WaitCommand.java   | 13 ++++++++++++
 3 files changed, 46 insertions(+)

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 5ea1a362ec..0186b6550d 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
@@ -4,8 +4,11 @@
 
 package edu.wpi.first.wpilibj2.command;
 
+import static edu.wpi.first.units.Units.Second;
 import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
 
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Time;
 import edu.wpi.first.util.function.BooleanConsumer;
 import edu.wpi.first.util.sendable.Sendable;
 import edu.wpi.first.util.sendable.SendableBuilder;
@@ -149,6 +152,23 @@ public abstract class Command implements Sendable {
     return raceWith(new WaitCommand(seconds));
   }
 
+  /**
+   * Decorates this command with a timeout. If the specified timeout is exceeded before the command
+   * finishes normally, the command will be interrupted and un-scheduled.
+   *
+   * <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 time the timeout duration
+   * @return the command with the timeout added
+   */
+  public ParallelRaceGroup withTimeout(Measure<Time> time) {
+    return withTimeout(time.in(Second));
+  }
+
   /**
    * Decorates this command with an interrupt condition. If the specified condition becomes true
    * before the command finishes normally, the command will be interrupted and un-scheduled.
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Commands.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Commands.java
index 9202d196a1..3b8f9f8fa8 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Commands.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Commands.java
@@ -6,6 +6,8 @@ package edu.wpi.first.wpilibj2.command;
 
 import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
 
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Time;
 import java.util.Map;
 import java.util.Set;
 import java.util.function.BooleanSupplier;
@@ -115,6 +117,17 @@ public final class Commands {
     return new WaitCommand(seconds);
   }
 
+  /**
+   * Constructs a command that does nothing, finishing after a specified duration.
+   *
+   * @param time after how long the command finishes
+   * @return the command
+   * @see WaitCommand
+   */
+  public static Command waitTime(Measure<Time> time) {
+    return new WaitCommand(time);
+  }
+
   /**
    * Constructs a command that does nothing, finishing once a condition becomes true.
    *
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java
index 6d4c52bb61..a085192513 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java
@@ -4,6 +4,10 @@
 
 package edu.wpi.first.wpilibj2.command;
 
+import static edu.wpi.first.units.Units.Second;
+
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Time;
 import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.Timer;
@@ -30,6 +34,15 @@ public class WaitCommand extends Command {
     SendableRegistry.setName(this, getName() + ": " + seconds + " seconds");
   }
 
+  /**
+   * Creates a new WaitCommand. This command will do nothing, and end after the specified duration.
+   *
+   * @param time the time to wait
+   */
+  public WaitCommand(Measure<Time> time) {
+    this(time.in(Second));
+  }
+
   @Override
   public void initialize() {
     m_timer.restart();