From 8c107e4b7583e952963c516a524d58d8aa95c8de Mon Sep 17 00:00:00 2001
From: Wispy <101812473+WispySparks@users.noreply.github.com>
Date: Fri, 24 May 2024 18:02:36 -0500
Subject: [PATCH] [wpilibj] Add TimedRobot.addPeriodic() measure overloads
 (#6654)

---
 .../edu/wpi/first/wpilibj/TimedRobot.java     | 32 +++++++++++++++++++
 1 file changed, 32 insertions(+)

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java
index 12346c445a..d6b5c32b9d 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java
@@ -4,11 +4,15 @@
 
 package edu.wpi.first.wpilibj;
 
+import static edu.wpi.first.units.Units.Seconds;
+
 import edu.wpi.first.hal.DriverStationJNI;
 import edu.wpi.first.hal.FRCNetComm.tInstances;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.NotifierJNI;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Time;
 import java.util.PriorityQueue;
 
 /**
@@ -178,4 +182,32 @@ public class TimedRobot extends IterativeRobotBase {
   public final void addPeriodic(Runnable callback, double periodSeconds, double offsetSeconds) {
     m_callbacks.add(new Callback(callback, m_startTime, periodSeconds, offsetSeconds));
   }
+
+  /**
+   * Add a callback to run at a specific period.
+   *
+   * <p>This is scheduled on TimedRobot's Notifier, so TimedRobot and the callback run
+   * synchronously. Interactions between them are thread-safe.
+   *
+   * @param callback The callback to run.
+   * @param period The period at which to run the callback.
+   */
+  public final void addPeriodic(Runnable callback, Measure<Time> period) {
+    addPeriodic(callback, period.in(Seconds));
+  }
+
+  /**
+   * Add a callback to run at a specific period with a starting time offset.
+   *
+   * <p>This is scheduled on TimedRobot's Notifier, so TimedRobot and the callback run
+   * synchronously. Interactions between them are thread-safe.
+   *
+   * @param callback The callback to run.
+   * @param period The period at which to run the callback.
+   * @param offset The offset from the common starting time. This is useful for scheduling a
+   *     callback in a different timeslot relative to TimedRobot.
+   */
+  public final void addPeriodic(Runnable callback, Measure<Time> period, Measure<Time> offset) {
+    addPeriodic(callback, period.in(Seconds), offset.in(Seconds));
+  }
 }