From 1321fd4716170cffd59fd6fa813fb590d4344db3 Mon Sep 17 00:00:00 2001
From: Sam Carlberg <sam@slfc.dev>
Date: Sun, 19 May 2024 11:35:24 -0400
Subject: [PATCH] Add simple wait to coroutines

Helpful for timed sequences without needing to wrap async methods in a command
---
 .../first/wpilibj/commandsv3/Coroutine.java   | 23 +++++++++++++++++++
 1 file changed, 23 insertions(+)

diff --git a/commandsv3/src/main/java/edu/wpi/first/wpilibj/commandsv3/Coroutine.java b/commandsv3/src/main/java/edu/wpi/first/wpilibj/commandsv3/Coroutine.java
index bafe3849ef1..67610bcdf40 100644
--- a/commandsv3/src/main/java/edu/wpi/first/wpilibj/commandsv3/Coroutine.java
+++ b/commandsv3/src/main/java/edu/wpi/first/wpilibj/commandsv3/Coroutine.java
@@ -1,5 +1,7 @@
 package edu.wpi.first.wpilibj.commandsv3;
 
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Time;
 import java.util.Collection;
 
 /**
@@ -68,6 +70,27 @@ default void awaitAny(Collection<Command> commands) {
     scheduler().awaitAny(commands);
   }
 
+  /**
+   * Waits for some duration of time to elapse. Returns immediately if the given duration is zero
+   * or negative. Call this within a command or command composition to introduce a simple delay.
+   *
+   * <p>For example, a basic autonomous routine that drives straight for 5 seconds:
+   * {@snippet lang = java :
+   * AsyncCommand timedDrive() {
+   *   return drivebase.run((coroutine) -> {
+   *     drivebase.tankDrive(1, 1);
+   *     coroutine.wait(Seconds.of(5));
+   *     drivebase.stop();
+   *   }).named("Timed Drive");
+   * }
+   * }
+   *
+   * @param duration the duration of time to wait
+   */
+  default void wait(Measure<Time> duration) {
+    await(new WaitCommand(duration));
+  }
+
   /**
    * Advanced users only: this permits access to the backing command scheduler to run custom logic
    * not provided by the standard coroutine methods.