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}