[wpilib] Only read DS control word once in IterativeRobotBase (#3504)

wpilibc/src/main/native/cpp/DSControlWord.cpp

@@ -0,0 +1,55 @@
+// 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.
+
+#include "frc/DSControlWord.h"
+
+#include <hal/DriverStation.h>
+
+using namespace frc;
+
+DSControlWord::DSControlWord() {
+  HAL_GetControlWord(&m_controlWord);
+}
+
+bool DSControlWord::IsEnabled() const {
+  return m_controlWord.enabled && m_controlWord.dsAttached;
+}
+
+bool DSControlWord::IsDisabled() const {
+  return !(m_controlWord.enabled && m_controlWord.dsAttached);
+}
+
+bool DSControlWord::IsEStopped() const {
+  return m_controlWord.eStop;
+}
+
+bool DSControlWord::IsAutonomous() const {
+  return m_controlWord.autonomous;
+}
+
+bool DSControlWord::IsAutonomousEnabled() const {
+  return m_controlWord.autonomous && m_controlWord.enabled &&
+         m_controlWord.dsAttached;
+}
+
+bool DSControlWord::IsTeleop() const {
+  return !(m_controlWord.autonomous || m_controlWord.test);
+}
+
+bool DSControlWord::IsTeleopEnabled() const {
+  return !m_controlWord.autonomous && !m_controlWord.test &&
+         m_controlWord.enabled && m_controlWord.dsAttached;
+}
+
+bool DSControlWord::IsTest() const {
+  return m_controlWord.test;
+}
+
+bool DSControlWord::IsDSAttached() const {
+  return m_controlWord.dsAttached;
+}
+
+bool DSControlWord::IsFMSAttached() const {
+  return m_controlWord.fmsAttached;
+}

wpilibc/src/main/native/cpp/IterativeRobotBase.cpp

@@ -8,6 +8,7 @@
 #include <hal/DriverStation.h>
 #include <networktables/NetworkTableInstance.h>
 
+#include "frc/DSControlWord.h"
 #include "frc/Errors.h"
 #include "frc/livewindow/LiveWindow.h"
 #include "frc/shuffleboard/Shuffleboard.h"
@@ -102,14 +103,15 @@ void IterativeRobotBase::LoopFunc() {
   m_watchdog.Reset();
 
   // Get current mode
+  DSControlWord word;
   Mode mode = Mode::kNone;
-  if (IsDisabled()) {
+  if (word.IsDisabled()) {
     mode = Mode::kDisabled;
-  } else if (IsAutonomous()) {
+  } else if (word.IsAutonomous()) {
     mode = Mode::kAutonomous;
-  } else if (IsOperatorControl()) {
+  } else if (word.IsTeleop()) {
     mode = Mode::kTeleop;
-  } else if (IsTest()) {
+  } else if (word.IsTest()) {
     mode = Mode::kTest;
   }
 

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

@@ -0,0 +1,102 @@
+// 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.
+
+#pragma once
+
+#include <hal/DriverStationTypes.h>
+
+namespace frc {
+
+/**
+ * A wrapper around Driver Station control word.
+ */
+class DSControlWord {
+ public:
+  /**
+   * DSControlWord constructor.
+   *
+   * Upon construction, the current Driver Station control word is read and
+   * stored internally.
+   */
+  DSControlWord();
+
+  /**
+   * Check if the DS has enabled the robot.
+   *
+   * @return True if the robot is enabled and the DS is connected
+   */
+  bool IsEnabled() const;
+
+  /**
+   * Check if the robot is disabled.
+   *
+   * @return True if the robot is explicitly disabled or the DS is not connected
+   */
+  bool IsDisabled() const;
+
+  /**
+   * Check if the robot is e-stopped.
+   *
+   * @return True if the robot is e-stopped
+   */
+  bool IsEStopped() const;
+
+  /**
+   * Check if the DS is commanding autonomous mode.
+   *
+   * @return True if the robot is being commanded to be in autonomous mode
+   */
+  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.
+   *
+   * @return True if the robot is being commanded to be in teleop mode
+   */
+  bool IsTeleop() 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 IsTeleopEnabled() const;
+
+  /**
+   * Check if the DS is commanding test mode.
+   *
+   * @return True if the robot is being commanded to be in test mode
+   */
+  bool IsTest() const;
+
+  /**
+   * Check if the DS is attached.
+   *
+   * @return True if the DS is connected to the robot
+   */
+  bool IsDSAttached() const;
+
+  /**
+   * Is 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
+   */
+  bool IsFMSAttached() const;
+
+ private:
+  HAL_ControlWord m_controlWord;
+};
+
+}  // namespace frc

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

@@ -0,0 +1,127 @@
+// 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 edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.ControlWord;
+import edu.wpi.first.hal.HAL;
+
+/** 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() {
+    update();
+  }
+
+  /** Update internal Driver Station control word. */
+  public void update() {
+    HAL.getControlWord(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();
+  }
+}

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

@@ -61,6 +61,7 @@ public abstract class IterativeRobotBase extends RobotBase {
     kTest
   }
 
+  private final DSControlWord m_word = new DSControlWord();
   private Mode m_lastMode = Mode.kNone;
   private final double m_period;
   private final Watchdog m_watchdog;
@@ -258,14 +259,15 @@ public abstract class IterativeRobotBase extends RobotBase {
     m_watchdog.reset();
 
     // Get current mode
+    m_word.update();
     Mode mode = Mode.kNone;
-    if (isDisabled()) {
+    if (m_word.isDisabled()) {
       mode = Mode.kDisabled;
-    } else if (isAutonomous()) {
+    } else if (m_word.isAutonomous()) {
       mode = Mode.kAutonomous;
-    } else if (isOperatorControl()) {
+    } else if (m_word.isTeleop()) {
       mode = Mode.kTeleop;
-    } else if (isTest()) {
+    } else if (m_word.isTest()) {
       mode = Mode.kTest;
     }