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