[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>

wpilibjExamples/src/main/java/org/wpilib/templates/educational/EducationalRobot.java

@@ -6,7 +6,9 @@ package org.wpilib.templates.educational;
 
 import org.wpilib.driverstation.DriverStation;
 import org.wpilib.framework.RobotBase;
+import org.wpilib.hardware.hal.ControlWord;
 import org.wpilib.hardware.hal.DriverStationJNI;
+import org.wpilib.hardware.hal.RobotMode;
 import org.wpilib.internal.DriverStationModeThread;
 import org.wpilib.util.WPIUtilJNI;
 
@@ -34,7 +36,14 @@ public class EducationalRobot extends RobotBase {
 
   @Override
   public void startCompetition() {
-    DriverStationModeThread modeThread = new DriverStationModeThread();
+    // Create an opmode per robot mode
+    DriverStation.addOpMode(RobotMode.AUTONOMOUS, "Auto");
+    DriverStation.addOpMode(RobotMode.TELEOPERATED, "Teleop");
+    DriverStation.addOpMode(RobotMode.TEST, "Test");
+    DriverStation.publishOpModes();
+
+    final ControlWord word = new ControlWord();
+    DriverStationModeThread modeThread = new DriverStationModeThread(word);
 
     int event = WPIUtilJNI.createEvent(false, false);
 
@@ -44,10 +53,10 @@ public class EducationalRobot extends RobotBase {
     DriverStationJNI.observeUserProgramStarting();
 
     while (!Thread.currentThread().isInterrupted() && !m_exit) {
+      DriverStation.refreshControlWordFromCache(word);
+      modeThread.inControl(word);
       if (isDisabled()) {
-        modeThread.inDisabled(true);
         disabled();
-        modeThread.inDisabled(false);
         while (isDisabled()) {
           try {
             WPIUtilJNI.waitForObject(event);
@@ -56,9 +65,7 @@ public class EducationalRobot extends RobotBase {
           }
         }
       } else if (isAutonomous()) {
-        modeThread.inAutonomous(true);
         autonomous();
-        modeThread.inAutonomous(false);
         while (isAutonomousEnabled()) {
           try {
             WPIUtilJNI.waitForObject(event);
@@ -67,9 +74,7 @@ public class EducationalRobot extends RobotBase {
           }
         }
       } else if (isTest()) {
-        modeThread.inTest(true);
         test();
-        modeThread.inTest(false);
         while (isTest() && isEnabled()) {
           try {
             WPIUtilJNI.waitForObject(event);
@@ -78,9 +83,7 @@ public class EducationalRobot extends RobotBase {
           }
         }
       } else {
-        modeThread.inTeleop(true);
         teleop();
-        modeThread.inTeleop(false);
         while (isTeleopEnabled()) {
           try {
             WPIUtilJNI.waitForObject(event);

wpilibjExamples/src/main/java/org/wpilib/templates/opmode/Main.java

@@ -0,0 +1,25 @@
+// 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.templates.opmode;
+
+import org.wpilib.framework.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/templates/opmode/Robot.java

@@ -0,0 +1,33 @@
+// 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.templates.opmode;
+
+import org.wpilib.framework.OpModeRobot;
+
+/**
+ * The methods in this class are called automatically as described in the OpModeRobot documentation.
+ * OpMode classes anywhere in the package (or sub-packages) where this class is located are
+ * automatically registered to display in the Driver Station. If you change the name of this class
+ * or the package after creating this project, you must also update the Main.java file in the
+ * project.
+ */
+public class Robot extends OpModeRobot {
+  /**
+   * This function is run when the robot is first started up and should be used for any
+   * initialization code.
+   */
+  public Robot() {}
+
+  /** This function is called exactly once when the DS first connects. */
+  @Override
+  public void driverStationConnected() {}
+
+  /**
+   * This function is called periodically anytime when no opmode is selected, including when the
+   * Driver Station is disconnected.
+   */
+  @Override
+  public void nonePeriodic() {}
+}

wpilibjExamples/src/main/java/org/wpilib/templates/opmode/opmode/MyAuto.java

@@ -0,0 +1,29 @@
+// 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.templates.opmode.opmode;
+
+import org.wpilib.opmode.Autonomous;
+import org.wpilib.opmode.PeriodicOpMode;
+import org.wpilib.templates.opmode.Robot;
+
+@Autonomous(name = "My Auto", group = "Group 1")
+public class MyAuto extends PeriodicOpMode {
+  private final Robot m_robot;
+
+  /** The Robot instance is passed into the opmode via the constructor. */
+  public MyAuto(Robot robot) {
+    m_robot = robot;
+    /*
+     * Can call super(period) to set a different periodic time interval.
+     *
+     * Additional periodic methods may be configured with addPeriodic().
+     */
+  }
+
+  @Override
+  public void periodic() {
+    // Put custom auto code here
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/templates/opmode/opmode/MyTeleop.java

@@ -0,0 +1,44 @@
+// 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.templates.opmode.opmode;
+
+import org.wpilib.opmode.PeriodicOpMode;
+import org.wpilib.opmode.Teleop;
+import org.wpilib.templates.opmode.Robot;
+
+@Teleop
+public class MyTeleop extends PeriodicOpMode {
+  private final Robot m_robot;
+
+  /** The Robot instance is passed into the opmode via the constructor. */
+  public MyTeleop(Robot robot) {
+    m_robot = robot;
+  }
+
+  @Override
+  public void disabledPeriodic() {
+    /* Called periodically (on every DS packet) while the robot is disabled. */
+  }
+
+  @Override
+  public void start() {
+    /* Called once when the robot is enabled. */
+  }
+
+  @Override
+  public void periodic() {
+    /* Called periodically (set time interval) while the robot is enabled. */
+  }
+
+  @Override
+  public void end() {
+    /* Called when the robot is disabled (after previously being enabled). */
+  }
+
+  @Override
+  public void close() {
+    /* Called when the opmode is de-selected / no additional methods will be called. */
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/templates/robotbaseskeleton/Robot.java

@@ -6,7 +6,9 @@ package org.wpilib.templates.robotbaseskeleton;
 
 import org.wpilib.driverstation.DriverStation;
 import org.wpilib.framework.RobotBase;
+import org.wpilib.hardware.hal.ControlWord;
 import org.wpilib.hardware.hal.DriverStationJNI;
+import org.wpilib.hardware.hal.RobotMode;
 import org.wpilib.internal.DriverStationModeThread;
 import org.wpilib.util.WPIUtilJNI;
 
@@ -29,7 +31,14 @@ public class Robot extends RobotBase {
 
   @Override
   public void startCompetition() {
-    DriverStationModeThread modeThread = new DriverStationModeThread();
+    // Create an opmode per robot mode
+    DriverStation.addOpMode(RobotMode.AUTONOMOUS, "Auto");
+    DriverStation.addOpMode(RobotMode.TELEOPERATED, "Teleop");
+    DriverStation.addOpMode(RobotMode.TEST, "Test");
+    DriverStation.publishOpModes();
+
+    final ControlWord word = new ControlWord();
+    DriverStationModeThread modeThread = new DriverStationModeThread(word);
 
     int event = WPIUtilJNI.createEvent(false, false);
 
@@ -39,10 +48,10 @@ public class Robot extends RobotBase {
     DriverStationJNI.observeUserProgramStarting();
 
     while (!Thread.currentThread().isInterrupted() && !m_exit) {
+      DriverStation.refreshControlWordFromCache(word);
+      modeThread.inControl(word);
       if (isDisabled()) {
-        modeThread.inDisabled(true);
         disabled();
-        modeThread.inDisabled(false);
         while (isDisabled()) {
           try {
             WPIUtilJNI.waitForObject(event);
@@ -51,9 +60,7 @@ public class Robot extends RobotBase {
           }
         }
       } else if (isAutonomous()) {
-        modeThread.inAutonomous(true);
         autonomous();
-        modeThread.inAutonomous(false);
         while (isAutonomousEnabled()) {
           try {
             WPIUtilJNI.waitForObject(event);
@@ -62,9 +69,7 @@ public class Robot extends RobotBase {
           }
         }
       } else if (isTest()) {
-        modeThread.inTest(true);
         test();
-        modeThread.inTest(false);
         while (isTest() && isEnabled()) {
           try {
             WPIUtilJNI.waitForObject(event);
@@ -73,9 +78,7 @@ public class Robot extends RobotBase {
           }
         }
       } else {
-        modeThread.inTeleop(true);
         teleop();
-        modeThread.inTeleop(false);
         while (isTeleopEnabled()) {
           try {
             WPIUtilJNI.waitForObject(event);

wpilibjExamples/src/main/java/org/wpilib/templates/romieducational/EducationalRobot.java

@@ -6,7 +6,9 @@ package org.wpilib.templates.romieducational;
 
 import org.wpilib.driverstation.DriverStation;
 import org.wpilib.framework.RobotBase;
+import org.wpilib.hardware.hal.ControlWord;
 import org.wpilib.hardware.hal.DriverStationJNI;
+import org.wpilib.hardware.hal.RobotMode;
 import org.wpilib.internal.DriverStationModeThread;
 import org.wpilib.util.WPIUtilJNI;
 
@@ -34,7 +36,14 @@ public class EducationalRobot extends RobotBase {
 
   @Override
   public void startCompetition() {
-    DriverStationModeThread modeThread = new DriverStationModeThread();
+    // Create an opmode per robot mode
+    DriverStation.addOpMode(RobotMode.AUTONOMOUS, "Auto");
+    DriverStation.addOpMode(RobotMode.TELEOPERATED, "Teleop");
+    DriverStation.addOpMode(RobotMode.TEST, "Test");
+    DriverStation.publishOpModes();
+
+    final ControlWord word = new ControlWord();
+    DriverStationModeThread modeThread = new DriverStationModeThread(word);
 
     int event = WPIUtilJNI.createEvent(false, false);
 
@@ -44,10 +53,10 @@ public class EducationalRobot extends RobotBase {
     DriverStationJNI.observeUserProgramStarting();
 
     while (!Thread.currentThread().isInterrupted() && !m_exit) {
+      DriverStation.refreshControlWordFromCache(word);
+      modeThread.inControl(word);
       if (isDisabled()) {
-        modeThread.inDisabled(true);
         disabled();
-        modeThread.inDisabled(false);
         while (isDisabled()) {
           try {
             WPIUtilJNI.waitForObject(event);
@@ -56,9 +65,7 @@ public class EducationalRobot extends RobotBase {
           }
         }
       } else if (isAutonomous()) {
-        modeThread.inAutonomous(true);
         autonomous();
-        modeThread.inAutonomous(false);
         while (isAutonomousEnabled()) {
           try {
             WPIUtilJNI.waitForObject(event);
@@ -67,9 +74,7 @@ public class EducationalRobot extends RobotBase {
           }
         }
       } else if (isTest()) {
-        modeThread.inTest(true);
         test();
-        modeThread.inTest(false);
         while (isTest() && isEnabled()) {
           try {
             WPIUtilJNI.waitForObject(event);
@@ -78,9 +83,7 @@ public class EducationalRobot extends RobotBase {
           }
         }
       } else {
-        modeThread.inTeleop(true);
         teleop();
-        modeThread.inTeleop(false);
         while (isTeleopEnabled()) {
           try {
             WPIUtilJNI.waitForObject(event);

wpilibjExamples/src/main/java/org/wpilib/templates/templates.json

@@ -22,6 +22,17 @@
     "mainclass": "Main",
     "commandversion": 2
   },
+  {
+    "name": "OpMode Robot",
+    "description": "OpMode style, with explanatory comments and example code.",
+    "tags": [
+      "OpMode"
+    ],
+    "foldername": "opmode",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
   {
     "name": "Timed Robot",
     "description": "Timed style, with explanatory comments and example code.",

wpilibjExamples/src/main/java/org/wpilib/templates/xrpeducational/EducationalRobot.java

@@ -6,7 +6,9 @@ package org.wpilib.templates.xrpeducational;
 
 import org.wpilib.driverstation.DriverStation;
 import org.wpilib.framework.RobotBase;
+import org.wpilib.hardware.hal.ControlWord;
 import org.wpilib.hardware.hal.DriverStationJNI;
+import org.wpilib.hardware.hal.RobotMode;
 import org.wpilib.internal.DriverStationModeThread;
 import org.wpilib.util.WPIUtilJNI;
 
@@ -34,7 +36,14 @@ public class EducationalRobot extends RobotBase {
 
   @Override
   public void startCompetition() {
-    DriverStationModeThread modeThread = new DriverStationModeThread();
+    // Create an opmode per robot mode
+    DriverStation.addOpMode(RobotMode.AUTONOMOUS, "Auto");
+    DriverStation.addOpMode(RobotMode.TELEOPERATED, "Teleop");
+    DriverStation.addOpMode(RobotMode.TEST, "Test");
+    DriverStation.publishOpModes();
+
+    final ControlWord word = new ControlWord();
+    DriverStationModeThread modeThread = new DriverStationModeThread(word);
 
     int event = WPIUtilJNI.createEvent(false, false);
 
@@ -44,10 +53,10 @@ public class EducationalRobot extends RobotBase {
     DriverStationJNI.observeUserProgramStarting();
 
     while (!Thread.currentThread().isInterrupted() && !m_exit) {
+      DriverStation.refreshControlWordFromCache(word);
+      modeThread.inControl(word);
       if (isDisabled()) {
-        modeThread.inDisabled(true);
         disabled();
-        modeThread.inDisabled(false);
         while (isDisabled()) {
           try {
             WPIUtilJNI.waitForObject(event);
@@ -56,9 +65,7 @@ public class EducationalRobot extends RobotBase {
           }
         }
       } else if (isAutonomous()) {
-        modeThread.inAutonomous(true);
         autonomous();
-        modeThread.inAutonomous(false);
         while (isAutonomousEnabled()) {
           try {
             WPIUtilJNI.waitForObject(event);
@@ -67,9 +74,7 @@ public class EducationalRobot extends RobotBase {
           }
         }
       } else if (isTest()) {
-        modeThread.inTest(true);
         test();
-        modeThread.inTest(false);
         while (isTest() && isEnabled()) {
           try {
             WPIUtilJNI.waitForObject(event);
@@ -78,9 +83,7 @@ public class EducationalRobot extends RobotBase {
           }
         }
       } else {
-        modeThread.inTeleop(true);
         teleop();
-        modeThread.inTeleop(false);
         while (isTeleopEnabled()) {
           try {
             WPIUtilJNI.waitForObject(event);