From fa1ceca83ad0d164f8205bace24aed6b16d92c96 Mon Sep 17 00:00:00 2001
From: Thad House <ThadHouse@users.noreply.github.com>
Date: Mon, 29 Nov 2021 20:56:58 -0800
Subject: [PATCH] [wpilibj] Use DS cache for iterative robot control word cache
 (#3748)

The root cause of #3747 is CommandScheduler's ds state checks are behind iterative robots checks. This means that the iterative robot state could return enabled, but the DS cache could still be reporting disabled. This results in a race in the Disabled -> Enabled transition, which manifests in commands not running.

Previously, iterative robot base pulled from the DS cache. This meant that the ds cache was always updated before an iterative robot base loop could run. This still had a race, but this could only occur on the Enabled -> Disable transition, which is much less noticeable and would usually just result in a command running for an extra loop.

We can move back to the old behavior by grabbing the new iterative robot base check variables to use the DS cache.
---
 .../main/java/edu/wpi/first/hal/ControlWord.java   | 14 ++++++++++++++
 .../java/edu/wpi/first/wpilibj/DSControlWord.java  |  3 +--
 .../java/edu/wpi/first/wpilibj/DriverStation.java  | 12 ++++++++++++
 3 files changed, 27 insertions(+), 2 deletions(-)

diff --git a/hal/src/main/java/edu/wpi/first/hal/ControlWord.java b/hal/src/main/java/edu/wpi/first/hal/ControlWord.java
index a56834369a..7dff0f4635 100644
--- a/hal/src/main/java/edu/wpi/first/hal/ControlWord.java
+++ b/hal/src/main/java/edu/wpi/first/hal/ControlWord.java
@@ -28,6 +28,20 @@ public class ControlWord {
     m_dsAttached = dsAttached;
   }
 
+  /**
+   * Updates from an existing word.
+   *
+   * @param word word to update from
+   */
+  public void update(ControlWord word) {
+    m_enabled = word.m_enabled;
+    m_autonomous = word.m_autonomous;
+    m_test = word.m_test;
+    m_emergencyStop = word.m_emergencyStop;
+    m_fmsAttached = word.m_fmsAttached;
+    m_dsAttached = word.m_dsAttached;
+  }
+
   public boolean getEnabled() {
     return m_enabled;
   }
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DSControlWord.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DSControlWord.java
index 016abc7ca3..37e046df0f 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DSControlWord.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DSControlWord.java
@@ -5,7 +5,6 @@
 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 {
@@ -22,7 +21,7 @@ public class DSControlWord {
 
   /** Update internal Driver Station control word. */
   public void update() {
-    HAL.getControlWord(m_controlWord);
+    DriverStation.updateControlWordFromCache(m_controlWord);
   }
 
   /**
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java
index c450c5e949..7400f00968 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java
@@ -1324,6 +1324,18 @@ public class DriverStation {
     }
   }
 
+  /**
+   * Forces a control word cache update, and update the passed in control word.
+   *
+   * @param word Word to update.
+   */
+  public static void updateControlWordFromCache(ControlWord word) {
+    synchronized (m_controlWordMutex) {
+      updateControlWord(true);
+      word.update(m_controlWordCache);
+    }
+  }
+
   /**
    * Updates the data in the control word cache. Updates if the force parameter is set, or if 50ms
    * have passed since the last update.