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

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>
2026-06-25 01:41:43 +00:00 · 2025-12-12 21:25:57 -07:00
parent 2a41b80e00
commit dacded37e5
163 changed files with 7454 additions and 2175 deletions
--- a/wpilibj/src/main/java/org/wpilib/driverstation/DSControlWord.java
+++ b/wpilibj/src/main/java/org/wpilib/driverstation/DSControlWord.java
@@ -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();
-  }
-}
--- a/wpilibj/src/main/java/org/wpilib/driverstation/DriverStation.java
+++ b/wpilibj/src/main/java/org/wpilib/driverstation/DriverStation.java
@@ -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();