[wpilib] Add methods to check game and enabled state together (#2661)

This avoids users having to call both IsOperatorControl() and IsEnabled() to figure out if their robot is
enabled and in the teleop state. The expression above involves calling two methods that each have their
own lock.

These new methods should only involve locking one mutex, since only one call is made to HAL_GetControlWord().

wpilibc/src/main/native/cpp/DriverStation.cpp

@@ -358,12 +358,24 @@ bool DriverStation::IsAutonomous() const {
   return controlWord.autonomous;
 }
 
+bool DriverStation::IsAutonomousEnabled() const {
+  HAL_ControlWord controlWord;
+  HAL_GetControlWord(&controlWord);
+  return controlWord.autonomous && controlWord.enabled;
+}
+
 bool DriverStation::IsOperatorControl() const {
   HAL_ControlWord controlWord;
   HAL_GetControlWord(&controlWord);
   return !(controlWord.autonomous || controlWord.test);
 }
 
+bool DriverStation::IsOperatorControlEnabled() const {
+  HAL_ControlWord controlWord;
+  HAL_GetControlWord(&controlWord);
+  return !controlWord.autonomous && !controlWord.test && controlWord.enabled;
+}
+
 bool DriverStation::IsTest() const {
   HAL_ControlWord controlWord;
   HAL_GetControlWord(&controlWord);

wpilibc/src/main/native/cppcs/RobotBase.cpp

@@ -154,8 +154,16 @@ bool RobotBase::IsDisabled() const { return m_ds.IsDisabled(); }
 
 bool RobotBase::IsAutonomous() const { return m_ds.IsAutonomous(); }
 
+bool RobotBase::IsAutonomousEnabled() const {
+  return m_ds.IsAutonomousEnabled();
+}
+
 bool RobotBase::IsOperatorControl() const { return m_ds.IsOperatorControl(); }
 
+bool RobotBase::IsOperatorControlEnabled() const {
+  return m_ds.IsOperatorControlEnabled();
+}
+
 bool RobotBase::IsTest() const { return m_ds.IsTest(); }
 
 bool RobotBase::IsNewDataAvailable() const { return m_ds.IsNewControlData(); }

wpilibc/src/main/native/include/frc/DriverStation.h

@@ -211,6 +211,15 @@ class DriverStation : public ErrorBase {
    */
   bool IsAutonomous() const;
 
+  /**
+   * Check if the DS is commanding autonomous mode and if it has enabled the
+   * robot.
+   *
+   * @return True if the robot is being commanded to be in autonomous mode and
+   * enabled.
+   */
+  bool IsAutonomousEnabled() const;
+
   /**
    * Check if the DS is commanding teleop mode.
    *
@@ -218,6 +227,14 @@ class DriverStation : public ErrorBase {
    */
   bool IsOperatorControl() const;
 
+  /**
+   * Check if the DS is commanding teleop mode and if it has enabled the robot.
+   *
+   * @return True if the robot is being commanded to be in teleop mode and
+   * enabled.
+   */
+  bool IsOperatorControlEnabled() const;
+
   /**
    * Check if the DS is commanding test mode.
    *

wpilibc/src/main/native/include/frc/RobotBase.h

@@ -133,6 +133,14 @@ class RobotBase {
    */
   bool IsAutonomous() const;
 
+  /**
+   * Determine if the robot is currently in Autonomous mode and enabled.
+   *
+   * @return True if the robot us currently operating Autonomously while enabled
+   * as determined by the field controls.
+   */
+  bool IsAutonomousEnabled() const;
+
   /**
    * Determine if the robot is currently in Operator Control mode.
    *
@@ -141,6 +149,14 @@ class RobotBase {
    */
   bool IsOperatorControl() const;
 
+  /**
+   * Determine if the robot is current in Operator Control mode and enabled.
+   *
+   * @return True if the robot is currently operating in Tele-Op mode while
+   * wnabled as determined by the field-controls.
+   */
+  bool IsOperatorControlEnabled() const;
+
   /**
    * Determine if the robot is currently in Test mode.
    *

wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -41,7 +41,7 @@ void Robot::StartCompetition() {
       m_ds.InAutonomous(true);
       Autonomous();
       m_ds.InAutonomous(false);
-      while (IsAutonomous() && IsEnabled()) m_ds.WaitForData();
+      while (IsAutonomousEnabled()) m_ds.WaitForData();
     } else if (IsTest()) {
       lw.SetEnabled(true);
       frc::Shuffleboard::EnableActuatorWidgets();
@@ -55,7 +55,7 @@ void Robot::StartCompetition() {
       m_ds.InOperatorControl(true);
       Teleop();
       m_ds.InOperatorControl(false);
-      while (IsOperatorControl() && IsEnabled()) m_ds.WaitForData();
+      while (IsOperatorControlEnabled()) m_ds.WaitForData();
     }
   }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java

@@ -641,6 +641,19 @@ public class DriverStation {
     }
   }
 
+  /**
+   * 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() {
+    synchronized (m_controlWordMutex) {
+      updateControlWord(false);
+      return m_controlWordCache.getAutonomous() && m_controlWordCache.getEnabled();
+    }
+  }
+
   /**
    * Gets a value indicating whether the Driver Station requires the robot to be running in
    * operator-controlled mode.
@@ -651,6 +664,20 @@ public class DriverStation {
     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 isOperatorControlEnabled() {
+    synchronized (m_controlWordMutex) {
+      updateControlWord(false);
+      return !m_controlWordCache.getAutonomous() && !m_controlWordCache.getTest()
+          && m_controlWordCache.getEnabled();
+    }
+  }
+
   /**
    * Gets a value indicating whether the Driver Station requires the robot to be running in test
    * mode.

wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java

@@ -211,6 +211,16 @@ public abstract class RobotBase implements AutoCloseable {
     return m_ds.isAutonomous();
   }
 
+  /**
+   * Determine if the robot is current in Autonomous mode and enabled as determined by
+   * the field controls.
+   *
+   * @return True if the robot is currently operating autonomously while enabled.
+   */
+  public boolean isAutonomousEnabled() {
+    return m_ds.isAutonomousEnabled();
+  }
+
   /**
    * Determine if the robot is currently in Test mode as determined by the driver
    * station.
@@ -231,6 +241,16 @@ public abstract class RobotBase implements AutoCloseable {
     return m_ds.isOperatorControl();
   }
 
+  /**
+   * Determine if the robot is current in Operator Control mode and enabled as determined by
+   * the field controls.
+   *
+   * @return True if the robot is currently operating in Tele-Op mode while enabled.
+   */
+  public boolean isOperatorControlEnabled() {
+    return m_ds.isOperatorControlEnabled();
+  }
+
   /**
    * Indicates if new data is available from the driver station.
    *

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -55,7 +55,7 @@ public class Robot extends RobotBase {
         m_ds.InAutonomous(true);
         autonomous();
         m_ds.InAutonomous(false);
-        while (isAutonomous() && !isDisabled()) {
+        while (isAutonomousEnabled()) {
           m_ds.waitForData();
         }
       } else if (isTest()) {
@@ -73,7 +73,7 @@ public class Robot extends RobotBase {
         m_ds.InOperatorControl(true);
         teleop();
         m_ds.InOperatorControl(false);
-        while (isOperatorControl() && !isDisabled()) {
+        while (isOperatorControlEnabled()) {
           m_ds.waitForData();
         }
       }