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

[hal, wpilib] Add OpMode support (#7744)

Browse Source 
User code:
- OpModeRobot used as the robot base class
- LinearOpMode and PeriodicOpMode are provided opmode base classes
- In Java, annotations can be used to automatically register opmode classes

Additional user code functionality:
- OpMode (string) is available in addition to the overall
auto/teleop/test robot mode
- OpMode does not indicate enable (enable/disable is still separate)
- The HAL API uses integer UIDs; these are exposed at the user API level
as well for faster checks
- User code creates opmodes on startup (these have name, category,
description, etc).

DS:
- DS will present opmode selection lists for auto and teleop for
match/practice. During a match, the DS will automatically activate the
selected opmode in the corresponding match period.
- For testing, an overall mode is selected (e.g. teleop/auto/test) and a
single opmode is selected

Future work:
- Command framework support/integration
- Python annotation support
- Unit tests (needs race-free DS sim updates)
- Porting of examples

Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com>
This commit is contained in:
Peter Johnson
2025-12-12 21:25:57 -07:00
committed by GitHub
parent 2a41b80e00
commit dacded37e5
163 changed files with 7454 additions and 2175 deletions
Download Patch File Download Diff File Expand all files Collapse all files

126
wpilibj/src/main/java/org/wpilib/driverstation/DSControlWord.java
View File

@@ -1,126 +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.driverstation;
import org.wpilib.hardware.hal.ControlWord;
/** A wrapper around Driver Station control word. */
public class DSControlWord {
private final ControlWord m_controlWord = new ControlWord();
/**
* DSControlWord constructor.
*
* <p>Upon construction, the current Driver Station control word is read and stored internally.
*/
public DSControlWord() {
refresh();
}
/** Update internal Driver Station control word. */
public final void refresh() {
DriverStation.refreshControlWordFromCache(m_controlWord);
}
/**
* Gets a value indicating whether the Driver Station requires the robot to be enabled.
*
* @return True if the robot is enabled, false otherwise.
*/
public boolean isEnabled() {
return m_controlWord.getEnabled() && m_controlWord.getDSAttached();
}
/**
* Gets a value indicating whether the Driver Station requires the robot to be disabled.
*
* @return True if the robot should be disabled, false otherwise.
*/
public boolean isDisabled() {
return !isEnabled();
}
/**
* Gets a value indicating whether the Robot is e-stopped.
*
* @return True if the robot is e-stopped, false otherwise.
*/
public boolean isEStopped() {
return m_controlWord.getEStop();
}
/**
* Gets a value indicating whether the Driver Station requires the robot to be running in
* autonomous mode.
*
* @return True if autonomous mode should be enabled, false otherwise.
*/
public boolean isAutonomous() {
return m_controlWord.getAutonomous();
}
/**
* Gets a value indicating whether the Driver Station requires the robot to be running in
* autonomous mode and enabled.
*
* @return True if autonomous should be set and the robot should be enabled.
*/
public boolean isAutonomousEnabled() {
return m_controlWord.getAutonomous()
&& m_controlWord.getEnabled()
&& m_controlWord.getDSAttached();
}
/**
* Gets a value indicating whether the Driver Station requires the robot to be running in
* operator-controlled mode.
*
* @return True if operator-controlled mode should be enabled, false otherwise.
*/
public boolean isTeleop() {
return !(isAutonomous() || isTest());
}
/**
* Gets a value indicating whether the Driver Station requires the robot to be running in
* operator-controller mode and enabled.
*
* @return True if operator-controlled mode should be set and the robot should be enabled.
*/
public boolean isTeleopEnabled() {
return !m_controlWord.getAutonomous()
&& !m_controlWord.getTest()
&& m_controlWord.getEnabled()
&& m_controlWord.getDSAttached();
}
/**
* Gets a value indicating whether the Driver Station requires the robot to be running in test
* mode.
*
* @return True if test mode should be enabled, false otherwise.
*/
public boolean isTest() {
return m_controlWord.getTest();
}
/**
* Gets a value indicating whether the Driver Station is attached.
*
* @return True if Driver Station is attached, false otherwise.
*/
public boolean isDSAttached() {
return m_controlWord.getDSAttached();
}
/**
* Gets if the driver station attached to a Field Management System.
*
* @return true if the robot is competing on a field being controlled by a Field Management System
*/
public boolean isFMSAttached() {
return m_controlWord.getFMSAttached();
}
}

377
wpilibj/src/main/java/org/wpilib/driverstation/DriverStation.java
View File

@@ -4,28 +4,34 @@
package org.wpilib.driverstation;
import java.util.HashMap;
import java.util.Map;
import java.util.Optional;
import java.util.OptionalDouble;
import java.util.OptionalInt;
import java.util.concurrent.locks.ReentrantLock;
import org.wpilib.datalog.BooleanArrayLogEntry;
import org.wpilib.datalog.BooleanLogEntry;
import org.wpilib.datalog.DataLog;
import org.wpilib.datalog.FloatArrayLogEntry;
import org.wpilib.datalog.IntegerArrayLogEntry;
import org.wpilib.datalog.StringLogEntry;
import org.wpilib.datalog.StructLogEntry;
import org.wpilib.hardware.hal.AllianceStationID;
import org.wpilib.hardware.hal.ControlWord;
import org.wpilib.hardware.hal.DriverStationJNI;
import org.wpilib.hardware.hal.HAL;
import org.wpilib.hardware.hal.MatchInfoData;
import org.wpilib.hardware.hal.OpModeOption;
import org.wpilib.hardware.hal.RobotMode;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.networktables.BooleanPublisher;
import org.wpilib.networktables.IntegerPublisher;
import org.wpilib.networktables.NetworkTableInstance;
import org.wpilib.networktables.StringPublisher;
import org.wpilib.networktables.StringTopic;
import org.wpilib.networktables.StructPublisher;
import org.wpilib.system.Timer;
import org.wpilib.util.Color;
import org.wpilib.util.WPIUtilJNI;
import org.wpilib.util.concurrent.EventVector;
@@ -242,6 +248,22 @@ public final class DriverStation {
private static final double JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL = 1.0;
private static double m_nextMessageTime;
private static String opModeToString(long id) {
if (id == 0) {
return "";
}
m_opModesMutex.lock();
try {
OpModeOption option = m_opModes.get(id);
if (option != null) {
return option.name;
}
} finally {
m_opModesMutex.unlock();
}
return "<" + id + ">";
}
@SuppressWarnings("MemberName")
private static class MatchDataSender {
private static final String kSmartDashboardType = "FMSInfo";
@@ -253,7 +275,8 @@ public final class DriverStation {
final IntegerPublisher matchType;
final BooleanPublisher alliance;
final IntegerPublisher station;
final IntegerPublisher controlWord;
final StructPublisher<ControlWord> controlWord;
final StringPublisher opMode;
boolean oldIsRedAlliance = true;
int oldStationNumber = 1;
String oldEventName = "";
@@ -261,7 +284,8 @@ public final class DriverStation {
int oldMatchNumber;
int oldReplayNumber;
int oldMatchType;
int oldControlWord;
final ControlWord oldControlWord = new ControlWord();
final ControlWord currentControlWord = new ControlWord();
MatchDataSender() {
var table = NetworkTableInstance.getDefault().getTable("FMSInfo");
@@ -284,10 +308,13 @@ public final class DriverStation {
alliance.set(true);
station = table.getIntegerTopic("StationNumber").publish();
station.set(1);
controlWord = table.getIntegerTopic("FMSControlData").publish();
controlWord.set(0);
controlWord = table.getStructTopic("ControlWord", ControlWord.struct).publish();
controlWord.set(oldControlWord);
opMode = table.getStringTopic("OpMode").publish();
opMode.set("");
}
@SuppressWarnings("VariableDeclarationUsageDistance")
private void sendMatchData() {
AllianceStationID allianceID = DriverStationJNI.getAllianceStation();
final int stationNumber =
@@ -307,7 +334,6 @@ public final class DriverStation {
int currentMatchNumber;
int currentReplayNumber;
int currentMatchType;
int currentControlWord;
m_cacheDataMutex.lock();
try {
currentEventName = DriverStation.m_matchInfo.eventName;
@@ -318,7 +344,7 @@ public final class DriverStation {
} finally {
m_cacheDataMutex.unlock();
}
currentControlWord = DriverStationJNI.nativeGetControlWord();
DriverStationJNI.getControlWord(currentControlWord);
if (oldIsRedAlliance != isRedAlliance) {
alliance.set(isRedAlliance);
@@ -348,9 +374,13 @@ public final class DriverStation {
matchType.set(currentMatchType);
oldMatchType = currentMatchType;
}
if (currentControlWord != oldControlWord) {
if (!currentControlWord.equals(oldControlWord)) {
long currentOpModeId = currentControlWord.getOpModeId();
if (currentOpModeId != oldControlWord.getOpModeId()) {
opMode.set(opModeToString(currentOpModeId));
}
controlWord.set(currentControlWord);
oldControlWord = currentControlWord;
oldControlWord.update(currentControlWord);
}
}
}
@@ -462,21 +492,16 @@ public final class DriverStation {
private static class DataLogSender {
DataLogSender(DataLog log, boolean logJoysticks, long timestamp) {
m_logEnabled = new BooleanLogEntry(log, "DS:enabled", timestamp);
m_logAutonomous = new BooleanLogEntry(log, "DS:autonomous", timestamp);
m_logTest = new BooleanLogEntry(log, "DS:test", timestamp);
m_logEstop = new BooleanLogEntry(log, "DS:estop", timestamp);
m_logControlWord =
StructLogEntry.create(log, "DS:controlWord", ControlWord.struct, timestamp);
// append initial control word values
m_wasEnabled = m_controlWordCache.getEnabled();
m_wasAutonomous = m_controlWordCache.getAutonomous();
m_wasTest = m_controlWordCache.getTest();
m_wasEstop = m_controlWordCache.getEStop();
// append initial control word value
m_logControlWord.append(m_controlWordCache, timestamp);
m_oldControlWord.update(m_controlWordCache);
m_logEnabled.append(m_wasEnabled, timestamp);
m_logAutonomous.append(m_wasAutonomous, timestamp);
m_logTest.append(m_wasTest, timestamp);
m_logEstop.append(m_wasEstop, timestamp);
// append initial opmode value
m_logOpMode = new StringLogEntry(log, "DS:opMode", timestamp);
m_logOpMode.append(m_opModeCache, timestamp);
if (logJoysticks) {
m_joysticks = new JoystickLogSender[kJoystickPorts];
@@ -490,29 +515,16 @@ public final class DriverStation {
public void send(long timestamp) {
// append control word value changes
boolean enabled = m_controlWordCache.getEnabled();
if (enabled != m_wasEnabled) {
m_logEnabled.append(enabled, timestamp);
}
m_wasEnabled = enabled;
if (!m_controlWordCache.equals(m_oldControlWord)) {
// append opmode value changes
long opModeId = m_controlWordCache.getOpModeId();
if (opModeId != m_oldControlWord.getOpModeId()) {
m_logOpMode.append(m_opModeCache, timestamp);
}
boolean autonomous = m_controlWordCache.getAutonomous();
if (autonomous != m_wasAutonomous) {
m_logAutonomous.append(autonomous, timestamp);
m_logControlWord.append(m_controlWordCache, timestamp);
m_oldControlWord.update(m_controlWordCache);
}
m_wasAutonomous = autonomous;
boolean test = m_controlWordCache.getTest();
if (test != m_wasTest) {
m_logTest.append(test, timestamp);
}
m_wasTest = test;
boolean estop = m_controlWordCache.getEStop();
if (estop != m_wasEstop) {
m_logEstop.append(estop, timestamp);
}
m_wasEstop = estop;
// append joystick value changes
for (JoystickLogSender joystick : m_joysticks) {
@@ -520,14 +532,9 @@ public final class DriverStation {
}
}
boolean m_wasEnabled;
boolean m_wasAutonomous;
boolean m_wasTest;
boolean m_wasEstop;
final BooleanLogEntry m_logEnabled;
final BooleanLogEntry m_logAutonomous;
final BooleanLogEntry m_logTest;
final BooleanLogEntry m_logEstop;
final ControlWord m_oldControlWord = new ControlWord();
final StructLogEntry<ControlWord> m_logControlWord;
final StringLogEntry m_logOpMode;
final JoystickLogSender[] m_joysticks;
}
@@ -541,6 +548,7 @@ public final class DriverStation {
new HALJoystickTouchpads[kJoystickPorts];
private static MatchInfoData m_matchInfo = new MatchInfoData();
private static ControlWord m_controlWord = new ControlWord();
private static String m_opMode = "";
private static EventVector m_refreshEvents = new EventVector();
// Joystick Cached Data
@@ -555,6 +563,8 @@ public final class DriverStation {
private static MatchInfoData m_matchInfoCache = new MatchInfoData();
private static ControlWord m_controlWordCache = new ControlWord();
private static String m_opModeCache = "";
// Joystick button rising/falling edge flags
private static long[] m_joystickButtonsPressed = new long[kJoystickPorts];
private static long[] m_joystickButtonsReleased = new long[kJoystickPorts];
@@ -566,6 +576,9 @@ public final class DriverStation {
private static boolean m_silenceJoystickWarning;
private static final Map<Long, OpModeOption> m_opModes = new HashMap<>();
private static final ReentrantLock m_opModesMutex = new ReentrantLock();
/**
* DriverStation constructor.
*
@@ -1185,7 +1198,7 @@ public final class DriverStation {
public static boolean isEnabled() {
m_cacheDataMutex.lock();
try {
return m_controlWord.getEnabled() && m_controlWord.getDSAttached();
return m_controlWord.isEnabled() && m_controlWord.isDSAttached();
} finally {
m_cacheDataMutex.unlock();
}
@@ -1208,7 +1221,23 @@ public final class DriverStation {
public static boolean isEStopped() {
m_cacheDataMutex.lock();
try {
return m_controlWord.getEStop();
return m_controlWord.isEStopped();
} finally {
m_cacheDataMutex.unlock();
}
}
/**
* Gets the current robot mode.
*
* <p>Note that this does not indicate whether the robot is enabled or disabled.
*
* @return robot mode
*/
public static RobotMode getRobotMode() {
m_cacheDataMutex.lock();
try {
return m_controlWord.getRobotMode();
} finally {
m_cacheDataMutex.unlock();
}
@@ -1223,7 +1252,7 @@ public final class DriverStation {
public static boolean isAutonomous() {
m_cacheDataMutex.lock();
try {
return m_controlWord.getAutonomous();
return m_controlWord.isAutonomous();
} finally {
m_cacheDataMutex.unlock();
}
@@ -1238,7 +1267,7 @@ public final class DriverStation {
public static boolean isAutonomousEnabled() {
m_cacheDataMutex.lock();
try {
return m_controlWord.getAutonomous() && m_controlWord.getEnabled();
return m_controlWord.isAutonomousEnabled();
} finally {
m_cacheDataMutex.unlock();
}
@@ -1251,7 +1280,7 @@ public final class DriverStation {
* @return True if operator-controlled mode should be enabled, false otherwise.
*/
public static boolean isTeleop() {
return !(isAutonomous() || isTest());
return m_controlWord.isTeleop();
}
/**
@@ -1263,9 +1292,7 @@ public final class DriverStation {
public static boolean isTeleopEnabled() {
m_cacheDataMutex.lock();
try {
return !m_controlWord.getAutonomous()
&& !m_controlWord.getTest()
&& m_controlWord.getEnabled();
return m_controlWord.isTeleopEnabled();
} finally {
m_cacheDataMutex.unlock();
}
@@ -1280,7 +1307,7 @@ public final class DriverStation {
public static boolean isTest() {
m_cacheDataMutex.lock();
try {
return m_controlWord.getTest();
return m_controlWord.isTest();
} finally {
m_cacheDataMutex.unlock();
}
@@ -1295,12 +1322,230 @@ public final class DriverStation {
public static boolean isTestEnabled() {
m_cacheDataMutex.lock();
try {
return m_controlWord.getTest() && m_controlWord.getEnabled();
return m_controlWord.isTestEnabled();
} finally {
m_cacheDataMutex.unlock();
}
}
private static int convertColorToInt(Color color) {
if (color == null) {
return -1;
}
return (((int) (color.red * 255) & 0xff) << 16)
| (((int) (color.green * 255) & 0xff) << 8)
| ((int) (color.blue * 255) & 0xff);
}
/**
* Adds an operating mode option. It's necessary to call publishOpModes() to make the added modes
* visible to the driver station.
*
* @param mode robot mode
* @param name name of the operating mode
* @param group group of the operating mode
* @param description description of the operating mode
* @param textColor text color, or null for default
* @param backgroundColor background color, or null for default
* @return unique ID used to later identify the operating mode
* @throws IllegalArgumentException if name is empty or an operating mode with the same robot mode
* and name already exists
*/
@SuppressWarnings("PMD.UseStringBufferForStringAppends")
public static long addOpMode(
RobotMode mode,
String name,
String group,
String description,
Color textColor,
Color backgroundColor) {
if (name.isBlank()) {
throw new IllegalArgumentException("OpMode name must be non-blank");
}
// find unique ID
m_opModesMutex.lock();
try {
String nameCopy = name;
for (; ; ) {
long id = OpModeOption.makeId(mode, nameCopy.hashCode());
OpModeOption existing = m_opModes.get(id);
if (existing == null) {
m_opModes.put(
id,
new OpModeOption(
id,
name,
group,
description,
convertColorToInt(textColor),
convertColorToInt(backgroundColor)));
return id;
}
if (existing.getMode() == mode && existing.name.equals(name)) {
// already exists
throw new IllegalArgumentException("OpMode " + name + " already exists for mode " + mode);
}
// collision, try again with space appended
nameCopy += ' ';
}
} finally {
m_opModesMutex.unlock();
}
}
/**
* Adds an operating mode option. It's necessary to call publishOpModes() to make the added modes
* visible to the driver station.
*
* @param mode robot mode
* @param name name of the operating mode
* @param group group of the operating mode
* @param description description of the operating mode
* @return unique ID used to later identify the operating mode
* @throws IllegalArgumentException if name is empty or an operating mode with the same name
* already exists
*/
public static long addOpMode(RobotMode mode, String name, String group, String description) {
return addOpMode(mode, name, group, description, null, null);
}
/**
* Adds an operating mode option. It's necessary to call publishOpModes() to make the added modes
* visible to the driver station.
*
* @param mode robot mode
* @param name name of the operating mode
* @param group group of the operating mode
* @return unique ID used to later identify the operating mode
* @throws IllegalArgumentException if name is empty or an operating mode with the same name
* already exists
*/
public static long addOpMode(RobotMode mode, String name, String group) {
return addOpMode(mode, name, group, "");
}
/**
* Adds an operating mode option. It's necessary to call publishOpModes() to make the added modes
* visible to the driver station.
*
* @param mode robot mode
* @param name name of the operating mode
* @return unique ID used to later identify the operating mode
* @throws IllegalArgumentException if name is empty or an operating mode with the same name
* already exists
*/
public static long addOpMode(RobotMode mode, String name) {
return addOpMode(mode, name, "");
}
/**
* Removes an operating mode option. It's necessary to call publishOpModes() to make the removed
* mode no longer visible to the driver station.
*
* @param mode robot mode
* @param name name of the operating mode
* @return unique ID for the opmode, or 0 if not found
*/
public static long removeOpMode(RobotMode mode, String name) {
if (name.isBlank()) {
return 0;
}
m_opModesMutex.lock();
try {
// we have to loop over all entries to find the one with the correct name
// because the of the unique ID generation scheme
for (OpModeOption opMode : m_opModes.values()) {
if (opMode.getMode() == mode && opMode.name.equals(name)) {
m_opModes.remove(opMode.id);
return opMode.id;
}
}
} finally {
m_opModesMutex.unlock();
}
return 0;
}
/** Publishes the operating mode options to the driver station. */
public static void publishOpModes() {
m_opModesMutex.lock();
try {
OpModeOption[] options = new OpModeOption[m_opModes.size()];
DriverStationJNI.setOpModeOptions(m_opModes.values().toArray(options));
} finally {
m_opModesMutex.unlock();
}
}
/** Clears all operating mode options and publishes an empty list to the driver station. */
public static void clearOpModes() {
m_opModesMutex.lock();
try {
m_opModes.clear();
DriverStationJNI.setOpModeOptions(new OpModeOption[0]);
} finally {
m_opModesMutex.unlock();
}
}
/**
* Gets the operating mode selected on the driver station. Note this does not mean the robot is
* enabled; use isEnabled() for that. In a match, this will indicate the operating mode selected
* for auto before the match starts (i.e., while the robot is disabled in auto mode); after the
* auto period ends, this will change to reflect the operating mode selected for teleop.
*
* @return the unique ID provided by the addOpMode() function; may return 0 or a unique ID not
* added, so callers should be prepared to handle that case
*/
public static long getOpModeId() {
m_cacheDataMutex.lock();
try {
return m_controlWord.getOpModeId();
} finally {
m_cacheDataMutex.unlock();
}
}
/**
* Gets the operating mode selected on the driver station. Note this does not mean the robot is
* enabled; use isEnabled() for that. In a match, this will indicate the operating mode selected
* for auto before the match starts (i.e., while the robot is disabled in auto mode); after the
* auto period ends, this will change to reflect the operating mode selected for teleop.
*
* @return Operating mode string; may return a string not in the list of options, so callers
* should be prepared to handle that case
*/
public static String getOpMode() {
m_cacheDataMutex.lock();
try {
return m_opMode;
} finally {
m_cacheDataMutex.unlock();
}
}
/**
* Check to see if the selected operating mode is a particular value. Note this does not mean the
* robot is enabled; use isEnabled() for that.
*
* @param id operating mode unique ID
* @return True if that mode is the current mode
*/
public static boolean isOpMode(long id) {
return getOpModeId() == id;
}
/**
* Check to see if the selected operating mode is a particular value. Note this does not mean the
* robot is enabled; use isEnabled() for that.
*
* @param mode operating mode
* @return True if that mode is the current mode
*/
public static boolean isOpMode(String mode) {
return getOpMode().equals(mode);
}
/**
* Gets a value indicating whether the Driver Station is attached.
*
@@ -1309,7 +1554,7 @@ public final class DriverStation {
public static boolean isDSAttached() {
m_cacheDataMutex.lock();
try {
return m_controlWord.getDSAttached();
return m_controlWord.isDSAttached();
} finally {
m_cacheDataMutex.unlock();
}
@@ -1323,7 +1568,7 @@ public final class DriverStation {
public static boolean isFMSAttached() {
m_cacheDataMutex.lock();
try {
return m_controlWord.getFMSAttached();
return m_controlWord.isFMSAttached();
} finally {
m_cacheDataMutex.unlock();
}
@@ -1569,6 +1814,8 @@ public final class DriverStation {
DriverStationJNI.getControlWord(m_controlWordCache);
m_opModeCache = opModeToString(m_controlWordCache.getOpModeId());
DataLogSender dataLogSender;
// lock joystick mutex to swap cache data
m_cacheDataMutex.lock();
@@ -1612,6 +1859,10 @@ public final class DriverStation {
m_controlWord = m_controlWordCache;
m_controlWordCache = currentWord;
String currentOpMode = m_opMode;
m_opMode = m_opModeCache;
m_opModeCache = currentOpMode;
dataLogSender = m_dataLogSender;
} finally {
m_cacheDataMutex.unlock();

71
wpilibj/src/main/java/org/wpilib/framework/IterativeRobotBase.java
View File

@@ -4,10 +4,11 @@
package org.wpilib.framework;
import org.wpilib.driverstation.DSControlWord;
import org.wpilib.driverstation.DriverStation;
import org.wpilib.hardware.hal.ControlWord;
import org.wpilib.hardware.hal.DriverStationJNI;
import org.wpilib.hardware.hal.HAL;
import org.wpilib.hardware.hal.RobotMode;
import org.wpilib.networktables.NetworkTableInstance;
import org.wpilib.smartdashboard.SmartDashboard;
import org.wpilib.system.Watchdog;
@@ -55,16 +56,8 @@ import org.wpilib.system.Watchdog;
* </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 ControlWord m_word = new ControlWord();
private RobotMode m_lastMode;
private final double m_period;
private final Watchdog m_watchdog;
private boolean m_ntFlushEnabled = true;
@@ -256,21 +249,12 @@ public abstract class IterativeRobotBase extends RobotBase {
/** Loop function. */
protected void loopFunc() {
DriverStation.refreshData();
DriverStation.refreshControlWordFromCache(m_word);
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;
}
// Get current mode; treat disabled as unknown
boolean enabled = m_word.isEnabled();
RobotMode mode = enabled ? m_word.getRobotMode() : RobotMode.UNKNOWN;
if (!m_calledDsConnected && m_word.isDSAttached()) {
m_calledDsConnected = true;
@@ -279,32 +263,34 @@ public abstract class IterativeRobotBase extends RobotBase {
// 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
if (m_lastMode != null) {
// Call last mode's exit function
switch (m_lastMode) {
case UNKNOWN -> disabledExit();
case AUTONOMOUS -> autonomousExit();
case TELEOPERATED -> teleopExit();
case TEST -> testExit();
default -> {
// NOP
}
}
}
// Call current mode's entry function
switch (mode) {
case kDisabled -> {
case UNKNOWN -> {
disabledInit();
m_watchdog.addEpoch("disabledInit()");
}
case kAutonomous -> {
case AUTONOMOUS -> {
autonomousInit();
m_watchdog.addEpoch("autonomousInit()");
}
case kTeleop -> {
case TELEOPERATED -> {
teleopInit();
m_watchdog.addEpoch("teleopInit()");
}
case kTest -> {
case TEST -> {
testInit();
m_watchdog.addEpoch("testInit()");
}
@@ -317,24 +303,21 @@ public abstract class IterativeRobotBase extends RobotBase {
}
// Call the appropriate function depending upon the current robot mode
DriverStationJNI.observeUserProgram(m_word.getNative());
switch (mode) {
case kDisabled -> {
DriverStationJNI.observeUserProgramDisabled();
case UNKNOWN -> {
disabledPeriodic();
m_watchdog.addEpoch("disabledPeriodic()");
}
case kAutonomous -> {
DriverStationJNI.observeUserProgramAutonomous();
case AUTONOMOUS -> {
autonomousPeriodic();
m_watchdog.addEpoch("autonomousPeriodic()");
}
case kTeleop -> {
DriverStationJNI.observeUserProgramTeleop();
case TELEOPERATED -> {
teleopPeriodic();
m_watchdog.addEpoch("teleopPeriodic()");
}
case kTest -> {
DriverStationJNI.observeUserProgramTest();
case TEST -> {
testPeriodic();
m_watchdog.addEpoch("testPeriodic()");
}

701
wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java Normal file
View File

@@ -0,0 +1,701 @@
// 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.framework;
import java.io.File;
import java.io.IOException;
import java.lang.reflect.Constructor;
import java.lang.reflect.Modifier;
import java.net.URL;
import java.util.Enumeration;
import java.util.HashMap;
import java.util.Map;
import java.util.concurrent.atomic.AtomicReference;
import java.util.function.Supplier;
import java.util.jar.JarEntry;
import java.util.jar.JarFile;
import org.wpilib.driverstation.DriverStation;
import org.wpilib.hardware.hal.ControlWord;
import org.wpilib.hardware.hal.DriverStationJNI;
import org.wpilib.hardware.hal.HAL;
import org.wpilib.hardware.hal.NotifierJNI;
import org.wpilib.hardware.hal.RobotMode;
import org.wpilib.opmode.Autonomous;
import org.wpilib.opmode.OpMode;
import org.wpilib.opmode.Teleop;
import org.wpilib.opmode.TestOpMode;
import org.wpilib.util.Color;
import org.wpilib.util.WPIUtilJNI;
/**
* OpModeRobot implements the opmode-based robot program framework.
*
* <p>The OpModeRobot class is intended to be subclassed by a user creating a robot program.
*
* <p>Classes annotated with {@link Autonomous}, {@link Teleop}, and {@link TestOpMode} in the same
* package or subpackages as the user's subclass will be automatically registered as autonomous,
* teleop, and test opmodes respectively.
*
* <p>Opmodes are constructed when selected on the driver station, and closed/no longer used when
* the robot is disabled after being enabled or a different opmode is selected. When no opmode is
* selected, nonePeriodic() is called. The driverStationConnected() function is called the first
* time the driver station connects to the robot.
*/
public abstract class OpModeRobot extends RobotBase {
private final ControlWord m_word = new ControlWord();
private record OpModeFactory(String name, Supplier<OpMode> supplier) {}
private final Map<Long, OpModeFactory> m_opModes = new HashMap<>();
private final AtomicReference<OpMode> m_activeOpMode = new AtomicReference<>(null);
private volatile int m_notifier;
private static void reportAddOpModeError(Class<?> cls, String message) {
DriverStation.reportError("Error adding OpMode " + cls.getSimpleName() + ": " + message, false);
}
/**
* Find a public constructor to instantiate the opmode. Prefer a single-arg public constructor
* whose parameter type is assignable from this.getClass() (if multiple, pick the most specific
* parameter type). Otherwise return the public no-arg constructor. Return null if neither exists.
*/
private Constructor<?> findOpModeConstructor(Class<?> cls) {
Constructor<?> bestCtor = null;
Class<?> bestParam = null;
for (Constructor<?> ctor : cls.getConstructors()) {
Class<?>[] params = ctor.getParameterTypes();
if (params.length != 1) {
continue;
}
Class<?> param = params[0];
if (!param.isAssignableFrom(getClass())) {
continue;
}
if (bestCtor == null || bestParam.isAssignableFrom(param)) {
bestCtor = ctor;
bestParam = param;
}
}
if (bestCtor != null) {
return bestCtor;
}
try {
return cls.getConstructor();
} catch (NoSuchMethodException e) {
return null;
}
}
private OpMode constructOpModeClass(Class<?> cls) {
Constructor<?> constructor = findOpModeConstructor(cls);
if (constructor == null) {
DriverStation.reportError(
"No suitable constructor to instantiate OpMode " + cls.getSimpleName(), true);
return null;
}
try {
if (constructor.getParameterCount() == 1) {
return (OpMode) constructor.newInstance(this);
} else {
return (OpMode) constructor.newInstance();
}
} catch (ReflectiveOperationException e) {
DriverStation.reportError(
"Could not instantiate OpMode " + cls.getSimpleName(), e.getStackTrace());
return null;
}
}
private void checkOpModeClass(Class<?> cls) {
// the class must be a subclass of OpMode
if (!OpMode.class.isAssignableFrom(cls)) {
throw new IllegalArgumentException("not a subclass of OpMode");
}
int modifiers = cls.getModifiers();
// it cannot be abstract
if (Modifier.isAbstract(modifiers)) {
throw new IllegalArgumentException("is abstract");
}
// it must be public
if (!Modifier.isPublic(modifiers)) {
throw new IllegalArgumentException("not public");
}
// it must not be a non-static inner class
if (cls.getEnclosingClass() != null && !Modifier.isStatic(modifiers)) {
throw new IllegalArgumentException("is a non-static inner class");
}
// it must have a public no-arg constructor or a public constructor that accepts this class
// (or a superclass/interface) as an argument
if (findOpModeConstructor(cls) == null) {
throw new IllegalArgumentException(
"missing public no-arg constructor or constructor accepting "
+ getClass().getSimpleName());
}
}
/**
* Adds an opmode using a factory function that creates the opmode. It's necessary to call
* publishOpModes() to make the added mode visible to the driver station.
*
* @param factory factory function to create the opmode
* @param mode robot mode
* @param name name of the operating mode
* @param group group of the operating mode
* @param description description of the operating mode
* @param textColor text color, or null for default
* @param backgroundColor background color, or null for default
* @throws IllegalArgumentException if class does not meet criteria
*/
public void addOpModeFactory(
Supplier<OpMode> factory,
RobotMode mode,
String name,
String group,
String description,
Color textColor,
Color backgroundColor) {
long id = DriverStation.addOpMode(mode, name, group, description, textColor, backgroundColor);
m_opModes.put(id, new OpModeFactory(name, factory));
}
/**
* Adds an opmode using a factory function that creates the opmode. It's necessary to call
* publishOpModes() to make the added mode visible to the driver station.
*
* @param factory factory function to create the opmode
* @param mode robot mode
* @param name name of the operating mode
* @param group group of the operating mode
* @param description description of the operating mode
* @throws IllegalArgumentException if class does not meet criteria
*/
public void addOpModeFactory(
Supplier<OpMode> factory, RobotMode mode, String name, String group, String description) {
addOpModeFactory(factory, mode, name, group, description, null, null);
}
/**
* Adds an opmode using a factory function that creates the opmode. It's necessary to call
* publishOpModes() to make the added mode visible to the driver station.
*
* @param factory factory function to create the opmode
* @param mode robot mode
* @param name name of the operating mode
* @param group group of the operating mode
* @throws IllegalArgumentException if class does not meet criteria
*/
public void addOpModeFactory(
Supplier<OpMode> factory, RobotMode mode, String name, String group) {
addOpModeFactory(factory, mode, name, group, "");
}
/**
* Adds an opmode using a factory function that creates the opmode. It's necessary to call
* publishOpModes() to make the added mode visible to the driver station.
*
* @param factory factory function to create the opmode
* @param mode robot mode
* @param name name of the operating mode
* @throws IllegalArgumentException if class does not meet criteria
*/
public void addOpModeFactory(Supplier<OpMode> factory, RobotMode mode, String name) {
addOpModeFactory(factory, mode, name, "");
}
/**
* Adds an opmode for an opmode class. The class must be a public, non-abstract subclass of OpMode
* with a public constructor that either takes no arguments or accepts a single argument of this
* class's type (the latter is preferred). It's necessary to call publishOpModes() to make the
* added mode visible to the driver station.
*
* @param cls class to add
* @param mode robot mode
* @param name name of the operating mode
* @param group group of the operating mode
* @param description description of the operating mode
* @param textColor text color, or null for default
* @param backgroundColor background color, or null for default
* @throws IllegalArgumentException if class does not meet criteria
*/
public void addOpMode(
Class<? extends OpMode> cls,
RobotMode mode,
String name,
String group,
String description,
Color textColor,
Color backgroundColor) {
checkOpModeClass(cls);
addOpModeFactory(
() -> constructOpModeClass(cls),
mode,
name,
group,
description,
textColor,
backgroundColor);
}
/**
* Adds an opmode for an opmode class. The class must be a public, non-abstract subclass of OpMode
* with a public constructor that either takes no arguments or accepts a single argument of this
* class's type (the latter is preferred). It's necessary to call publishOpModes() to make the
* added mode visible to the driver station.
*
* @param cls class to add
* @param mode robot mode
* @param name name of the operating mode
* @param group group of the operating mode
* @param description description of the operating mode
* @throws IllegalArgumentException if class does not meet criteria
*/
public void addOpMode(
Class<? extends OpMode> cls, RobotMode mode, String name, String group, String description) {
addOpMode(cls, mode, name, group, description, null, null);
}
/**
* Adds an opmode for an opmode class. The class must be a public, non-abstract subclass of OpMode
* with a public constructor that either takes no arguments or accepts a single argument of this
* class's type (the latter is preferred). It's necessary to call publishOpModes() to make the
* added mode visible to the driver station.
*
* @param cls class to add
* @param mode robot mode
* @param name name of the operating mode
* @param group group of the operating mode
* @throws IllegalArgumentException if class does not meet criteria
*/
public void addOpMode(Class<? extends OpMode> cls, RobotMode mode, String name, String group) {
addOpMode(cls, mode, name, group, "");
}
/**
* Adds an opmode for an opmode class. The class must be a public, non-abstract subclass of OpMode
* with a public constructor that either takes no arguments or accepts a single argument of this
* class's type (the latter is preferred). It's necessary to call publishOpModes() to make the
* added mode visible to the driver station.
*
* @param cls class to add
* @param mode robot mode
* @param name name of the operating mode
* @throws IllegalArgumentException if class does not meet criteria
*/
public void addOpMode(Class<? extends OpMode> cls, RobotMode mode, String name) {
addOpMode(cls, mode, name, "");
}
private void addOpModeClassImpl(
Class<?> cls,
RobotMode mode,
String name,
String group,
String description,
String textColor,
String backgroundColor) {
if (name == null || name.isBlank()) {
name = cls.getSimpleName();
}
Color tColor = textColor.isBlank() ? null : Color.fromString(textColor);
Color bColor = backgroundColor.isBlank() ? null : Color.fromString(backgroundColor);
long id = DriverStation.addOpMode(mode, name, group, description, tColor, bColor);
m_opModes.put(id, new OpModeFactory(name, () -> constructOpModeClass(cls)));
}
private void addAnnotatedOpModeImpl(
Class<?> cls, Autonomous auto, Teleop teleop, TestOpMode test) {
checkOpModeClass(cls);
// add an opmode for each annotation
if (auto != null) {
addOpModeClassImpl(
cls,
RobotMode.AUTONOMOUS,
auto.name(),
auto.group(),
auto.description(),
auto.textColor(),
auto.backgroundColor());
}
if (teleop != null) {
addOpModeClassImpl(
cls,
RobotMode.TELEOPERATED,
teleop.name(),
teleop.group(),
teleop.description(),
teleop.textColor(),
teleop.backgroundColor());
}
if (test != null) {
addOpModeClassImpl(
cls,
RobotMode.TEST,
test.name(),
test.group(),
test.description(),
test.textColor(),
test.backgroundColor());
}
}
/**
* Adds an opmode for an opmode class annotated with {@link Autonomous}, {@link Teleop}, or {@link
* TestOpMode}. The class must be a public, non-abstract subclass of OpMode with a public
* constructor that either takes no arguments or accepts a single argument of this class's type.
* It's necessary to call publishOpModes() to make the added mode visible to the driver station.
*
* @param cls class to add
* @throws IllegalArgumentException if class does not meet criteria
*/
public void addAnnotatedOpMode(Class<? extends OpMode> cls) {
Autonomous auto = cls.getAnnotation(Autonomous.class);
Teleop teleop = cls.getAnnotation(Teleop.class);
TestOpMode test = cls.getAnnotation(TestOpMode.class);
if (auto == null && teleop == null && test == null) {
throw new IllegalArgumentException(
"must be annotated with Autonomous, Teleop, or TestOpMode");
}
addAnnotatedOpModeImpl(cls, auto, teleop, test);
}
private void addAnnotatedOpModeClass(String name) {
// trim ".class" from end
String className = name.replace('/', '.').substring(0, name.length() - 6);
Class<?> cls;
try {
cls = Class.forName(className);
} catch (ClassNotFoundException e) {
return;
}
Autonomous auto = cls.getAnnotation(Autonomous.class);
Teleop teleop = cls.getAnnotation(Teleop.class);
TestOpMode test = cls.getAnnotation(TestOpMode.class);
if (auto == null && teleop == null && test == null) {
return;
}
try {
addAnnotatedOpModeImpl(cls, auto, teleop, test);
} catch (IllegalArgumentException e) {
reportAddOpModeError(cls, e.getMessage());
}
}
private void addAnnotatedOpModeClassesDir(File root, File dir, String packagePath) {
File[] files = dir.listFiles();
if (files == null) {
return;
}
for (File file : files) {
if (file.isDirectory()) {
addAnnotatedOpModeClassesDir(root, file, packagePath);
} else if (file.getName().endsWith(".class")) {
String relPath = root.toPath().relativize(file.toPath()).toString().replace('\\', '/');
addAnnotatedOpModeClass(packagePath + "." + relPath);
}
}
}
/**
* Scans for classes in the specified package and all nested packages that are annotated with
* {@link Autonomous}, {@link Teleop}, or {@link TestOpMode} and registers them. It's necessary to
* call publishOpModes() to make the added modes visible to the driver station.
*
* @param pkg package to scan
*/
public void addAnnotatedOpModeClasses(Package pkg) {
String packageName = pkg.getName();
String packagePath = packageName.replace('.', '/');
ClassLoader classLoader = Thread.currentThread().getContextClassLoader();
try {
Enumeration<URL> resources = classLoader.getResources(packagePath);
while (resources.hasMoreElements()) {
URL resource = resources.nextElement();
if ("jar".equals(resource.getProtocol())) {
// Get path of JAR file from URL path (format "file:<path_to_jar_file>!/path_to_entry")
String jarPath = resource.getPath().substring(5, resource.getPath().indexOf('!'));
try (JarFile jar = new JarFile(jarPath)) {
Enumeration<JarEntry> entries = jar.entries();
while (entries.hasMoreElements()) {
String name = entries.nextElement().getName();
if (!name.startsWith(packagePath) || !name.endsWith(".class")) {
continue;
}
addAnnotatedOpModeClass(name);
}
}
} else if ("file".equals(resource.getProtocol())) {
// Handle .class files in directories
File dir = new File(resource.getPath());
if (dir.exists() && dir.isDirectory()) {
addAnnotatedOpModeClassesDir(dir, dir, packagePath);
}
}
}
} catch (IOException e) {
e.printStackTrace();
}
}
/**
* Removes an operating mode option. It's necessary to call publishOpModes() to make the removed
* mode no longer visible to the driver station.
*
* @param mode robot mode
* @param name name of the operating mode
*/
public void removeOpMode(RobotMode mode, String name) {
long id = DriverStation.removeOpMode(mode, name);
if (id != 0) {
m_opModes.remove(id);
}
}
/** Publishes the operating mode options to the driver station. */
public void publishOpModes() {
DriverStation.publishOpModes();
}
/** Clears all operating mode options and publishes an empty list to the driver station. */
public void clearOpModes() {
DriverStation.clearOpModes();
m_opModes.clear();
}
/** Constructor. */
@SuppressWarnings("this-escape")
public OpModeRobot() {
// Scan for annotated opmode classes within the derived class's package and subpackages
addAnnotatedOpModeClasses(getClass().getPackage());
DriverStation.publishOpModes();
}
/**
* Function called exactly once after the DS is connected.
*
* <p>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() {}
/**
* Function called periodically anytime when no opmode is selected, including when the Driver
* Station is disconnected.
*/
public void nonePeriodic() {}
/**
* Background monitor thread. On mode/opmode change, this checks to see if the change is actually
* reflected in this class within a reasonable amount of time. If not, that means that the user
* code is stuck and we need to take action to try to get it to exit (up to and including program
* termination).
*/
private void monitorThreadMain(Thread thr, long opmode, int event, int endEvent) {
ControlWord word = new ControlWord();
int[] events = {event, endEvent};
while (true) {
try {
int[] signaled = WPIUtilJNI.waitForObjects(events);
for (int val : signaled) {
if (val < 0) {
return; // handle destroyed
}
}
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
return;
}
// did the opmode or enable state change?
DriverStationJNI.getUncachedControlWord(word);
if (!word.isEnabled() || word.getOpModeId() != opmode) {
break;
}
}
// call opmode stop
OpMode opMode = m_activeOpMode.get();
if (opMode != null) {
opMode.opModeStop();
}
events[0] = m_notifier;
NotifierJNI.setNotifierAlarm(m_notifier, 200000, 0, false, true); // 200 ms
try {
int[] signaled = WPIUtilJNI.waitForObjects(events);
for (int val : signaled) {
if (val < 0 || val == endEvent) {
return; // transitioned, or handle destroyed
}
}
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
return;
}
// if it hasn't transitioned after 200 ms, call thread.interrupt()
DriverStation.reportError("OpMode did not exit, interrupting thread", false);
thr.interrupt();
NotifierJNI.setNotifierAlarm(m_notifier, 800000, 0, false, true); // 800 ms
try {
int[] signaled = WPIUtilJNI.waitForObjects(events);
for (int val : signaled) {
if (val < 0 || val == endEvent) {
return; // transitioned, or handle destroyed
}
}
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
return;
}
// if it hasn't transitioned after 1 second, terminate the program
DriverStation.reportError("OpMode did not exit, terminating program", false);
HAL.terminate();
HAL.shutdown();
System.exit(0);
}
/** Provide an alternate "main loop" via startCompetition(). */
@Override
public final void startCompetition() {
System.out.println("********** Robot program startup complete **********");
DriverStationJNI.observeUserProgramStarting();
int event = WPIUtilJNI.createEvent(false, false);
DriverStationJNI.provideNewDataEventHandle(event);
m_notifier = NotifierJNI.createNotifier();
NotifierJNI.setNotifierName(m_notifier, "OpModeRobot");
try {
// Implement the opmode lifecycle
long lastModeId = -1;
boolean calledDriverStationConnected = false;
int[] events = {event, m_notifier};
while (true) {
// Wait for new data from the driver station, with 50 ms timeout
NotifierJNI.setNotifierAlarm(m_notifier, 50000, 0, false, true);
try {
int[] signaled = WPIUtilJNI.waitForObjects(events);
for (int val : signaled) {
if (val < 0) {
return; // handle destroyed
}
}
} catch (InterruptedException ex) {
Thread.currentThread().interrupt();
break;
}
// Get the latest control word and opmode
DriverStation.refreshData();
DriverStation.refreshControlWordFromCache(m_word);
if (!calledDriverStationConnected && m_word.isDSAttached()) {
calledDriverStationConnected = true;
driverStationConnected();
}
long modeId;
if (!m_word.isDSAttached()) {
modeId = 0;
} else {
modeId = m_word.getOpModeId();
}
OpMode opMode = m_activeOpMode.get();
if (opMode == null || modeId != lastModeId) {
if (opMode != null) {
// no or different opmode selected
m_activeOpMode.set(null);
opMode.opModeClose();
}
if (modeId == 0) {
// no opmode selected
nonePeriodic();
DriverStationJNI.observeUserProgram(m_word.getNative());
continue;
}
OpModeFactory factory = m_opModes.get(modeId);
if (factory == null) {
DriverStation.reportError("No OpMode found for mode " + modeId, false);
m_word.setOpModeId(0);
DriverStationJNI.observeUserProgram(m_word.getNative());
continue;
}
// Instantiate the opmode
System.out.println("********** Starting OpMode " + factory.name() + " **********");
opMode = factory.supplier().get();
if (opMode == null) {
// could not construct
m_word.setOpModeId(0);
DriverStationJNI.observeUserProgram(m_word.getNative());
continue;
}
m_activeOpMode.set(opMode);
lastModeId = modeId;
// Ensure disabledPeriodic is always called at least once
opMode.disabledPeriodic();
}
DriverStationJNI.observeUserProgram(m_word.getNative());
if (m_word.isEnabled()) {
// When enabled, call the opmode run function, then close and clear
int endMonitor = WPIUtilJNI.createEvent(true, false);
Thread curThread = Thread.currentThread();
Thread monitor =
new Thread(
() -> {
monitorThreadMain(curThread, modeId, event, endMonitor);
});
monitor.start();
try {
opMode.opModeRun(modeId);
} catch (InterruptedException e) {
// ignored
} finally {
Thread.interrupted();
WPIUtilJNI.destroyEvent(endMonitor);
try {
monitor.join();
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
}
}
opMode = m_activeOpMode.getAndSet(null);
if (opMode != null) {
opMode.opModeClose();
}
} else {
// When disabled, call the disabledPeriodic function
opMode.disabledPeriodic();
}
}
} finally {
DriverStationJNI.removeNewDataEventHandle(event);
WPIUtilJNI.destroyEvent(event);
NotifierJNI.destroyNotifier(m_notifier);
}
}
/** Ends the main loop in startCompetition(). */
@Override
public final void endCompetition() {
NotifierJNI.destroyNotifier(m_notifier);
OpMode opMode = m_activeOpMode.get();
if (opMode != null) {
opMode.opModeStop();
}
}
}

38
wpilibj/src/main/java/org/wpilib/framework/RobotBase.java
View File

@@ -180,7 +180,7 @@ public abstract class RobotBase implements AutoCloseable {
*
* @return True if the Robot is currently disabled by the Driver Station.
*/
public boolean isDisabled() {
public static boolean isDisabled() {
return DriverStation.isDisabled();
}
@@ -189,7 +189,7 @@ public abstract class RobotBase implements AutoCloseable {
*
* @return True if the Robot is currently enabled by the Driver Station.
*/
public boolean isEnabled() {
public static boolean isEnabled() {
return DriverStation.isEnabled();
}
@@ -198,7 +198,7 @@ public abstract class RobotBase implements AutoCloseable {
*
* @return True if the robot is currently operating Autonomously.
*/
public boolean isAutonomous() {
public static boolean isAutonomous() {
return DriverStation.isAutonomous();
}
@@ -208,7 +208,7 @@ public abstract class RobotBase implements AutoCloseable {
*
* @return True if the robot is currently operating autonomously while enabled.
*/
public boolean isAutonomousEnabled() {
public static boolean isAutonomousEnabled() {
return DriverStation.isAutonomousEnabled();
}
@@ -217,7 +217,7 @@ public abstract class RobotBase implements AutoCloseable {
*
* @return True if the robot is currently operating in Test mode.
*/
public boolean isTest() {
public static boolean isTest() {
return DriverStation.isTest();
}
@@ -226,7 +226,7 @@ public abstract class RobotBase implements AutoCloseable {
*
* @return True if the robot is currently operating in Test mode while enabled.
*/
public boolean isTestEnabled() {
public static boolean isTestEnabled() {
return DriverStation.isTestEnabled();
}
@@ -236,7 +236,7 @@ public abstract class RobotBase implements AutoCloseable {
*
* @return True if the robot is currently operating in Tele-Op mode.
*/
public boolean isTeleop() {
public static boolean isTeleop() {
return DriverStation.isTeleop();
}
@@ -246,10 +246,32 @@ public abstract class RobotBase implements AutoCloseable {
*
* @return True if the robot is currently operating in Tele-Op mode while enabled.
*/
public boolean isTeleopEnabled() {
public static boolean isTeleopEnabled() {
return DriverStation.isTeleopEnabled();
}
/**
* Gets the currently selected operating mode of the driver station. Note this does not mean the
* robot is enabled; use isEnabled() for that.
*
* @return the unique ID provided by the DriverStation.addOpMode() function; may return 0 or a
* unique ID not added, so callers should be prepared to handle that case
*/
public static long getOpModeId() {
return DriverStation.getOpModeId();
}
/**
* Gets the currently selected operating mode of the driver station. Note this does not mean the
* robot is enabled; use isEnabled() for that.
*
* @return Operating mode string; may return a string not in the list of options, so callers
* should be prepared to handle that case
*/
public static String getOpMode() {
return DriverStation.getOpMode();
}
/**
* Start the main robot code. This function will be called once and should not exit until
* signalled by {@link #endCompetition()}

66
wpilibj/src/main/java/org/wpilib/framework/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.framework;
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() {}
}

5
wpilibj/src/main/java/org/wpilib/hardware/motor/MotorSafety.java
View File

@@ -7,7 +7,6 @@ package org.wpilib.hardware.motor;
import java.util.LinkedHashSet;
import java.util.Set;
import org.wpilib.driverstation.DriverStation;
import org.wpilib.framework.RobotState;
import org.wpilib.hardware.hal.ControlWord;
import org.wpilib.hardware.hal.DriverStationJNI;
import org.wpilib.system.Timer;
@@ -51,7 +50,7 @@ public abstract class MotorSafety {
}
if (!timedOut) {
DriverStationJNI.getControlWord(controlWord);
if (!(controlWord.getEnabled() && controlWord.getDSAttached())) {
if (!(controlWord.isEnabled() && controlWord.isDSAttached())) {
safetyCounter = 0;
}
if (++safetyCounter >= 4) {
@@ -135,7 +134,7 @@ public abstract class MotorSafety {
stopTime = m_stopTime;
}
if (!enabled || RobotState.isDisabled() || RobotState.isTest()) {
if (!enabled || DriverStation.isDisabled() || DriverStation.isTest()) {
return;
}

88
wpilibj/src/main/java/org/wpilib/internal/DriverStationModeThread.java
View File

@@ -5,101 +5,61 @@
package org.wpilib.internal;
import java.util.concurrent.atomic.AtomicBoolean;
import java.util.concurrent.atomic.AtomicLong;
import org.wpilib.driverstation.DriverStation;
import org.wpilib.hardware.hal.ControlWord;
import org.wpilib.hardware.hal.DriverStationJNI;
import org.wpilib.util.WPIUtilJNI;
/** For internal use only. */
public class DriverStationModeThread implements AutoCloseable {
private final AtomicBoolean m_keepAlive = new AtomicBoolean();
private final AtomicBoolean m_keepAlive = new AtomicBoolean(true);
private final AtomicLong m_userControlWord;
private final int m_handle = WPIUtilJNI.createEvent(false, false);
private final Thread m_thread;
private boolean m_userInDisabled;
private boolean m_userInAutonomous;
private boolean m_userInTeleop;
private boolean m_userInTest;
/** Internal use only. */
public DriverStationModeThread() {
m_keepAlive.set(true);
/**
* Internal use only.
*
* @param word control word
*/
public DriverStationModeThread(ControlWord word) {
m_userControlWord = new AtomicLong(word.getNative());
DriverStationJNI.provideNewDataEventHandle(m_handle);
m_thread = new Thread(this::run, "DriverStationMode");
m_thread.start();
}
private void run() {
int handle = WPIUtilJNI.createEvent(false, false);
DriverStationJNI.provideNewDataEventHandle(handle);
while (m_keepAlive.get()) {
while (true) {
try {
WPIUtilJNI.waitForObjectTimeout(handle, 0.1);
WPIUtilJNI.waitForObjectTimeout(m_handle, 0.1);
} catch (InterruptedException e) {
DriverStationJNI.removeNewDataEventHandle(handle);
WPIUtilJNI.destroyEvent(handle);
Thread.currentThread().interrupt();
return;
}
if (!m_keepAlive.get()) {
return;
}
DriverStation.refreshData();
if (m_userInDisabled) {
DriverStationJNI.observeUserProgramDisabled();
}
if (m_userInAutonomous) {
DriverStationJNI.observeUserProgramAutonomous();
}
if (m_userInTeleop) {
DriverStationJNI.observeUserProgramTeleop();
}
if (m_userInTest) {
DriverStationJNI.observeUserProgramTest();
}
DriverStationJNI.observeUserProgram(m_userControlWord.get());
}
DriverStationJNI.removeNewDataEventHandle(handle);
WPIUtilJNI.destroyEvent(handle);
}
/**
* Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
* purposes only.
*
* @param entering If true, starting disabled code; if false, leaving disabled code
* @param word control word
*/
public void inDisabled(boolean entering) {
m_userInDisabled = entering;
}
/**
* Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
* purposes only.
*
* @param entering If true, starting autonomous code; if false, leaving autonomous code
*/
public void inAutonomous(boolean entering) {
m_userInAutonomous = entering;
}
/**
* Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
* purposes only.
*
* @param entering If true, starting teleop code; if false, leaving teleop code
*/
public void inTeleop(boolean entering) {
m_userInTeleop = entering;
}
/**
* Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
* purposes only.
*
* @param entering If true, starting test code; if false, leaving test code
*/
public void inTest(boolean entering) {
m_userInTest = entering;
public void inControl(ControlWord word) {
m_userControlWord.set(word.getNative());
}
@Override
public void close() {
DriverStationJNI.removeNewDataEventHandle(m_handle);
WPIUtilJNI.destroyEvent(m_handle);
m_keepAlive.set(false);
try {
m_thread.join();

56
wpilibj/src/main/java/org/wpilib/opmode/Autonomous.java Normal file
View File

@@ -0,0 +1,56 @@
// 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.lang.annotation.Documented;
import java.lang.annotation.ElementType;
import java.lang.annotation.Retention;
import java.lang.annotation.RetentionPolicy;
import java.lang.annotation.Target;
/** Annotation for automatic registration of autonomous opmode classes. */
@Retention(RetentionPolicy.RUNTIME)
@Target(ElementType.TYPE)
@Documented
public @interface Autonomous {
/**
* Name. This is shown as the selection name in the Driver Station, and must be unique across all
* autonomous opmodes in the project. If not specified, defaults to the name of the class.
*
* @return Name
*/
String name() default "";
/**
* Group. All opmodes with the same group are grouped together for selection. If not specified,
* defaults to ungrouped.
*
* @return Group
*/
String group() default "";
/**
* Extended description. Optional.
*
* @return Description
*/
String description() default "";
/**
* Text color. Optional. Supports all formats supported by {@link
* org.wpilib.util.Color#fromString(String)}.
*
* @return Text color
*/
String textColor() default "";
/**
* Text background color. Optional. Supports all formats supported by {@link
* org.wpilib.util.Color#fromString(String)}.
*
* @return Text background color
*/
String backgroundColor() default "";
}

84
wpilibj/src/main/java/org/wpilib/opmode/LinearOpMode.java Normal file
View File

@@ -0,0 +1,84 @@
// 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.atomic.AtomicBoolean;
import org.wpilib.driverstation.DriverStation;
import org.wpilib.hardware.hal.ControlWord;
import org.wpilib.internal.DriverStationModeThread;
/**
* An opmode structure for "linear" operation. The user is responsible for implementing any looping
* behavior; after run() returns it will not be called again on the same object.
*
* <p>Lifecycle:
*
* <ul>
* <li>constructed when opmode selected on driver station
* <li>disabledPeriodic() called periodically as long as DS is disabled
* <li>when DS transitions from disabled to enabled, run() will be called exactly once
* <li>when DS transitions from enabled to disabled, or a different opmode is selected on the
* driver station, close() is called; the object is not reused
* </ul>
*
* <p>The user is responsible for exiting run() when the opmode is directed to stop executing. This
* is indicated by isRunning() returning false. All other methods should be written to return as
* quickly as possible when isRunning() returns false.
*/
public abstract class LinearOpMode implements OpMode {
/** Constructor. */
public LinearOpMode() {}
/** Called periodically while the opmode is selected on the DS and the robot is disabled. */
@Override
public void disabledPeriodic() {}
/**
* Called when the opmode is de-selected on the DS. The object is not reused even if the same
* opmode is selected again (a new object will be created).
*/
public void close() {}
/**
* Called once when the robot is enabled. It will not be called a second time on the same object.
*
* @throws InterruptedException when interrupted
*/
public abstract void run() throws InterruptedException;
/**
* Returns true while this opmode is selected (regardless of enable state). All other functions
* should be written to return as quickly as possible when this returns false.
*
* @return True if opmode selected, false otherwise
*/
public boolean isRunning() {
return m_running.get();
}
// implements OpMode interface
@Override
public final void opModeRun(long opModeId) throws InterruptedException {
ControlWord word = new ControlWord();
DriverStation.refreshControlWordFromCache(word);
word.setOpModeId(opModeId);
try (DriverStationModeThread bgThread = new DriverStationModeThread(word)) {
run();
}
}
@Override
public final void opModeStop() {
m_running.set(false);
}
@Override
public final void opModeClose() {
close();
}
private final AtomicBoolean m_running = new AtomicBoolean(true);
}

39
wpilibj/src/main/java/org/wpilib/opmode/OpMode.java Normal file
View File

@@ -0,0 +1,39 @@
// 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;
/**
* Top-level interface for opmode classes. Users should generally extend one of the abstract
* implementations of this interface (e.g. {@link PeriodicOpMode} or {@link LinearOpMode}) rather
* than directly implementing this interface.
*/
public interface OpMode {
/**
* This function is called periodically while the opmode is selected on the DS (robot is
* disabled). Code that should only run once when the opmode is selected should go in the opmode
* constructor.
*/
default void disabledPeriodic() {}
/**
* This function is called when the opmode starts (robot is enabled).
*
* @param opModeId opmode unique ID
* @throws InterruptedException when interrupted
*/
void opModeRun(long opModeId) throws InterruptedException;
/**
* This function is called asynchronously when the robot is disabled, to request the opmode return
* from opModeRun().
*/
void opModeStop();
/**
* This function is called when the opmode is no longer selected on the DS or after opModeRun()
* returns. The object will not be reused after this is called.
*/
void opModeClose();
}

344
wpilibj/src/main/java/org/wpilib/opmode/PeriodicOpMode.java Normal file
View File

@@ -0,0 +1,344 @@
// 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.driverstation.DriverStation;
import org.wpilib.hardware.hal.ControlWord;
import org.wpilib.hardware.hal.DriverStationJNI;
import org.wpilib.hardware.hal.HAL;
import org.wpilib.hardware.hal.NotifierJNI;
import org.wpilib.networktables.NetworkTableInstance;
import org.wpilib.smartdashboard.SmartDashboard;
import org.wpilib.system.RobotController;
import org.wpilib.system.Watchdog;
import org.wpilib.units.measure.Time;
import org.wpilib.util.WPIUtilJNI;
/**
* An opmode structure for periodic operation. This base class implements a loop that runs one or
* more functions periodically (on a set time interval aka loop period). The primary periodic
* callback function is the abstract periodic() function; the time interval for this callback is 20
* ms by default, but may be changed via passing a different time interval to the constructor.
* Additional periodic callbacks with different intervals can be added using the addPeriodic() set
* of functions.
*
* <p>Lifecycle:
*
* <ul>
* <li>constructed when opmode selected on driver station
* <li>disabledPeriodic() called periodically as long as DS is disabled. Note this is not called
* on a set time interval (it does not use the same time interval as periodic())
* <li>when DS transitions from disabled to enabled, start() is called once
* <li>while DS is enabled, periodic() is called periodically on the time interval set by the
* constructor, and additional periodic callbacks added via addPeriodic() are called
* periodically on their set time intervals
* <li>when DS transitions from enabled to disabled, or a different opmode is selected on the
* driver station when the DS is enabled, end() is called, followed by close(); the object is
* not reused
* <li>if a different opmode is selected on the driver station when the DS is disabled, only
* close() is called; the object is not reused
* </ul>
*/
public abstract class PeriodicOpMode implements OpMode {
@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 int m_notifier = NotifierJNI.createNotifier();
private long m_startTimeUs;
private long m_loopStartTimeUs;
private final ControlWord m_word = new ControlWord();
private final double m_period;
private final Watchdog m_watchdog;
private long m_opModeId;
private boolean m_running = true;
private final PriorityQueue<Callback> m_callbacks = new PriorityQueue<>();
/**
* Constructor. Periodic opmodes may specify the period used for the periodic() function; the
* no-argument constructor uses a default period of 20 ms.
*/
protected PeriodicOpMode() {
this(kDefaultPeriod);
}
/**
* Constructor. Periodic opmodes may specify the period used for the periodic() function.
*
* @param period period (in seconds) for callbacks to the periodic() function
*/
protected PeriodicOpMode(double period) {
m_startTimeUs = RobotController.getFPGATime();
m_period = period;
m_watchdog = new Watchdog(period, this::printLoopOverrunMessage);
addPeriodic(this::loopFunc, period);
NotifierJNI.setNotifierName(m_notifier, "PeriodicOpMode");
HAL.reportUsage("OpMode", "PeriodicOpMode");
}
/** Called periodically while the opmode is selected on the DS (robot is disabled). */
@Override
public void disabledPeriodic() {}
/**
* Called when the opmode is de-selected on the DS. The object is not reused even if the same
* opmode is selected again (a new object will be created).
*/
public void close() {}
/**
* Called a single time when the robot transitions from disabled to enabled. This is called prior
* to periodic() being called.
*/
public void start() {}
/** Called periodically while the robot is enabled. */
public abstract void periodic();
/**
* Called a single time when the robot transitions from enabled to disabled, or just before
* close() is called if a different opmode is selected while the robot is enabled.
*/
public void end() {}
/**
* 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 the same Notifier as periodic(), so periodic() 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 the same Notifier as periodic(), so periodic() 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 PeriodicOpMode.
*/
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 the same Notifier as periodic(), so periodic() 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 the same Notifier as periodic(), so periodic() 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 PeriodicOpMode.
*/
public final void addPeriodic(Runnable callback, Time period, Time offset) {
addPeriodic(callback, period.in(Seconds), offset.in(Seconds));
}
/**
* 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();
DriverStation.refreshControlWordFromCache(m_word);
m_word.setOpModeId(m_opModeId);
DriverStationJNI.observeUserProgram(m_word.getNative());
if (!DriverStation.isEnabled() || DriverStation.getOpModeId() != m_opModeId) {
m_running = false;
return;
}
m_watchdog.reset();
periodic();
m_watchdog.addEpoch("periodic()");
SmartDashboard.updateValues();
m_watchdog.addEpoch("SmartDashboard.updateValues()");
// if (isSimulation()) {
// HAL.simPeriodicBefore();
// simulationPeriodic();
// HAL.simPeriodicAfter();
// m_watchdog.addEpoch("simulationPeriodic()");
// }
m_watchdog.disable();
// Flush NetworkTables
NetworkTableInstance.getDefault().flushLocal();
// Warn on loop time overruns
if (m_watchdog.isExpired()) {
m_watchdog.printEpochs();
}
}
// implements OpMode interface
@Override
public final void opModeRun(long opModeId) {
m_opModeId = opModeId;
start();
while (m_running) {
// 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.setNotifierAlarm(m_notifier, callback.expirationTime, 0, true, true);
try {
WPIUtilJNI.waitForObject(m_notifier);
} catch (InterruptedException ex) {
Thread.currentThread().interrupt();
break;
}
long currentTime = RobotController.getFPGATime();
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);
}
}
end();
}
@Override
public final void opModeStop() {
NotifierJNI.destroyNotifier(m_notifier);
m_notifier = 0;
}
@Override
public final void opModeClose() {
if (m_notifier != 0) {
NotifierJNI.destroyNotifier(m_notifier);
}
close();
}
/** 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);
}
}

56
wpilibj/src/main/java/org/wpilib/opmode/Teleop.java Normal file
View File

@@ -0,0 +1,56 @@
// 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.lang.annotation.Documented;
import java.lang.annotation.ElementType;
import java.lang.annotation.Retention;
import java.lang.annotation.RetentionPolicy;
import java.lang.annotation.Target;
/** Annotation for automatic registration of teleoperated opmode classes. */
@Retention(RetentionPolicy.RUNTIME)
@Target(ElementType.TYPE)
@Documented
public @interface Teleop {
/**
* Name. This is shown as the selection name in the Driver Station, and must be unique across all
* teleoperated opmodes in the project. If not specified, defaults to the name of the class.
*
* @return Name
*/
String name() default "";
/**
* Group. All opmodes with the same group are grouped together for selection. If not specified,
* defaults to ungrouped.
*
* @return Group
*/
String group() default "";
/**
* Extended description. Optional.
*
* @return Description
*/
String description() default "";
/**
* Text color. Optional. Supports all formats supported by {@link
* org.wpilib.util.Color#fromString(String)}.
*
* @return Text color
*/
String textColor() default "";
/**
* Text background color. Optional. Supports all formats supported by {@link
* org.wpilib.util.Color#fromString(String)}.
*
* @return Text background color
*/
String backgroundColor() default "";
}

56
wpilibj/src/main/java/org/wpilib/opmode/TestOpMode.java Normal file
View File

@@ -0,0 +1,56 @@
// 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.lang.annotation.Documented;
import java.lang.annotation.ElementType;
import java.lang.annotation.Retention;
import java.lang.annotation.RetentionPolicy;
import java.lang.annotation.Target;
/** Annotation for automatic registration of test opmode classes. */
@Retention(RetentionPolicy.RUNTIME)
@Target(ElementType.TYPE)
@Documented
public @interface TestOpMode {
/**
* Name. This is shown as the selection name in the Driver Station, and must be unique across all
* test opmodes in the project. If not specified, defaults to the name of the class.
*
* @return Name
*/
String name() default "";
/**
* Group. All opmodes with the same group are grouped together for selection. If not specified,
* defaults to ungrouped.
*
* @return Group
*/
String group() default "";
/**
* Extended description. Optional.
*
* @return Description
*/
String description() default "";
/**
* Text color. Optional. Supports all formats supported by {@link
* org.wpilib.util.Color#fromString(String)}.
*
* @return Text color
*/
String textColor() default "";
/**
* Text background color. Optional. Supports all formats supported by {@link
* org.wpilib.util.Color#fromString(String)}.
*
* @return Text background color
*/
String backgroundColor() default "";
}

112
wpilibj/src/main/java/org/wpilib/simulation/DriverStationSim.java
View File

@@ -4,9 +4,12 @@
package org.wpilib.simulation;
import java.util.function.BiConsumer;
import org.wpilib.driverstation.DriverStation;
import org.wpilib.hardware.hal.AllianceStationID;
import org.wpilib.hardware.hal.DriverStationJNI;
import org.wpilib.hardware.hal.OpModeOption;
import org.wpilib.hardware.hal.RobotMode;
import org.wpilib.hardware.hal.simulation.DriverStationDataJNI;
import org.wpilib.hardware.hal.simulation.NotifyCallback;
import org.wpilib.util.WPIUtilJNI;
@@ -49,64 +52,34 @@ public final class DriverStationSim {
}
/**
* Register a callback on whether the DS is in autonomous mode.
* Register a callback on DS robot mode changes.
*
* @param callback the callback that will be called on autonomous mode entrance/exit
* @param callback the callback that will be called when the robot mode changes
* @param initialNotify if true, the callback will be run on the initial value
* @return the {@link CallbackStore} object associated with this callback.
*/
public static CallbackStore registerAutonomousCallback(
public static CallbackStore registerRobotModeCallback(
NotifyCallback callback, boolean initialNotify) {
int uid = DriverStationDataJNI.registerAutonomousCallback(callback, initialNotify);
return new CallbackStore(uid, DriverStationDataJNI::cancelAutonomousCallback);
int uid = DriverStationDataJNI.registerRobotModeCallback(callback, initialNotify);
return new CallbackStore(uid, DriverStationDataJNI::cancelRobotModeCallback);
}
/**
* Check if the DS is in autonomous.
* Gets the robot mode set by the DS.
*
* @return true if autonomous
* @return robot mode
*/
public static boolean getAutonomous() {
return DriverStationDataJNI.getAutonomous();
public static RobotMode getRobotMode() {
return DriverStationDataJNI.getRobotMode();
}
/**
* Change whether the DS is in autonomous.
* Change the robot mode set by the DS.
*
* @param autonomous the new value
* @param mode the new value
*/
public static void setAutonomous(boolean autonomous) {
DriverStationDataJNI.setAutonomous(autonomous);
}
/**
* Register a callback on whether the DS is in test mode.
*
* @param callback the callback that will be called whenever the test mode is entered or left
* @param initialNotify if true, the callback will be run on the initial value
* @return the {@link CallbackStore} object associated with this callback.
*/
public static CallbackStore registerTestCallback(NotifyCallback callback, boolean initialNotify) {
int uid = DriverStationDataJNI.registerTestCallback(callback, initialNotify);
return new CallbackStore(uid, DriverStationDataJNI::cancelTestCallback);
}
/**
* Check if the DS is in test.
*
* @return true if test
*/
public static boolean getTest() {
return DriverStationDataJNI.getTest();
}
/**
* Change whether the DS is in test.
*
* @param test the new value
*/
public static void setTest(boolean test) {
DriverStationDataJNI.setTest(test);
public static void setRobotMode(RobotMode mode) {
DriverStationDataJNI.setRobotMode(mode);
}
/**
@@ -283,6 +256,59 @@ public final class DriverStationSim {
DriverStationDataJNI.setMatchTime(matchTime);
}
/**
* Register a callback on opmode changes.
*
* @param callback the callback that will be called when the opmode changes
* @param initialNotify if true, the callback will be run on the initial value
* @return the {@link CallbackStore} object associated with this callback.
*/
public static CallbackStore registerOpModeCallback(
NotifyCallback callback, boolean initialNotify) {
int uid = DriverStationDataJNI.registerOpModeCallback(callback, initialNotify);
return new CallbackStore(uid, DriverStationDataJNI::cancelOpModeCallback);
}
/**
* Gets the opmode.
*
* @return opmode
*/
public static long getOpMode() {
return DriverStationDataJNI.getOpMode();
}
/**
* Change the opmode.
*
* @param opmode the new value
*/
public static void setOpMode(long opmode) {
DriverStationDataJNI.setOpMode(opmode);
}
/**
* Register a callback on opmode options changes.
*
* @param callback the callback that will be called when the list of opmodes changes
* @param initialNotify if true, the callback will be run on the initial value
* @return the {@link CallbackStore} object associated with this callback.
*/
public static CallbackStore registerOpModeOptionsCallback(
BiConsumer<String, OpModeOption[]> callback, boolean initialNotify) {
int uid = DriverStationDataJNI.registerOpModeOptionsCallback(callback, initialNotify);
return new CallbackStore(uid, DriverStationDataJNI::cancelOpModeOptionsCallback);
}
/**
* Gets the list of opmode options.
*
* @return opmodes list
*/
public static OpModeOption[] getOpModeOptions() {
return DriverStationDataJNI.getOpModeOptions();
}
/** Updates DriverStation data so that new values are visible to the user program. */
public static void notifyNewData() {
int handle = WPIUtilJNI.createEvent(false, false);

19
wpilibj/src/main/java/org/wpilib/simulation/SimHooks.java
View File

@@ -4,6 +4,7 @@
package org.wpilib.simulation;
import org.wpilib.hardware.hal.ControlWord;
import org.wpilib.hardware.hal.simulation.SimulatorJNI;
/** Simulation hooks. */
@@ -42,6 +43,24 @@ public final class SimHooks {
return SimulatorJNI.getProgramStarted();
}
/**
* Sets the user program state (control word).
*
* @param controlWord control word
*/
public static void setProgramState(ControlWord controlWord) {
SimulatorJNI.setProgramState(controlWord.getNative());
}
/**
* Gets the user program state (control word).
*
* @param controlWord control word (output)
*/
public static void getProgramState(ControlWord controlWord) {
SimulatorJNI.getProgramState(controlWord);
}
/** Restart the simulator time. */
public static void restartTiming() {
SimulatorJNI.restartTiming();