001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.wpilibj;
006
007import static edu.wpi.first.units.Units.Seconds;
008
009import edu.wpi.first.hal.DriverStationJNI;
010import edu.wpi.first.hal.HAL;
011import edu.wpi.first.hal.NotifierJNI;
012import edu.wpi.first.units.measure.Time;
013import java.util.PriorityQueue;
014
015/**
016 * TimedRobot implements the IterativeRobotBase robot program framework.
017 *
018 * <p>The TimedRobot class is intended to be subclassed by a user creating a robot program.
019 *
020 * <p>periodic() functions from the base class are called on an interval by a Notifier instance.
021 */
022public class TimedRobot extends IterativeRobotBase {
023  @SuppressWarnings("MemberName")
024  static class Callback implements Comparable<Callback> {
025    public Runnable func;
026    public long period;
027    public long expirationTime;
028
029    /**
030     * Construct a callback container.
031     *
032     * @param func The callback to run.
033     * @param startTime The common starting point for all callback scheduling in microseconds.
034     * @param period The period at which to run the callback in microseconds.
035     * @param offset The offset from the common starting time in microseconds.
036     */
037    Callback(Runnable func, long startTime, long period, long offset) {
038      this.func = func;
039      this.period = period;
040      this.expirationTime =
041          startTime
042              + offset
043              + this.period
044              + (RobotController.getFPGATime() - startTime) / this.period * this.period;
045    }
046
047    @Override
048    public boolean equals(Object rhs) {
049      return rhs instanceof Callback callback && expirationTime == callback.expirationTime;
050    }
051
052    @Override
053    public int hashCode() {
054      return Long.hashCode(expirationTime);
055    }
056
057    @Override
058    public int compareTo(Callback rhs) {
059      // Elements with sooner expiration times are sorted as lesser. The head of
060      // Java's PriorityQueue is the least element.
061      return Long.compare(expirationTime, rhs.expirationTime);
062    }
063  }
064
065  /** Default loop period. */
066  public static final double kDefaultPeriod = 0.02;
067
068  // The C pointer to the notifier object. We don't use it directly, it is
069  // just passed to the JNI bindings.
070  private final int m_notifier = NotifierJNI.initializeNotifier();
071
072  private long m_startTimeUs;
073  private long m_loopStartTimeUs;
074
075  private final PriorityQueue<Callback> m_callbacks = new PriorityQueue<>();
076
077  /** Constructor for TimedRobot. */
078  protected TimedRobot() {
079    this(kDefaultPeriod);
080  }
081
082  /**
083   * Constructor for TimedRobot.
084   *
085   * @param period Period in seconds.
086   */
087  protected TimedRobot(double period) {
088    super(period);
089    m_startTimeUs = RobotController.getFPGATime();
090    addPeriodic(this::loopFunc, period);
091    NotifierJNI.setNotifierName(m_notifier, "TimedRobot");
092
093    HAL.reportUsage("Framework", "TimedRobot");
094  }
095
096  @Override
097  public void close() {
098    NotifierJNI.stopNotifier(m_notifier);
099    NotifierJNI.cleanNotifier(m_notifier);
100  }
101
102  /** Provide an alternate "main loop" via startCompetition(). */
103  @Override
104  public void startCompetition() {
105    robotInit();
106
107    if (isSimulation()) {
108      simulationInit();
109    }
110
111    // Tell the DS that the robot is ready to be enabled
112    System.out.println("********** Robot program startup complete **********");
113    DriverStationJNI.observeUserProgramStarting();
114
115    // Loop forever, calling the appropriate mode-dependent function
116    while (true) {
117      // We don't have to check there's an element in the queue first because
118      // there's always at least one (the constructor adds one). It's reenqueued
119      // at the end of the loop.
120      var callback = m_callbacks.poll();
121
122      NotifierJNI.updateNotifierAlarm(m_notifier, callback.expirationTime);
123
124      long currentTime = NotifierJNI.waitForNotifierAlarm(m_notifier);
125      if (currentTime == 0) {
126        break;
127      }
128
129      m_loopStartTimeUs = RobotController.getFPGATime();
130
131      callback.func.run();
132
133      // Increment the expiration time by the number of full periods it's behind
134      // plus one to avoid rapid repeat fires from a large loop overrun. We
135      // assume currentTime ≥ expirationTime rather than checking for it since
136      // the callback wouldn't be running otherwise.
137      callback.expirationTime +=
138          callback.period
139              + (currentTime - callback.expirationTime) / callback.period * callback.period;
140      m_callbacks.add(callback);
141
142      // Process all other callbacks that are ready to run
143      while (m_callbacks.peek().expirationTime <= currentTime) {
144        callback = m_callbacks.poll();
145
146        callback.func.run();
147
148        callback.expirationTime +=
149            callback.period
150                + (currentTime - callback.expirationTime) / callback.period * callback.period;
151        m_callbacks.add(callback);
152      }
153    }
154  }
155
156  /** Ends the main loop in startCompetition(). */
157  @Override
158  public void endCompetition() {
159    NotifierJNI.stopNotifier(m_notifier);
160  }
161
162  /**
163   * Return the system clock time in micrseconds for the start of the current periodic loop. This is
164   * in the same time base as Timer.getFPGATimestamp(), but is stable through a loop. It is updated
165   * at the beginning of every periodic callback (including the normal periodic loop).
166   *
167   * @return Robot running time in microseconds, as of the start of the current periodic function.
168   */
169  public long getLoopStartTime() {
170    return m_loopStartTimeUs;
171  }
172
173  /**
174   * Add a callback to run at a specific period.
175   *
176   * <p>This is scheduled on TimedRobot's Notifier, so TimedRobot and the callback run
177   * synchronously. Interactions between them are thread-safe.
178   *
179   * @param callback The callback to run.
180   * @param period The period at which to run the callback in seconds.
181   */
182  public final void addPeriodic(Runnable callback, double period) {
183    m_callbacks.add(new Callback(callback, m_startTimeUs, (long) (period * 1e6), 0));
184  }
185
186  /**
187   * Add a callback to run at a specific period with a starting time offset.
188   *
189   * <p>This is scheduled on TimedRobot's Notifier, so TimedRobot and the callback run
190   * synchronously. Interactions between them are thread-safe.
191   *
192   * @param callback The callback to run.
193   * @param period The period at which to run the callback in seconds.
194   * @param offset The offset from the common starting time in seconds. This is useful for
195   *     scheduling a callback in a different timeslot relative to TimedRobot.
196   */
197  public final void addPeriodic(Runnable callback, double period, double offset) {
198    m_callbacks.add(
199        new Callback(callback, m_startTimeUs, (long) (period * 1e6), (long) (offset * 1e6)));
200  }
201
202  /**
203   * Add a callback to run at a specific period.
204   *
205   * <p>This is scheduled on TimedRobot's Notifier, so TimedRobot and the callback run
206   * synchronously. Interactions between them are thread-safe.
207   *
208   * @param callback The callback to run.
209   * @param period The period at which to run the callback.
210   */
211  public final void addPeriodic(Runnable callback, Time period) {
212    addPeriodic(callback, period.in(Seconds));
213  }
214
215  /**
216   * Add a callback to run at a specific period with a starting time offset.
217   *
218   * <p>This is scheduled on TimedRobot's Notifier, so TimedRobot and the callback run
219   * synchronously. Interactions between them are thread-safe.
220   *
221   * @param callback The callback to run.
222   * @param period The period at which to run the callback.
223   * @param offset The offset from the common starting time. This is useful for scheduling a
224   *     callback in a different timeslot relative to TimedRobot.
225   */
226  public final void addPeriodic(Runnable callback, Time period, Time offset) {
227    addPeriodic(callback, period.in(Seconds), offset.in(Seconds));
228  }
229}