Remove D-pad test code in favor of edu.wpi.first.wpilibj2.command.button.POVButton

Update DriverStation overload to include 2022.4.1 changes
Add windows support to PathPlanner task

.vscode/tasks.json

@@ -21,6 +21,9 @@
     {
       "label": "pathPlanner",
       "type": "shell",
+      "windows": {
+        "command": "explorer.exe shell:AppsFolder\\39956MichaelJansen.FRCPathPlanner_x7a2cjep50mda!pathplanner"
+      },
       "osx": {
         "command": "open -n /Applications/PathPlanner.app"
       },
@@ -31,7 +34,8 @@
         "panel": "dedicated",
         "showReuseMessage": false,
         "clear": true
-      }
+      },
+      "problemMatcher": []
     }
   ]
 }

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

@@ -11,6 +11,12 @@ import edu.wpi.first.hal.MatchInfoData;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.util.WPIUtilJNI;
+import edu.wpi.first.util.datalog.BooleanArrayLogEntry;
+import edu.wpi.first.util.datalog.BooleanLogEntry;
+import edu.wpi.first.util.datalog.DataLog;
+import edu.wpi.first.util.datalog.FloatArrayLogEntry;
+import edu.wpi.first.util.datalog.IntegerArrayLogEntry;
 import java.nio.ByteBuffer;
 import java.util.concurrent.TimeUnit;
 import java.util.concurrent.locks.Condition;
@@ -235,6 +241,163 @@ public class DriverStation {
     }
   }
 
+  private static class JoystickLogSender {
+    JoystickLogSender(DataLog log, int stick, long timestamp) {
+      m_stick = stick;
+
+      m_logButtons = new BooleanArrayLogEntry(log, "DS:joystick" + stick + "/buttons", timestamp);
+      m_logAxes = new FloatArrayLogEntry(log, "DS:joystick" + stick + "/axes", timestamp);
+      m_logPOVs = new IntegerArrayLogEntry(log, "DS:joystick" + stick + "/povs", timestamp);
+
+      appendButtons(timestamp);
+      appendAxes(timestamp);
+      appendPOVs(timestamp);
+    }
+
+    public void send(long timestamp) {
+      if (m_joystickButtonsCache[m_stick].m_count != m_joystickButtons[m_stick].m_count
+          || m_joystickButtonsCache[m_stick].m_buttons != m_joystickButtons[m_stick].m_buttons) {
+        appendButtons(timestamp);
+      }
+
+      if (m_joystickAxesCache[m_stick].m_count != m_joystickAxes[m_stick].m_count) {
+        appendAxes(timestamp);
+      } else {
+        int count = m_joystickAxesCache[m_stick].m_count;
+        for (int i = 0; i < count; i++) {
+          if (m_joystickAxesCache[m_stick].m_axes[i] != m_joystickAxes[m_stick].m_axes[i]) {
+            appendAxes(timestamp);
+            break;
+          }
+        }
+      }
+
+      if (m_joystickPOVsCache[m_stick].m_count != m_joystickPOVs[m_stick].m_count) {
+        appendPOVs(timestamp);
+      } else {
+        int count = m_joystickPOVsCache[m_stick].m_count;
+        for (int i = 0; i < count; i++) {
+          if (m_joystickPOVsCache[m_stick].m_povs[i] != m_joystickPOVs[m_stick].m_povs[i]) {
+            appendPOVs(timestamp);
+            break;
+          }
+        }
+      }
+    }
+
+    void appendButtons(long timestamp) {
+      int count = m_joystickButtonsCache[m_stick].m_count;
+      if (m_sizedButtons == null || m_sizedButtons.length != count) {
+        m_sizedButtons = new boolean[count];
+      }
+      int buttons = m_joystickButtonsCache[m_stick].m_buttons;
+      for (int i = 0; i < count; i++) {
+        m_sizedButtons[i] = (buttons & (1 << i)) != 0;
+      }
+      m_logButtons.append(m_sizedButtons, timestamp);
+    }
+
+    void appendAxes(long timestamp) {
+      int count = m_joystickAxesCache[m_stick].m_count;
+      if (m_sizedAxes == null || m_sizedAxes.length != count) {
+        m_sizedAxes = new float[count];
+      }
+      System.arraycopy(m_joystickAxesCache[m_stick].m_axes, 0, m_sizedAxes, 0, count);
+      m_logAxes.append(m_sizedAxes, timestamp);
+    }
+
+    void appendPOVs(long timestamp) {
+      int count = m_joystickPOVsCache[m_stick].m_count;
+      if (m_sizedPOVs == null || m_sizedPOVs.length != count) {
+        m_sizedPOVs = new long[count];
+      }
+      for (int i = 0; i < count; i++) {
+        m_sizedPOVs[i] = m_joystickPOVsCache[m_stick].m_povs[i];
+      }
+      m_logPOVs.append(m_sizedPOVs, timestamp);
+    }
+
+    final int m_stick;
+    boolean[] m_sizedButtons;
+    float[] m_sizedAxes;
+    long[] m_sizedPOVs;
+    final BooleanArrayLogEntry m_logButtons;
+    final FloatArrayLogEntry m_logAxes;
+    final IntegerArrayLogEntry m_logPOVs;
+  }
+
+  private static class DataLogSender {
+    DataLogSender(DataLog log, boolean logJoysticks, long timestamp) {
+      m_logEnabled = new BooleanLogEntry(log, "DS:enabled", timestamp);
+      m_logAutonomous = new BooleanLogEntry(log, "DS:autonomous", timestamp);
+      m_logTest = new BooleanLogEntry(log, "DS:test", timestamp);
+      m_logEstop = new BooleanLogEntry(log, "DS:estop", timestamp);
+
+      // append initial control word values
+      m_wasEnabled = m_controlWordCache.getEnabled();
+      m_wasAutonomous = m_controlWordCache.getAutonomous();
+      m_wasTest = m_controlWordCache.getTest();
+      m_wasEstop = m_controlWordCache.getEStop();
+
+      m_logEnabled.append(m_wasEnabled, timestamp);
+      m_logAutonomous.append(m_wasAutonomous, timestamp);
+      m_logTest.append(m_wasTest, timestamp);
+      m_logEstop.append(m_wasEstop, timestamp);
+
+      if (logJoysticks) {
+        m_joysticks = new JoystickLogSender[kJoystickPorts];
+        for (int i = 0; i < kJoystickPorts; i++) {
+          m_joysticks[i] = new JoystickLogSender(log, i, timestamp);
+        }
+      } else {
+        m_joysticks = new JoystickLogSender[0];
+      }
+    }
+
+    public void send(long timestamp) {
+      // append control word value changes
+      boolean enabled = m_controlWordCache.getEnabled();
+      if (enabled != m_wasEnabled) {
+        m_logEnabled.append(enabled, timestamp);
+      }
+      m_wasEnabled = enabled;
+
+      boolean autonomous = m_controlWordCache.getAutonomous();
+      if (autonomous != m_wasAutonomous) {
+        m_logAutonomous.append(autonomous, timestamp);
+      }
+      m_wasAutonomous = autonomous;
+
+      boolean test = m_controlWordCache.getTest();
+      if (test != m_wasTest) {
+        m_logTest.append(test, timestamp);
+      }
+      m_wasTest = test;
+
+      boolean estop = m_controlWordCache.getEStop();
+      if (estop != m_wasEstop) {
+        m_logEstop.append(estop, timestamp);
+      }
+      m_wasEstop = estop;
+
+      // append joystick value changes
+      for (JoystickLogSender joystick : m_joysticks) {
+        joystick.send(timestamp);
+      }
+    }
+
+    boolean m_wasEnabled;
+    boolean m_wasAutonomous;
+    boolean m_wasTest;
+    boolean m_wasEstop;
+    final BooleanLogEntry m_logEnabled;
+    final BooleanLogEntry m_logAutonomous;
+    final BooleanLogEntry m_logTest;
+    final BooleanLogEntry m_logEstop;
+
+    final JoystickLogSender[] m_joysticks;
+  }
+
   private static DriverStation instance = new DriverStation();
 
   // Joystick User Data
@@ -258,6 +421,7 @@ public class DriverStation {
   private static final ByteBuffer m_buttonCountBuffer = ByteBuffer.allocateDirect(1);
 
   private static final MatchDataSender m_matchDataSender;
+  private static DataLogSender m_dataLogSender;
 
   // Internal Driver Station thread
   private static Thread m_thread;
@@ -399,28 +563,28 @@ public class DriverStation {
       boolean printTrace,
       StackTraceElement[] stackTrace,
       int stackTraceFirst) {
-    // String locString;
+    /*
-    // if (stackTrace.length >= stackTraceFirst + 1) {
+    String locString;
-    //   locString = stackTrace[stackTraceFirst].toString();
+    if (stackTrace.length >= stackTraceFirst + 1) {
-    // } else {
+      locString = stackTrace[stackTraceFirst].toString();
-    //   locString = "";
+    } else {
-    // }
+      locString = "";
-    // StringBuilder traceString = new StringBuilder();
+    }
-    // if (printTrace) {
+    StringBuilder traceString = new StringBuilder();
-    //   boolean haveLoc = false;
+    if (printTrace) {
-    //   for (int i = stackTraceFirst; i < stackTrace.length; i++) {
+      boolean haveLoc = false;
-    //     String loc = stackTrace[i].toString();
+      for (int i = stackTraceFirst; i < stackTrace.length; i++) {
-    //     traceString.append("\tat ").append(loc).append('\n');
+        String loc = stackTrace[i].toString();
-    //     // get first user function
+        traceString.append("\tat ").append(loc).append('\n');
-    //     if (!haveLoc && !loc.startsWith("edu.wpi.first")) {
+        // get first user function
-    //       locString = loc;
+        if (!haveLoc && !loc.startsWith("edu.wpi.first")) {
-    //       haveLoc = true;
+          locString = loc;
-    //     }
+          haveLoc = true;
-    //   }
+        }
-    // }
+      }
-    // HAL.sendError(isError, code, false, error, locString, traceString.toString(), true);
+    }
-    // for (StackTraceElement traceElement : stackTrace)
+    HAL.sendError(isError, code, false, error, locString, traceString.toString(), true);
-    //   System.err.println("\tat " + traceElement);
+    */
     java.util.logging.LogRecord logRecord = new java.util.logging.LogRecord(isError ? java.util.logging.Level.SEVERE : java.util.logging.Level.FINER, error.stripTrailing());
     java.util.Optional.ofNullable(stackTrace).filter(s -> s.length >= stackTraceFirst + 1).map(s -> java.util.Arrays.copyOfRange(s, Math.min(Math.max(0, stackTraceFirst), s.length - 1), s.length - 1)).ifPresent(presentStackTrace -> {
       logRecord.setSourceMethodName(presentStackTrace[0].getMethodName());
@@ -1279,6 +1443,7 @@ public class DriverStation {
       m_controlWordMutex.unlock();
     }
 
+    DataLogSender dataLogSender;
     // lock joystick mutex to swap cache data
     m_cacheDataMutex.lock();
     try {
@@ -1308,12 +1473,17 @@ public class DriverStation {
       MatchInfoData currentInfo = m_matchInfo;
       m_matchInfo = m_matchInfoCache;
       m_matchInfoCache = currentInfo;
+
+      dataLogSender = m_dataLogSender;
     } finally {
       m_cacheDataMutex.unlock();
     }
 
     wakeupWaitForData();
     m_matchDataSender.sendMatchData();
+    if (dataLogSender != null) {
+      dataLogSender.send(WPIUtilJNI.now());
+    }
   }
 
   /**
@@ -1403,4 +1573,32 @@ public class DriverStation {
       m_lastControlWordUpdate = now;
     }
   }
+  
+  /**
+   * Starts logging DriverStation data to data log. Repeated calls are ignored.
+   *
+   * @param log data log
+   * @param logJoysticks if true, log joystick data
+   */
+  @SuppressWarnings("PMD.NonThreadSafeSingleton")
+  public static void startDataLog(DataLog log, boolean logJoysticks) {
+    m_cacheDataMutex.lock();
+    try {
+      if (m_dataLogSender == null) {
+        m_dataLogSender = new DataLogSender(log, logJoysticks, WPIUtilJNI.now());
+      }
+    } finally {
+      m_cacheDataMutex.unlock();
+    }
+  }
+
+  /**
+   * Starts logging DriverStation data to data log, including joystick data. Repeated calls are
+   * ignored.
+   *
+   * @param log data log
+   */
+  public static void startDataLog(DataLog log) {
+    startDataLog(log, true);
+  }
 }

src/main/java/frc4388/utility/controller/DeadbandedXboxController.java

@@ -13,27 +13,6 @@ public class DeadbandedXboxController extends XboxController {
   @Override public double getLeftY() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()).getY(); }
   @Override public double getRightX() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getX(); }
   @Override public double getRightY() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getY(); }
-
-  public int getDpadAngle() {
-    return getPOV(0);
-  }
-
-  public boolean getDPadLeft(){
-    return (getRawAxis(6) < -0.9);
-  }
-
-  public boolean getDPadRight(){
-    return (getRawAxis(6) > 0.9);
-  }
-
-  public boolean getDPadTop(){
-    return (getRawAxis(6) < 0.9);
-  }
-
-  public boolean getDPadBottom(){
-    return (getRawAxis(6) > -0.9);
-  }
-
   public static Translation2d skewToDeadzonedCircle(double x, double y) {
     Translation2d translation2d = new Translation2d(x, y);
     double magnitude = translation2d.getNorm();