Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-25 01:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

Move robot base classes from opmode to framework (#8344)

Browse Source 
Having these in opmode will be confusing to users when opmodes are added.
This commit is contained in:
Peter Johnson
2025-11-08 15:08:38 -08:00
committed by GitHub
parent aeedfa588c
commit 18efd1e534
275 changed files with 285 additions and 285 deletions
Download Patch File Download Diff File Expand all files Collapse all files

380
wpilibj/src/main/java/org/wpilib/opmode/IterativeRobotBase.java
View File

@@ -1,380 +0,0 @@
// 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 org.wpilib.opmode;
import org.wpilib.driverstation.DSControlWord;
import org.wpilib.driverstation.DriverStation;
import org.wpilib.hardware.hal.DriverStationJNI;
import org.wpilib.hardware.hal.HAL;
import org.wpilib.networktables.NetworkTableInstance;
import org.wpilib.smartdashboard.SmartDashboard;
import org.wpilib.system.Watchdog;
/**
* IterativeRobotBase implements a specific type of robot program framework, extending the RobotBase
* class.
*
* <p>The IterativeRobotBase class does not implement startCompetition(), so it should not be used
* by teams directly.
*
* <p>This class provides the following functions which are called by the main loop,
* startCompetition(), at the appropriate times:
*
* <p>driverStationConnected() -- provide for initialization the first time the DS is connected
*
* <p>init() functions -- each of the following functions is called once when the appropriate mode
* is entered:
*
* <ul>
* <li>disabledInit() -- called each and every time disabled is entered from another mode
* <li>autonomousInit() -- called each and every time autonomous is entered from another mode
* <li>teleopInit() -- called each and every time teleop is entered from another mode
* <li>testInit() -- called each and every time test is entered from another mode
* </ul>
*
* <p>periodic() functions -- each of these functions is called on an interval:
*
* <ul>
* <li>robotPeriodic()
* <li>disabledPeriodic()
* <li>autonomousPeriodic()
* <li>teleopPeriodic()
* <li>testPeriodic()
* </ul>
*
* <p>exit() functions -- each of the following functions is called once when the appropriate mode
* is exited:
*
* <ul>
* <li>disabledExit() -- called each and every time disabled is exited
* <li>autonomousExit() -- called each and every time autonomous is exited
* <li>teleopExit() -- called each and every time teleop is exited
* <li>testExit() -- called each and every time test is exited
* </ul>
*/
public abstract class IterativeRobotBase extends RobotBase {
private enum Mode {
kNone,
kDisabled,
kAutonomous,
kTeleop,
kTest
}
private final DSControlWord m_word = new DSControlWord();
private Mode m_lastMode = Mode.kNone;
private final double m_period;
private final Watchdog m_watchdog;
private boolean m_ntFlushEnabled = true;
private boolean m_calledDsConnected;
/**
* Constructor for IterativeRobotBase.
*
* @param period Period in seconds.
*/
protected IterativeRobotBase(double period) {
m_period = period;
m_watchdog = new Watchdog(period, this::printLoopOverrunMessage);
}
/** Provide an alternate "main loop" via startCompetition(). */
@Override
public abstract void startCompetition();
/* ----------- Overridable initialization code ----------------- */
/**
* Code that needs to know the DS state should go here.
*
* <p>Users should override this method for initialization that needs to occur after the DS is
* connected, such as needing the alliance information.
*/
public void driverStationConnected() {}
/**
* Robot-wide simulation initialization code should go here.
*
* <p>Users should override this method for default Robot-wide simulation related initialization
* which will be called when the robot is first started. It will be called exactly one time after
* the robot class constructor is called only when the robot is in simulation.
*/
public void simulationInit() {}
/**
* Initialization code for disabled mode should go here.
*
* <p>Users should override this method for initialization code which will be called each time the
* robot enters disabled mode.
*/
public void disabledInit() {}
/**
* Initialization code for autonomous mode should go here.
*
* <p>Users should override this method for initialization code which will be called each time the
* robot enters autonomous mode.
*/
public void autonomousInit() {}
/**
* Initialization code for teleop mode should go here.
*
* <p>Users should override this method for initialization code which will be called each time the
* robot enters teleop mode.
*/
public void teleopInit() {}
/**
* Initialization code for test mode should go here.
*
* <p>Users should override this method for initialization code which will be called each time the
* robot enters test mode.
*/
public void testInit() {}
/* ----------- Overridable periodic code ----------------- */
private boolean m_rpFirstRun = true;
/** Periodic code for all robot modes should go here. */
public void robotPeriodic() {
if (m_rpFirstRun) {
System.out.println("Default robotPeriodic() method... Override me!");
m_rpFirstRun = false;
}
}
private boolean m_spFirstRun = true;
/**
* Periodic simulation code should go here.
*
* <p>This function is called in a simulated robot after user code executes.
*/
public void simulationPeriodic() {
if (m_spFirstRun) {
System.out.println("Default simulationPeriodic() method... Override me!");
m_spFirstRun = false;
}
}
private boolean m_dpFirstRun = true;
/** Periodic code for disabled mode should go here. */
public void disabledPeriodic() {
if (m_dpFirstRun) {
System.out.println("Default disabledPeriodic() method... Override me!");
m_dpFirstRun = false;
}
}
private boolean m_apFirstRun = true;
/** Periodic code for autonomous mode should go here. */
public void autonomousPeriodic() {
if (m_apFirstRun) {
System.out.println("Default autonomousPeriodic() method... Override me!");
m_apFirstRun = false;
}
}
private boolean m_tpFirstRun = true;
/** Periodic code for teleop mode should go here. */
public void teleopPeriodic() {
if (m_tpFirstRun) {
System.out.println("Default teleopPeriodic() method... Override me!");
m_tpFirstRun = false;
}
}
private boolean m_tmpFirstRun = true;
/** Periodic code for test mode should go here. */
public void testPeriodic() {
if (m_tmpFirstRun) {
System.out.println("Default testPeriodic() method... Override me!");
m_tmpFirstRun = false;
}
}
/**
* Exit code for disabled mode should go here.
*
* <p>Users should override this method for code which will be called each time the robot exits
* disabled mode.
*/
public void disabledExit() {}
/**
* Exit code for autonomous mode should go here.
*
* <p>Users should override this method for code which will be called each time the robot exits
* autonomous mode.
*/
public void autonomousExit() {}
/**
* Exit code for teleop mode should go here.
*
* <p>Users should override this method for code which will be called each time the robot exits
* teleop mode.
*/
public void teleopExit() {}
/**
* Exit code for test mode should go here.
*
* <p>Users should override this method for code which will be called each time the robot exits
* test mode.
*/
public void testExit() {}
/**
* Enables or disables flushing NetworkTables every loop iteration. By default, this is enabled.
*
* @param enabled True to enable, false to disable
* @deprecated Deprecated without replacement.
*/
@Deprecated(forRemoval = true, since = "2025")
public void setNetworkTablesFlushEnabled(boolean enabled) {
m_ntFlushEnabled = enabled;
}
/**
* Gets time period between calls to Periodic() functions.
*
* @return The time period between calls to Periodic() functions.
*/
public double getPeriod() {
return m_period;
}
/** Loop function. */
protected void loopFunc() {
DriverStation.refreshData();
m_watchdog.reset();
m_word.refresh();
// Get current mode
Mode mode = Mode.kNone;
if (m_word.isDisabled()) {
mode = Mode.kDisabled;
} else if (m_word.isAutonomous()) {
mode = Mode.kAutonomous;
} else if (m_word.isTeleop()) {
mode = Mode.kTeleop;
} else if (m_word.isTest()) {
mode = Mode.kTest;
}
if (!m_calledDsConnected && m_word.isDSAttached()) {
m_calledDsConnected = true;
driverStationConnected();
}
// If mode changed, call mode exit and entry functions
if (m_lastMode != mode) {
// Call last mode's exit function
switch (m_lastMode) {
case kDisabled -> disabledExit();
case kAutonomous -> autonomousExit();
case kTeleop -> teleopExit();
case kTest -> testExit();
default -> {
// NOP
}
}
// Call current mode's entry function
switch (mode) {
case kDisabled -> {
disabledInit();
m_watchdog.addEpoch("disabledInit()");
}
case kAutonomous -> {
autonomousInit();
m_watchdog.addEpoch("autonomousInit()");
}
case kTeleop -> {
teleopInit();
m_watchdog.addEpoch("teleopInit()");
}
case kTest -> {
testInit();
m_watchdog.addEpoch("testInit()");
}
default -> {
// NOP
}
}
m_lastMode = mode;
}
// Call the appropriate function depending upon the current robot mode
switch (mode) {
case kDisabled -> {
DriverStationJNI.observeUserProgramDisabled();
disabledPeriodic();
m_watchdog.addEpoch("disabledPeriodic()");
}
case kAutonomous -> {
DriverStationJNI.observeUserProgramAutonomous();
autonomousPeriodic();
m_watchdog.addEpoch("autonomousPeriodic()");
}
case kTeleop -> {
DriverStationJNI.observeUserProgramTeleop();
teleopPeriodic();
m_watchdog.addEpoch("teleopPeriodic()");
}
case kTest -> {
DriverStationJNI.observeUserProgramTest();
testPeriodic();
m_watchdog.addEpoch("testPeriodic()");
}
default -> {
// NOP
}
}
robotPeriodic();
m_watchdog.addEpoch("robotPeriodic()");
SmartDashboard.updateValues();
m_watchdog.addEpoch("SmartDashboard.updateValues()");
if (isSimulation()) {
HAL.simPeriodicBefore();
simulationPeriodic();
HAL.simPeriodicAfter();
m_watchdog.addEpoch("simulationPeriodic()");
}
m_watchdog.disable();
// Flush NetworkTables
if (m_ntFlushEnabled) {
NetworkTableInstance.getDefault().flushLocal();
}
// Warn on loop time overruns
if (m_watchdog.isExpired()) {
m_watchdog.printEpochs();
}
}
/** Prints list of epochs added so far and their times. */
public void printWatchdogEpochs() {
m_watchdog.printEpochs();
}
private void printLoopOverrunMessage() {
DriverStation.reportWarning("Loop time of " + m_period + "s overrun\n", false);
}
}

404
wpilibj/src/main/java/org/wpilib/opmode/RobotBase.java
View File

@@ -1,404 +0,0 @@
// 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 org.wpilib.opmode;
import java.util.concurrent.locks.ReentrantLock;
import java.util.function.Supplier;
import org.wpilib.driverstation.DriverStation;
import org.wpilib.hardware.hal.HAL;
import org.wpilib.hardware.hal.HALUtil;
import org.wpilib.math.util.MathShared;
import org.wpilib.math.util.MathSharedStore;
import org.wpilib.networktables.MultiSubscriber;
import org.wpilib.networktables.NetworkTableEvent;
import org.wpilib.networktables.NetworkTableInstance;
import org.wpilib.system.Notifier;
import org.wpilib.system.RuntimeType;
import org.wpilib.system.Timer;
import org.wpilib.system.WPILibVersion;
import org.wpilib.util.WPIUtilJNI;
import org.wpilib.vision.stream.CameraServerShared;
import org.wpilib.vision.stream.CameraServerSharedStore;
/**
* Implement a Robot Program framework. The RobotBase class is intended to be subclassed to create a
* robot program. The user must implement {@link #startCompetition()}, which will be called once and
* is not expected to exit. The user must also implement {@link #endCompetition()}, which signals to
* the code in {@link #startCompetition()} that it should exit.
*
* <p>It is not recommended to subclass this class directly - instead subclass IterativeRobotBase or
* TimedRobot.
*/
public abstract class RobotBase implements AutoCloseable {
/** The ID of the main Java thread. */
// This is usually 1, but it is best to make sure
private static long m_threadId = -1;
private final MultiSubscriber m_suball;
private final int m_connListenerHandle;
private static void setupCameraServerShared() {
CameraServerShared shared =
new CameraServerShared() {
@Override
public void reportUsage(String resource, String data) {
HAL.reportUsage(resource, data);
}
@Override
public void reportDriverStationError(String error) {
DriverStation.reportError(error, true);
}
@Override
public Long getRobotMainThreadId() {
return RobotBase.getMainThreadId();
}
@Override
public boolean isRoboRIO() {
return !RobotBase.isSimulation();
}
};
CameraServerSharedStore.setCameraServerShared(shared);
}
private static void setupMathShared() {
MathSharedStore.setMathShared(
new MathShared() {
@Override
public void reportError(String error, StackTraceElement[] stackTrace) {
DriverStation.reportError(error, stackTrace);
}
@Override
public void reportUsage(String resource, String data) {
HAL.reportUsage(resource, data);
}
@Override
public double getTimestamp() {
return Timer.getTimestamp();
}
});
}
/**
* Constructor for a generic robot program. User code can be placed in the constructor that runs
* before the Autonomous or Operator Control period starts. The constructor will run to completion
* before Autonomous is entered.
*
* <p>This must be used to ensure that the communications code starts. In the future it would be
* nice to put this code into its own task that loads on boot so ensure that it runs.
*/
protected RobotBase() {
final NetworkTableInstance inst = NetworkTableInstance.getDefault();
m_threadId = Thread.currentThread().threadId();
setupCameraServerShared();
setupMathShared();
// subscribe to "" to force persistent values to propagate to local
m_suball = new MultiSubscriber(inst, new String[] {""});
if (!isSimulation()) {
inst.startServer("/home/systemcore/networktables.json");
} else {
inst.startServer();
}
// wait for the NT server to actually start
try {
int count = 0;
while (inst.getNetworkMode().contains(NetworkTableInstance.NetworkMode.kStarting)) {
Thread.sleep(10);
count++;
if (count > 100) {
throw new InterruptedException();
}
}
} catch (InterruptedException ex) {
System.err.println("timed out while waiting for NT server to start");
}
m_connListenerHandle =
inst.addConnectionListener(
false,
event -> {
if (event.is(NetworkTableEvent.Kind.kConnected)) {
HAL.reportUsage("NT/" + event.connInfo.remote_id, "");
}
});
}
/**
* Returns the main thread ID.
*
* @return The main thread ID.
*/
public static long getMainThreadId() {
return m_threadId;
}
@Override
public void close() {
m_suball.close();
NetworkTableInstance.getDefault().removeListener(m_connListenerHandle);
}
/**
* Get the current runtime type.
*
* @return Current runtime type.
*/
public static RuntimeType getRuntimeType() {
return RuntimeType.getValue(HALUtil.getHALRuntimeType());
}
/**
* Get if the robot is a simulation.
*
* @return If the robot is running in simulation.
*/
public static boolean isSimulation() {
return getRuntimeType() == RuntimeType.kSimulation;
}
/**
* Get if the robot is real.
*
* @return If the robot is running in the real world.
*/
public static boolean isReal() {
RuntimeType runtimeType = getRuntimeType();
return runtimeType == RuntimeType.kSystemCore;
}
/**
* Determine if the Robot is currently disabled.
*
* @return True if the Robot is currently disabled by the Driver Station.
*/
public boolean isDisabled() {
return DriverStation.isDisabled();
}
/**
* Determine if the Robot is currently enabled.
*
* @return True if the Robot is currently enabled by the Driver Station.
*/
public boolean isEnabled() {
return DriverStation.isEnabled();
}
/**
* Determine if the robot is currently in Autonomous mode as determined by the Driver Station.
*
* @return True if the robot is currently operating Autonomously.
*/
public boolean isAutonomous() {
return DriverStation.isAutonomous();
}
/**
* Determine if the robot is currently in Autonomous mode and enabled as determined by the Driver
* Station.
*
* @return True if the robot is currently operating autonomously while enabled.
*/
public boolean isAutonomousEnabled() {
return DriverStation.isAutonomousEnabled();
}
/**
* Determine if the robot is currently in Test mode as determined by the Driver Station.
*
* @return True if the robot is currently operating in Test mode.
*/
public boolean isTest() {
return DriverStation.isTest();
}
/**
* Determine if the robot is current in Test mode and enabled as determined by the Driver Station.
*
* @return True if the robot is currently operating in Test mode while enabled.
*/
public boolean isTestEnabled() {
return DriverStation.isTestEnabled();
}
/**
* Determine if the robot is currently in Operator Control mode as determined by the Driver
* Station.
*
* @return True if the robot is currently operating in Tele-Op mode.
*/
public boolean isTeleop() {
return DriverStation.isTeleop();
}
/**
* Determine if the robot is currently in Operator Control mode and enabled as determined by the
* Driver Station.
*
* @return True if the robot is currently operating in Tele-Op mode while enabled.
*/
public boolean isTeleopEnabled() {
return DriverStation.isTeleopEnabled();
}
/**
* Start the main robot code. This function will be called once and should not exit until
* signalled by {@link #endCompetition()}
*/
public abstract void startCompetition();
/** Ends the main loop in {@link #startCompetition()}. */
public abstract void endCompetition();
private static final ReentrantLock m_runMutex = new ReentrantLock();
private static RobotBase m_robotCopy;
private static boolean m_suppressExitWarning;
/** Run the robot main loop. */
private static <T extends RobotBase> void runRobot(Supplier<T> robotSupplier) {
System.out.println("********** Robot program starting **********");
T robot;
try {
robot = robotSupplier.get();
} catch (Throwable throwable) {
Throwable cause = throwable.getCause();
if (cause != null) {
throwable = cause;
}
String robotName = "Unknown";
StackTraceElement[] elements = throwable.getStackTrace();
if (elements.length > 0) {
robotName = elements[0].getClassName();
}
DriverStation.reportError(
"Unhandled exception instantiating robot " + robotName + " " + throwable, elements);
DriverStation.reportError(
"The robot program quit unexpectedly."
+ " This is usually due to a code error.\n"
+ " The above stacktrace can help determine where the error occurred.\n"
+ " See https://wpilib.org/stacktrace for more information.\n",
false);
DriverStation.reportError("Could not instantiate robot " + robotName + "!", false);
return;
}
m_runMutex.lock();
m_robotCopy = robot;
m_runMutex.unlock();
boolean errorOnExit = false;
try {
robot.startCompetition();
} catch (Throwable throwable) {
Throwable cause = throwable.getCause();
if (cause != null) {
throwable = cause;
}
DriverStation.reportError("Unhandled exception: " + throwable, throwable.getStackTrace());
errorOnExit = true;
} finally {
m_runMutex.lock();
boolean suppressExitWarning = m_suppressExitWarning;
m_runMutex.unlock();
if (!suppressExitWarning) {
// startCompetition never returns unless exception occurs....
DriverStation.reportWarning(
"The robot program quit unexpectedly."
+ " This is usually due to a code error.\n"
+ " The above stacktrace can help determine where the error occurred.\n"
+ " See https://wpilib.org/stacktrace for more information.",
false);
if (errorOnExit) {
DriverStation.reportError(
"The startCompetition() method (or methods called by it) should have "
+ "handled the exception above.",
false);
} else {
DriverStation.reportError("Unexpected return from startCompetition() method.", false);
}
}
}
}
/**
* Suppress the "The robot program quit unexpectedly." message.
*
* @param value True if exit warning should be suppressed.
*/
public static void suppressExitWarning(boolean value) {
m_runMutex.lock();
m_suppressExitWarning = value;
m_runMutex.unlock();
}
/**
* Starting point for the applications.
*
* @param <T> Robot subclass.
* @param robotSupplier Function that returns an instance of the robot subclass.
*/
public static <T extends RobotBase> void startRobot(Supplier<T> robotSupplier) {
// Check that the MSVC runtime is valid.
WPIUtilJNI.checkMsvcRuntime();
if (!HAL.initialize(500, 0)) {
throw new IllegalStateException("Failed to initialize. Terminating");
}
// Force refresh DS data
DriverStation.refreshData();
HAL.reportUsage("Language", "Java");
HAL.reportUsage("WPILibVersion", WPILibVersion.Version);
if (!Notifier.setHALThreadPriority(true, 40)) {
DriverStation.reportWarning("Setting HAL Notifier RT priority to 40 failed", false);
}
if (HAL.hasMain()) {
Thread thread =
new Thread(
() -> {
runRobot(robotSupplier);
HAL.exitMain();
},
"robot main");
thread.setDaemon(true);
thread.start();
HAL.runMain();
suppressExitWarning(true);
m_runMutex.lock();
RobotBase robot = m_robotCopy;
m_runMutex.unlock();
if (robot != null) {
robot.endCompetition();
}
try {
thread.join(1000);
} catch (InterruptedException ex) {
Thread.currentThread().interrupt();
}
} else {
runRobot(robotSupplier);
}
// On RIO, this will just terminate rather than shutting down cleanly (it's a no-op in sim).
// It's not worth the risk of hanging on shutdown when we want the code to restart as quickly
// as possible.
HAL.terminate();
HAL.shutdown();
System.exit(0);
}
}

66
wpilibj/src/main/java/org/wpilib/opmode/RobotState.java
View File

@@ -1,66 +0,0 @@
// 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 org.wpilib.opmode;
import org.wpilib.driverstation.DriverStation;
/** Robot state utility functions. */
public final class RobotState {
/**
* Returns true if the robot is disabled.
*
* @return True if the robot is disabled.
*/
public static boolean isDisabled() {
return DriverStation.isDisabled();
}
/**
* Returns true if the robot is enabled.
*
* @return True if the robot is enabled.
*/
public static boolean isEnabled() {
return DriverStation.isEnabled();
}
/**
* Returns true if the robot is E-stopped.
*
* @return True if the robot is E-stopped.
*/
public static boolean isEStopped() {
return DriverStation.isEStopped();
}
/**
* Returns true if the robot is in teleop mode.
*
* @return True if the robot is in teleop mode.
*/
public static boolean isTeleop() {
return DriverStation.isTeleop();
}
/**
* Returns true if the robot is in autonomous mode.
*
* @return True if the robot is in autonomous mode.
*/
public static boolean isAutonomous() {
return DriverStation.isAutonomous();
}
/**
* Returns true if the robot is in test mode.
*
* @return True if the robot is in test mode.
*/
public static boolean isTest() {
return DriverStation.isTest();
}
private RobotState() {}
}

247
wpilibj/src/main/java/org/wpilib/opmode/TimedRobot.java
View File

@@ -1,247 +0,0 @@
// 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 org.wpilib.opmode;
import static org.wpilib.units.Units.Seconds;
import java.util.PriorityQueue;
import org.wpilib.hardware.hal.DriverStationJNI;
import org.wpilib.hardware.hal.HAL;
import org.wpilib.hardware.hal.NotifierJNI;
import org.wpilib.system.RobotController;
import org.wpilib.units.measure.Frequency;
import org.wpilib.units.measure.Time;
/**
* TimedRobot implements the IterativeRobotBase robot program framework.
*
* <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.
*/
public class TimedRobot extends IterativeRobotBase {
@SuppressWarnings("MemberName")
static class Callback implements Comparable<Callback> {
public Runnable func;
public long period;
public long expirationTime;
/**
* Construct a callback container.
*
* @param func The callback to run.
* @param startTime The common starting point for all callback scheduling in microseconds.
* @param period The period at which to run the callback in microseconds.
* @param offset The offset from the common starting time in microseconds.
*/
Callback(Runnable func, long startTime, long period, long offset) {
this.func = func;
this.period = period;
this.expirationTime =
startTime
+ offset
+ this.period
+ (RobotController.getFPGATime() - startTime) / this.period * this.period;
}
@Override
public boolean equals(Object rhs) {
return rhs instanceof Callback callback && expirationTime == callback.expirationTime;
}
@Override
public int hashCode() {
return Long.hashCode(expirationTime);
}
@Override
public int compareTo(Callback rhs) {
// Elements with sooner expiration times are sorted as lesser. The head of
// Java's PriorityQueue is the least element.
return Long.compare(expirationTime, rhs.expirationTime);
}
}
/** Default loop period. */
public static final double kDefaultPeriod = 0.02;
// The C pointer to the notifier object. We don't use it directly, it is
// just passed to the JNI bindings.
private final int m_notifier = NotifierJNI.initializeNotifier();
private long m_startTimeUs;
private long m_loopStartTimeUs;
private final PriorityQueue<Callback> m_callbacks = new PriorityQueue<>();
/** Constructor for TimedRobot. */
protected TimedRobot() {
this(kDefaultPeriod);
}
/**
* Constructor for TimedRobot.
*
* @param period The period of the robot loop function.
*/
protected TimedRobot(double period) {
super(period);
m_startTimeUs = RobotController.getFPGATime();
addPeriodic(this::loopFunc, period);
NotifierJNI.setNotifierName(m_notifier, "TimedRobot");
HAL.reportUsage("Framework", "TimedRobot");
}
/**
* Constructor for TimedRobot.
*
* @param period The period of the robot loop function.
*/
protected TimedRobot(Time period) {
this(period.in(Seconds));
}
/**
* Constructor for TimedRobot.
*
* @param frequency The frequency of the robot loop function.
*/
protected TimedRobot(Frequency frequency) {
this(frequency.asPeriod());
}
@Override
public void close() {
NotifierJNI.stopNotifier(m_notifier);
NotifierJNI.cleanNotifier(m_notifier);
}
/** Provide an alternate "main loop" via startCompetition(). */
@Override
public void startCompetition() {
if (isSimulation()) {
simulationInit();
}
// Tell the DS that the robot is ready to be enabled
System.out.println("********** Robot program startup complete **********");
DriverStationJNI.observeUserProgramStarting();
// 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();
NotifierJNI.updateNotifierAlarm(m_notifier, callback.expirationTime);
long currentTime = NotifierJNI.waitForNotifierAlarm(m_notifier);
if (currentTime == 0) {
break;
}
m_loopStartTimeUs = RobotController.getFPGATime();
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
while (m_callbacks.peek().expirationTime <= currentTime) {
callback = m_callbacks.poll();
callback.func.run();
callback.expirationTime +=
callback.period
+ (currentTime - callback.expirationTime) / callback.period * callback.period;
m_callbacks.add(callback);
}
}
}
/** Ends the main loop in startCompetition(). */
@Override
public void endCompetition() {
NotifierJNI.stopNotifier(m_notifier);
}
/**
* Return the system clock time in micrseconds for the start of the current periodic loop. This is
* in the same time base as Timer.getFPGATimestamp(), but is stable through a loop. It is updated
* at the beginning of every periodic callback (including the normal periodic loop).
*
* @return Robot running time in microseconds, as of the start of the current periodic function.
*/
public long getLoopStartTime() {
return m_loopStartTimeUs;
}
/**
* Add a callback to run at a specific period.
*
* <p>This is scheduled on TimedRobot's Notifier, so TimedRobot and the callback run
* synchronously. Interactions between them are thread-safe.
*
* @param callback The callback to run.
* @param period The period at which to run the callback in seconds.
*/
public final void addPeriodic(Runnable callback, double period) {
m_callbacks.add(new Callback(callback, m_startTimeUs, (long) (period * 1e6), 0));
}
/**
* Add a callback to run at a specific period with a starting time offset.
*
* <p>This is scheduled on TimedRobot's Notifier, so TimedRobot and the callback run
* synchronously. Interactions between them are thread-safe.
*
* @param callback The callback to run.
* @param period The period at which to run the callback in seconds.
* @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.
*/
public final void addPeriodic(Runnable callback, double period, double offset) {
m_callbacks.add(
new Callback(callback, m_startTimeUs, (long) (period * 1e6), (long) (offset * 1e6)));
}
/**
* Add a callback to run at a specific period.
*
* <p>This is scheduled on TimedRobot's Notifier, so TimedRobot and the callback run
* synchronously. Interactions between them are thread-safe.
*
* @param callback The callback to run.
* @param period The period at which to run the callback.
*/
public final void addPeriodic(Runnable callback, Time period) {
addPeriodic(callback, period.in(Seconds));
}
/**
* Add a callback to run at a specific period with a starting time offset.
*
* <p>This is scheduled on TimedRobot's Notifier, so TimedRobot and the callback run
* synchronously. Interactions between them are thread-safe.
*
* @param callback The callback to run.
* @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.
*/
public final void addPeriodic(Runnable callback, Time period, Time offset) {
addPeriodic(callback, period.in(Seconds), offset.in(Seconds));
}
}

117
wpilibj/src/main/java/org/wpilib/opmode/TimesliceRobot.java
View File

@@ -1,117 +0,0 @@
// 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 org.wpilib.opmode;
/**
* TimesliceRobot extends the TimedRobot robot program framework to provide timeslice scheduling of
* periodic functions.
*
* <p>The TimesliceRobot class is intended to be subclassed by a user creating a robot program.
*
* <p>This class schedules robot operations serially in a timeslice format. TimedRobot's periodic
* functions are the first in the timeslice table with 0 ms offset and 20 ms period. You can
* schedule additional controller periodic functions at a shorter period (5 ms by default). You give
* each one a timeslice duration, then they're run sequentially. The main benefit of this approach
* is consistent starting times for each controller periodic, which can make odometry and estimators
* more accurate and controller outputs change more consistently.
*
* <p>Here's an example of measured subsystem durations and their timeslice allocations:
*
* <table>
* <tr>
* <td><b>Subsystem</b></td>
* <td><b>Duration (ms)</b></td>
* <td><b>Allocation (ms)</b></td>
* </tr>
* <tr>
* <td><b>Total</b></td>
* <td>5.0</td>
* <td>5.0</td>
* </tr>
* <tr>
* <td>TimedRobot</td>
* <td>?</td>
* <td>2.0</td>
* </tr>
* <tr>
* <td>Drivetrain</td>
* <td>1.32</td>
* <td>1.5</td>
* </tr>
* <tr>
* <td>Flywheel</td>
* <td>0.6</td>
* <td>0.7</td>
* </tr>
* <tr>
* <td>Turret</td>
* <td>0.6</td>
* <td>0.8</td>
* </tr>
* <tr>
* <td><b>Free</b></td>
* <td>0.0</td>
* <td>N/A</td>
* </tr>
* </table>
*
* <p>Since TimedRobot periodic functions only run every 20ms, that leaves a 2 ms empty spot in the
* allocation table for three of the four 5 ms cycles comprising 20 ms. That's OK because the OS
* needs time to do other things.
*
* <p>If the robot periodic functions and the controller periodic functions have a lot of scheduling
* jitter that cause them to occasionally overlap with later timeslices, consider giving the main
* robot thread a real-time priority using {@link
* org.wpilib.system.Threads#setCurrentThreadPriority(boolean,int)}. An RT priority of 15 is a
* reasonable choice.
*
* <p>If you do enable RT though, <i>make sure your periodic functions do not block</i>. If they do,
* the operating system will lock up, and you'll have to boot the roboRIO into safe mode and delete
* the robot program to recover.
*/
public class TimesliceRobot extends TimedRobot {
/**
* Constructor for TimesliceRobot.
*
* @param robotPeriodicAllocation The allocation in seconds to give the TimesliceRobot periodic
* functions.
* @param controllerPeriod The controller period in seconds. The sum of all scheduler allocations
* should be less than or equal to this value.
*/
public TimesliceRobot(double robotPeriodicAllocation, double controllerPeriod) {
m_nextOffset = robotPeriodicAllocation;
m_controllerPeriod = controllerPeriod;
}
/**
* Schedule a periodic function with the constructor's controller period and the given allocation.
* The function's runtime allocation will be placed after the end of the previous one's.
*
* <p>If a call to this function makes the allocations exceed the controller period, an exception
* will be thrown since that means the TimesliceRobot periodic functions and the given function
* will have conflicting timeslices.
*
* @param func Function to schedule.
* @param allocation The function's runtime allocation in seconds out of the controller period.
*/
public void schedule(Runnable func, double allocation) {
if (m_nextOffset + allocation > m_controllerPeriod) {
throw new IllegalStateException(
"Function scheduled at offset "
+ m_nextOffset
+ " with allocation "
+ allocation
+ " exceeded controller period of "
+ m_controllerPeriod
+ "\n");
}
addPeriodic(func, m_controllerPeriod, m_nextOffset);
m_nextOffset += allocation;
}
private double m_nextOffset;
private final double m_controllerPeriod;
}