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 org.wpilib.framework;
006
007import static org.wpilib.units.Units.Seconds;
008
009import org.wpilib.driverstation.internal.DriverStationBackend;
010import org.wpilib.hardware.hal.HAL;
011import org.wpilib.hardware.hal.NotifierJNI;
012import org.wpilib.internal.PeriodicPriorityQueue;
013import org.wpilib.system.RobotController;
014import org.wpilib.units.measure.Frequency;
015import org.wpilib.units.measure.Time;
016
017/**
018 * TimedRobot implements the IterativeRobotBase robot program framework.
019 *
020 * <p>The TimedRobot class is intended to be subclassed by a user creating a robot program.
021 *
022 * <p>periodic() functions from the base class are called on an interval by a Notifier instance.
023 */
024public class TimedRobot extends IterativeRobotBase {
025  /** Default loop period. */
026  public static final double DEFAULT_PERIOD = 0.02;
027
028  // The C pointer to the notifier object. We don't use it directly, it is
029  // just passed to the JNI bindings.
030  private final int m_notifier = NotifierJNI.createNotifier();
031
032  private final long m_startTimeUs;
033
034  private final PeriodicPriorityQueue m_callbackQueue = new PeriodicPriorityQueue();
035
036  /** Constructor for TimedRobot. */
037  protected TimedRobot() {
038    this(DEFAULT_PERIOD);
039  }
040
041  /**
042   * Constructor for TimedRobot.
043   *
044   * @param period The period of the robot loop function.
045   */
046  @SuppressWarnings("this-escape")
047  protected TimedRobot(double period) {
048    super(period);
049    m_startTimeUs = RobotController.getMonotonicTime();
050    addPeriodic(this::loopFunc, period);
051    NotifierJNI.setNotifierName(m_notifier, "TimedRobot");
052
053    HAL.reportUsage("Framework", "TimedRobot");
054  }
055
056  /**
057   * Constructor for TimedRobot.
058   *
059   * @param period The period of the robot loop function.
060   */
061  protected TimedRobot(Time period) {
062    this(period.in(Seconds));
063  }
064
065  /**
066   * Constructor for TimedRobot.
067   *
068   * @param frequency The frequency of the robot loop function.
069   */
070  protected TimedRobot(Frequency frequency) {
071    this(frequency.asPeriod());
072  }
073
074  @Override
075  public void close() {
076    NotifierJNI.destroyNotifier(m_notifier);
077  }
078
079  /** Provide an alternate "main loop" via startCompetition(). */
080  @Override
081  public void startCompetition() {
082    if (isSimulation()) {
083      simulationInit();
084    }
085
086    // Tell the DS that the robot is ready to be enabled
087    System.out.println("********** Robot program startup complete **********");
088    DriverStationBackend.observeUserProgramStarting();
089
090    // Loop forever, calling the appropriate mode-dependent function
091    while (true) {
092      if (!m_callbackQueue.runCallbacks(m_notifier)) {
093        break;
094      }
095    }
096  }
097
098  /** Ends the main loop in startCompetition(). */
099  @Override
100  public void endCompetition() {
101    NotifierJNI.destroyNotifier(m_notifier);
102  }
103
104  /**
105   * Return the system clock time in microseconds for the start of the current periodic loop. This
106   * is in the same time base as Timer.getMonotonicTimestamp(), but is stable through a loop. It is
107   * updated at the beginning of every periodic callback (including the normal periodic loop).
108   *
109   * @return Robot running time in microseconds, as of the start of the current periodic function.
110   */
111  public long getLoopStartTime() {
112    return m_callbackQueue.getLoopStartTime();
113  }
114
115  /**
116   * Add a callback to run at a specific period.
117   *
118   * <p>This is scheduled on TimedRobot's Notifier, so TimedRobot and the callback run
119   * synchronously. Interactions between them are thread-safe.
120   *
121   * @param callback The callback to run.
122   * @param period The period at which to run the callback in seconds.
123   */
124  public final void addPeriodic(Runnable callback, double period) {
125    addPeriodic(callback, period, period);
126  }
127
128  /**
129   * Add a callback to run at a specific period with a starting time offset.
130   *
131   * <p>This is scheduled on TimedRobot's Notifier, so TimedRobot and the callback run
132   * synchronously. Interactions between them are thread-safe.
133   *
134   * @param callback The callback to run.
135   * @param period The period at which to run the callback in seconds.
136   * @param offset The offset from the common starting time in seconds. This is useful for
137   *     scheduling a callback in a different timeslot relative to TimedRobot.
138   */
139  public final void addPeriodic(Runnable callback, double period, double offset) {
140    m_callbackQueue.add(callback, m_startTimeUs, period, offset);
141  }
142}