[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