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