Refactor TimedRobot, add support for one-shot actions and cancelling …

wpilibj/src/main/java/edu/wpi/first/wpilibj/SelfCancellableRunnable.java

@@ -0,0 +1,91 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.units.measure.Time;
+import edu.wpi.first.wpilibj.TimedRobot.Cancellable;
+
+public abstract class SelfCancellableRunnable implements Runnable {
+
+  private Cancellable m_cancelThisCallback;
+
+  /**
+   * Schedule this {@link SelfCancellableRunnable} to run every {@code periodSeconds} seconds on the
+   * specified {@code robot}
+   *
+   * <p>This is scheduled on TimedRobot's Notifier, so TimedRobot and the callback run
+   * synchronously. Interactions between them are thread-safe.
+   *
+   * @param robot The {@link TimedRobot} to run this {@link SelfCancellableRunnable} {@code robot}
+   *     must <em>not</em> be {@code null}.
+   * @param periodSeconds The period at which to run the callback in seconds.
+   * @return a {@link TimedRobot.Cancellable} that allows the user to cancel periodic invocation.
+   * @see TimedRobot#addPeriodic(Runnable, double)
+   */
+  public Cancellable schedulePeriodic(TimedRobot robot, double periodSeconds) {
+    return m_cancelThisCallback = robot.addPeriodic(this, periodSeconds);
+  }
+
+  /**
+   * Schedule this {@link SelfCancellableRunnable} to run every {@code periodSeconds} on {@code
+   * robot}. Offset invocation by {@code offsetSeconds} seconds.
+   *
+   * <p>This is scheduled on TimedRobot's Notifier, so TimedRobot and the callback run
+   * synchronously. Interactions between them are thread-safe.
+   *
+   * @param robot The {@link TimedRobot} to run this {@link SelfCancellableRunnable} {@code robot}
+   *     must <em>not</em> be {@code null}.
+   * @param periodSeconds The period at which to run the callback in seconds.
+   * @param offsetSeconds The offset from the common starting time in seconds. This is useful for
+   *     scheduling a callback in a different timeslot relative to TimedRobot.
+   * @return a {@link TimedRobot.Cancellable} that allows the user to cancel periodic invocation.
+   * @see TimedRobot#addPeriodic(Runnable, double, double)
+   */
+  public final Cancellable schedulePeriodic(
+      TimedRobot robot, double periodSeconds, double offsetSeconds) {
+    return m_cancelThisCallback = robot.addPeriodic(this, periodSeconds, offsetSeconds);
+  }
+
+  /**
+   * Schedule this {@link SelfCancellableRunnable} to run periodically on the specified {@link
+   * TimedRobot} at the specified {@code period}.
+   *
+   * <p>This is scheduled on TimedRobot's Notifier, so TimedRobot and the callback run
+   * synchronously. Interactions between them are thread-safe.
+   *
+   * @param robot The {@link TimedRobot} to run this {@link SelfCancellableRunnable} {@code robot}
+   *     must <em>not</em> be {@code null}.
+   * @param period The period at which to run the callback.
+   * @return A {@link Cancellable} that allows the user to cancel periodic invocation.
+   * @see TimedRobot#addPeriodic(Runnable, Time)
+   */
+  public final Cancellable schedulePeriodic(TimedRobot robot, Time period) {
+    return m_cancelThisCallback = robot.addPeriodic(this, period);
+  }
+
+  /**
+   * Schedule this {@link SelfCancellableRunnable} to run periodically at the specified {@code
+   * period} on the specified {@code robot}. Offset invocation by the specified {@code offset}.
+   *
+   * <p>This is scheduled on TimedRobot's Notifier, so TimedRobot and the callback run
+   * synchronously. Interactions between them are thread-safe.
+   *
+   * @param robot The {@link TimedRobot} to run this {@link SelfCancellableRunnable} {@code robot}
+   *     must <em>not</em> be {@code null}.
+   * @param period The period at which to run the callback.
+   * @param offset The offset from the common starting time in seconds. This is useful for
+   *     scheduling a callback in a different timeslot relative to TimedRobot.
+   * @return A {@link Cancellable} that allows the user to cancel periodic invocation.
+   * @see TimedRobot#addPeriodic(Runnable, Time, Time)
+   */
+  public final Cancellable schedulePeriodic(TimedRobot robot, Time period, Time offset) {
+    return m_cancelThisCallback = robot.addPeriodic(this, period, offset);
+  }
+
+  /** Cancel periodic invocation of this {@link SelfCancellableRunnable}. */
+  protected void cancel() {
+    m_cancelThisCallback.cancel();
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java

@@ -20,31 +20,91 @@
  * <p>The TimedRobot class is intended to be subclassed by a user creating a robot program.
  *
  * <p>periodic() functions from the base class are called on an interval by a Notifier instance.
+ *
+ * <p>The addOneShot() function enqueues an action to be run after a specified fixed delay.
  */
 public class TimedRobot extends IterativeRobotBase {
+
+  /** Callback schedule types. */
+  enum CallbackScheduleType {
+    /** Run the callback indefinitely at a preset interval. */
+    PERIODIC {
+      @Override
+      boolean invoke(Runnable action) {
+        action.run();
+        return true;
+      }
+    },
+    /** Run the callback only once. */
+    ONE_SHOT {
+      @Override
+      boolean invoke(Runnable action) {
+        action.run();
+        return false;
+      }
+    },
+    /** Callback is cancelled. Don't run it at all. */
+    CANCELLED {
+      @Override
+      boolean invoke(Runnable action) {
+        return false;
+      }
+    };
+
+    /**
+     * Invoke the specified {@code action} if it should run.
+     *
+     * @param action action to invoke, provided that it should run.
+     * @return {@code true} if the action should be rescheduled, {@code false} otherwise.
+     */
+    abstract boolean invoke(Runnable action);
+  }
+
+  public interface Cancellable {
+    void cancel();
+  }
+
+  /** Holds and invokes the action (a {@link Runnable}) to invoke at a set time. */
   @SuppressWarnings("MemberName")
-  static class Callback implements Comparable<Callback> {
-    public Runnable func;
-    public long period;
-    public long expirationTime;
+  static class Callback implements Comparable<Callback>, Cancellable {
+    private final Runnable func; // Scheduled action
+    public long period; // Time between invocations of a periodic callback
+    public long expirationTime; // The next time to invoke the callback
+    public volatile CallbackScheduleType m_scheduleType;
 
     /**
      * Construct a callback container.
      *
      * @param func The callback to run.
-     * @param startTimeSeconds The common starting point for all callback scheduling in
-     *     microseconds.
-     * @param periodSeconds The period at which to run the callback in microseconds.
-     * @param offsetSeconds The offset from the common starting time in microseconds.
+     * @param startTimeUs The common starting point for all callback scheduling in microseconds.
+     * @param periodUs The period at which to run the callback in microseconds.
+     * @param offsetUs The offset from the common starting time in microseconds.
+     * @param scheduleType How to schedule the callback. Meaningful options are periodic or
+     *     one-shot.
      */
-    Callback(Runnable func, long startTimeUs, long periodUs, long offsetUs) {
+    Callback(
+        Runnable func,
+        long startTimeUs,
+        long periodUs,
+        long offsetUs,
+        CallbackScheduleType scheduleType) {
       this.func = func;
       this.period = periodUs;
       this.expirationTime =
           startTimeUs
               + offsetUs
               + this.period
               + (RobotController.getFPGATime() - startTimeUs) / this.period * this.period;
+      m_scheduleType = scheduleType;
+    }
+
+    @Override
+    public void cancel() {
+      m_scheduleType = CallbackScheduleType.CANCELLED;
+    }
+
+    boolean invoke() {
+      return m_scheduleType.invoke(func);
     }
 
     @Override
@@ -72,7 +132,7 @@ public int compareTo(Callback rhs) {
   // just passed to the JNI bindings.
   private final int m_notifier = NotifierJNI.initializeNotifier();
 
-  private long m_startTimeUs;
+  private final long m_startTimeUs;
 
   private final PriorityQueue<Callback> m_callbacks = new PriorityQueue<>();
 
@@ -117,9 +177,9 @@ public void startCompetition() {
     // Loop forever, calling the appropriate mode-dependent function
     while (true) {
       // We don't have to check there's an element in the queue first because
-      // there's always at least one (the constructor adds one). It's reenqueued
-      // at the end of the loop.
-      var callback = m_callbacks.poll();
+      // there's always at least one (the constructor adds one). If the action
+      // runs periodically, it is rescheduled immediately after it runs.
+      var callback = m_callbacks.peek();
 
       NotifierJNI.updateNotifierAlarm(m_notifier, callback.expirationTime);
 
@@ -128,27 +188,17 @@ public void startCompetition() {
         break;
       }
 
-      callback.func.run();
-
-      // Increment the expiration time by the number of full periods it's behind
-      // plus one to avoid rapid repeat fires from a large loop overrun. We
-      // assume currentTime ≥ expirationTime rather than checking for it since
-      // the callback wouldn't be running otherwise.
-      callback.expirationTime +=
-          callback.period
-              + (currentTime - callback.expirationTime) / callback.period * callback.period;
-      m_callbacks.add(callback);
-
-      // Process all other callbacks that are ready to run
+      // Process all callbacks that are ready to run
       while (m_callbacks.peek().expirationTime <= currentTime) {
         callback = m_callbacks.poll();
 
-        callback.func.run();
+        if (callback.invoke()) {
 
-        callback.expirationTime +=
-            callback.period
-                + (currentTime - callback.expirationTime) / callback.period * callback.period;
-        m_callbacks.add(callback);
+          callback.expirationTime +=
+              callback.period
+                  + (currentTime - callback.expirationTime) / callback.period * callback.period;
+          m_callbacks.add(callback);
+        }
       }
     }
   }
@@ -167,9 +217,18 @@ public void endCompetition() {
    *
    * @param callback The callback to run.
    * @param periodSeconds The period at which to run the callback in seconds.
+   * @return a {@link Cancellable} that allows the user to cancel periodic invocation.
    */
-  public final void addPeriodic(Runnable callback, double periodSeconds) {
-    m_callbacks.add(new Callback(callback, m_startTimeUs, (long) (periodSeconds * 1e6), 0));
+  public final Cancellable addPeriodic(Runnable callback, double periodSeconds) {
+    Callback scheduledCallback =
+        new Callback(
+            callback,
+            m_startTimeUs,
+            (long) (periodSeconds * 1e6),
+            0,
+            CallbackScheduleType.PERIODIC);
+    m_callbacks.add(scheduledCallback);
+    return scheduledCallback;
   }
 
   /**
@@ -182,11 +241,19 @@ public final void addPeriodic(Runnable callback, double periodSeconds) {
    * @param periodSeconds The period at which to run the callback in seconds.
    * @param offsetSeconds The offset from the common starting time in seconds. This is useful for
    *     scheduling a callback in a different timeslot relative to TimedRobot.
+   * @return a {@link Cancellable} that allows the user to cancel periodic invocation.
    */
-  public final void addPeriodic(Runnable callback, double periodSeconds, double offsetSeconds) {
-    m_callbacks.add(
+  public final Cancellable addPeriodic(
+      Runnable callback, double periodSeconds, double offsetSeconds) {
+    Callback scheduledCallback =
         new Callback(
-            callback, m_startTimeUs, (long) (periodSeconds * 1e6), (long) (offsetSeconds * 1e6)));
+            callback,
+            m_startTimeUs,
+            (long) (periodSeconds * 1e6),
+            (long) (offsetSeconds * 1e6),
+            CallbackScheduleType.PERIODIC);
+    m_callbacks.add(scheduledCallback);
+    return scheduledCallback;
   }
 
   /**
@@ -197,9 +264,10 @@ public final void addPeriodic(Runnable callback, double periodSeconds, double of
    *
    * @param callback The callback to run.
    * @param period The period at which to run the callback.
+   * @return a {@link Cancellable} that allows the user to cancel periodic invocation.
    */
-  public final void addPeriodic(Runnable callback, Time period) {
-    addPeriodic(callback, period.in(Seconds));
+  public final Cancellable addPeriodic(Runnable callback, Time period) {
+    return addPeriodic(callback, period.in(Seconds));
   }
 
   /**
@@ -212,8 +280,24 @@ public final void addPeriodic(Runnable callback, Time period) {
    * @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.
+   * @return a {@link Cancellable} that allows the user to cancel periodic invocation.
    */
-  public final void addPeriodic(Runnable callback, Time period, Time offset) {
-    addPeriodic(callback, period.in(Seconds), offset.in(Seconds));
+  public final Cancellable addPeriodic(Runnable callback, Time period, Time offset) {
+    return addPeriodic(callback, period.in(Seconds), offset.in(Seconds));
+  }
+
+  /**
+   * Add a one-shot that, unless cancelled, invokes an action exactly once after a specified delay.
+   *
+   * @param callback action to run
+   * @param delaySeconds the number of seconds to wait before running the action
+   * @return a {@link Cancellable} that allows the user to cancel the invocation
+   */
+  public final Cancellable addOneShot(Runnable callback, double delaySeconds) {
+    Callback scheduledCallback =
+        new Callback(
+            callback, m_startTimeUs, (long) (delaySeconds * 1e6), 0, CallbackScheduleType.ONE_SHOT);
+    m_callbacks.add(scheduledCallback);
+    return scheduledCallback;
   }
 }