001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.wpilibj;
006
007import edu.wpi.first.datalog.BooleanArrayLogEntry;
008import edu.wpi.first.datalog.BooleanLogEntry;
009import edu.wpi.first.datalog.DataLog;
010import edu.wpi.first.datalog.FloatArrayLogEntry;
011import edu.wpi.first.datalog.IntegerArrayLogEntry;
012import edu.wpi.first.hal.AllianceStationID;
013import edu.wpi.first.hal.ControlWord;
014import edu.wpi.first.hal.DriverStationJNI;
015import edu.wpi.first.hal.HAL;
016import edu.wpi.first.hal.MatchInfoData;
017import edu.wpi.first.math.geometry.Rotation2d;
018import edu.wpi.first.networktables.BooleanPublisher;
019import edu.wpi.first.networktables.IntegerPublisher;
020import edu.wpi.first.networktables.NetworkTableInstance;
021import edu.wpi.first.networktables.StringPublisher;
022import edu.wpi.first.networktables.StringTopic;
023import edu.wpi.first.util.EventVector;
024import edu.wpi.first.util.WPIUtilJNI;
025import java.nio.ByteBuffer;
026import java.util.Map;
027import java.util.Optional;
028import java.util.OptionalInt;
029import java.util.concurrent.locks.ReentrantLock;
030
031/** Provide access to the network communication data to / from the Driver Station. */
032public final class DriverStation {
033  /** Number of Joystick ports. */
034  public static final int kJoystickPorts = 6;
035
036  private static final class HALJoystickButtons {
037    public int m_buttons;
038    public byte m_count;
039  }
040
041  private static class HALJoystickAxes {
042    public final float[] m_axes;
043    public int m_count;
044
045    HALJoystickAxes(int count) {
046      m_axes = new float[count];
047    }
048  }
049
050  private static class HALJoystickAxesRaw {
051    public final short[] m_axes;
052
053    @SuppressWarnings("unused")
054    public int m_count;
055
056    HALJoystickAxesRaw(int count) {
057      m_axes = new short[count];
058    }
059  }
060
061  private static class HALJoystickPOVs {
062    public final byte[] m_povs;
063    public int m_count;
064
065    HALJoystickPOVs(int count) {
066      m_povs = new byte[count];
067      for (int i = 0; i < count; i++) {
068        m_povs[i] = 0;
069      }
070    }
071  }
072
073  /** The robot alliance that the robot is a part of. */
074  public enum Alliance {
075    /** Red alliance. */
076    Red,
077    /** Blue alliance. */
078    Blue
079  }
080
081  /** The type of robot match that the robot is part of. */
082  public enum MatchType {
083    /** None. */
084    None,
085    /** Practice. */
086    Practice,
087    /** Qualification. */
088    Qualification,
089    /** Elimination. */
090    Elimination
091  }
092
093  /** A controller POV direction. */
094  public enum POVDirection {
095    /** POV center. */
096    Center(0x00),
097    /** POV up. */
098    Up(0x01),
099    /** POV up right. */
100    UpRight(0x01 | 0x02),
101    /** POV right. */
102    Right(0x02),
103    /** POV down right. */
104    DownRight(0x02 | 0x04),
105    /** POV down. */
106    Down(0x04),
107    /** POV down left. */
108    DownLeft(0x04 | 0x08),
109    /** POV left. */
110    Left(0x08),
111    /** POV up left. */
112    UpLeft(0x01 | 0x08);
113
114    private static final double INVALID_POV_VALUE_INTERVAL = 1.0;
115    private static double s_nextMessageTime;
116
117    /**
118     * Converts a byte value into a POVDirection enum value.
119     *
120     * @param value The byte value to convert.
121     * @return The corresponding POVDirection enum value.
122     * @throws IllegalArgumentException If value does not correspond to a POVDirection.
123     */
124    private static POVDirection of(byte value) {
125      for (var direction : values()) {
126        if (direction.value == value) {
127          return direction;
128        }
129      }
130      double currentTime = Timer.getTimestamp();
131      if (currentTime > s_nextMessageTime) {
132        reportError("Invalid POV value " + value + "!", false);
133        s_nextMessageTime = currentTime + INVALID_POV_VALUE_INTERVAL;
134      }
135      return Center;
136    }
137
138    /** The corresponding HAL value. */
139    public final byte value;
140
141    POVDirection(int value) {
142      this.value = (byte) value;
143    }
144
145    /**
146     * Gets the angle of a POVDirection.
147     *
148     * @return The angle clockwise from straight up, or Optional.empty() if this POVDirection is
149     *     Center.
150     */
151    public Optional<Rotation2d> getAngle() {
152      return switch (this) {
153        case Center -> Optional.empty();
154        case Up -> Optional.of(Rotation2d.fromDegrees(0));
155        case UpRight -> Optional.of(Rotation2d.fromDegrees(45));
156        case Right -> Optional.of(Rotation2d.fromDegrees(90));
157        case DownRight -> Optional.of(Rotation2d.fromDegrees(135));
158        case Down -> Optional.of(Rotation2d.fromDegrees(180));
159        case DownLeft -> Optional.of(Rotation2d.fromDegrees(225));
160        case Left -> Optional.of(Rotation2d.fromDegrees(270));
161        case UpLeft -> Optional.of(Rotation2d.fromDegrees(315));
162      };
163    }
164  }
165
166  private static final double JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL = 1.0;
167  private static double m_nextMessageTime;
168
169  @SuppressWarnings("MemberName")
170  private static class MatchDataSender {
171    private static final String kSmartDashboardType = "FMSInfo";
172
173    final StringPublisher gameSpecificMessage;
174    final StringPublisher eventName;
175    final IntegerPublisher matchNumber;
176    final IntegerPublisher replayNumber;
177    final IntegerPublisher matchType;
178    final BooleanPublisher alliance;
179    final IntegerPublisher station;
180    final IntegerPublisher controlWord;
181    boolean oldIsRedAlliance = true;
182    int oldStationNumber = 1;
183    String oldEventName = "";
184    String oldGameSpecificMessage = "";
185    int oldMatchNumber;
186    int oldReplayNumber;
187    int oldMatchType;
188    int oldControlWord;
189
190    MatchDataSender() {
191      var table = NetworkTableInstance.getDefault().getTable("FMSInfo");
192      table
193          .getStringTopic(".type")
194          .publishEx(
195              StringTopic.kTypeString, "{\"SmartDashboard\":\"" + kSmartDashboardType + "\"}")
196          .set(kSmartDashboardType);
197      gameSpecificMessage = table.getStringTopic("GameSpecificMessage").publish();
198      gameSpecificMessage.set("");
199      eventName = table.getStringTopic("EventName").publish();
200      eventName.set("");
201      matchNumber = table.getIntegerTopic("MatchNumber").publish();
202      matchNumber.set(0);
203      replayNumber = table.getIntegerTopic("ReplayNumber").publish();
204      replayNumber.set(0);
205      matchType = table.getIntegerTopic("MatchType").publish();
206      matchType.set(0);
207      alliance = table.getBooleanTopic("IsRedAlliance").publish();
208      alliance.set(true);
209      station = table.getIntegerTopic("StationNumber").publish();
210      station.set(1);
211      controlWord = table.getIntegerTopic("FMSControlData").publish();
212      controlWord.set(0);
213    }
214
215    private void sendMatchData() {
216      AllianceStationID allianceID = DriverStationJNI.getAllianceStation();
217      final int stationNumber =
218          switch (allianceID) {
219            case Blue1, Red1 -> 1;
220            case Blue2, Red2 -> 2;
221            case Blue3, Red3, Unknown -> 3;
222          };
223      final boolean isRedAlliance =
224          switch (allianceID) {
225            case Blue1, Blue2, Blue3 -> false;
226            case Red1, Red2, Red3, Unknown -> true;
227          };
228
229      String currentEventName;
230      String currentGameSpecificMessage;
231      int currentMatchNumber;
232      int currentReplayNumber;
233      int currentMatchType;
234      int currentControlWord;
235      m_cacheDataMutex.lock();
236      try {
237        currentEventName = DriverStation.m_matchInfo.eventName;
238        currentGameSpecificMessage = DriverStation.m_matchInfo.gameSpecificMessage;
239        currentMatchNumber = DriverStation.m_matchInfo.matchNumber;
240        currentReplayNumber = DriverStation.m_matchInfo.replayNumber;
241        currentMatchType = DriverStation.m_matchInfo.matchType;
242      } finally {
243        m_cacheDataMutex.unlock();
244      }
245      currentControlWord = DriverStationJNI.nativeGetControlWord();
246
247      if (oldIsRedAlliance != isRedAlliance) {
248        alliance.set(isRedAlliance);
249        oldIsRedAlliance = isRedAlliance;
250      }
251      if (oldStationNumber != stationNumber) {
252        station.set(stationNumber);
253        oldStationNumber = stationNumber;
254      }
255      if (!oldEventName.equals(currentEventName)) {
256        eventName.set(currentEventName);
257        oldEventName = currentEventName;
258      }
259      if (!oldGameSpecificMessage.equals(currentGameSpecificMessage)) {
260        gameSpecificMessage.set(currentGameSpecificMessage);
261        oldGameSpecificMessage = currentGameSpecificMessage;
262      }
263      if (currentMatchNumber != oldMatchNumber) {
264        matchNumber.set(currentMatchNumber);
265        oldMatchNumber = currentMatchNumber;
266      }
267      if (currentReplayNumber != oldReplayNumber) {
268        replayNumber.set(currentReplayNumber);
269        oldReplayNumber = currentReplayNumber;
270      }
271      if (currentMatchType != oldMatchType) {
272        matchType.set(currentMatchType);
273        oldMatchType = currentMatchType;
274      }
275      if (currentControlWord != oldControlWord) {
276        controlWord.set(currentControlWord);
277        oldControlWord = currentControlWord;
278      }
279    }
280  }
281
282  private static class JoystickLogSender {
283    JoystickLogSender(DataLog log, int stick, long timestamp) {
284      m_stick = stick;
285
286      m_logButtons = new BooleanArrayLogEntry(log, "DS:joystick" + stick + "/buttons", timestamp);
287      m_logAxes = new FloatArrayLogEntry(log, "DS:joystick" + stick + "/axes", timestamp);
288      m_logPOVs = new IntegerArrayLogEntry(log, "DS:joystick" + stick + "/povs", timestamp);
289
290      appendButtons(m_joystickButtons[m_stick], timestamp);
291      appendAxes(m_joystickAxes[m_stick], timestamp);
292      appendPOVs(m_joystickPOVs[m_stick], timestamp);
293    }
294
295    public void send(long timestamp) {
296      HALJoystickButtons buttons = m_joystickButtons[m_stick];
297      if (buttons.m_count != m_prevButtons.m_count
298          || buttons.m_buttons != m_prevButtons.m_buttons) {
299        appendButtons(buttons, timestamp);
300      }
301
302      HALJoystickAxes axes = m_joystickAxes[m_stick];
303      int count = axes.m_count;
304      boolean needToLog = false;
305      if (count != m_prevAxes.m_count) {
306        needToLog = true;
307      } else {
308        for (int i = 0; i < count; i++) {
309          if (axes.m_axes[i] != m_prevAxes.m_axes[i]) {
310            needToLog = true;
311            break;
312          }
313        }
314      }
315      if (needToLog) {
316        appendAxes(axes, timestamp);
317      }
318
319      HALJoystickPOVs povs = m_joystickPOVs[m_stick];
320      count = m_joystickPOVs[m_stick].m_count;
321      needToLog = false;
322      if (count != m_prevPOVs.m_count) {
323        needToLog = true;
324      } else {
325        for (int i = 0; i < count; i++) {
326          if (povs.m_povs[i] != m_prevPOVs.m_povs[i]) {
327            needToLog = true;
328            break;
329          }
330        }
331      }
332      if (needToLog) {
333        appendPOVs(povs, timestamp);
334      }
335    }
336
337    void appendButtons(HALJoystickButtons buttons, long timestamp) {
338      byte count = buttons.m_count;
339      if (m_sizedButtons == null || m_sizedButtons.length != count) {
340        m_sizedButtons = new boolean[count];
341      }
342      int buttonsValue = buttons.m_buttons;
343      for (int i = 0; i < count; i++) {
344        m_sizedButtons[i] = (buttonsValue & (1 << i)) != 0;
345      }
346      m_logButtons.append(m_sizedButtons, timestamp);
347      m_prevButtons.m_count = count;
348      m_prevButtons.m_buttons = buttons.m_buttons;
349    }
350
351    void appendAxes(HALJoystickAxes axes, long timestamp) {
352      int count = axes.m_count;
353      if (m_sizedAxes == null || m_sizedAxes.length != count) {
354        m_sizedAxes = new float[count];
355      }
356      System.arraycopy(axes.m_axes, 0, m_sizedAxes, 0, count);
357      m_logAxes.append(m_sizedAxes, timestamp);
358      m_prevAxes.m_count = count;
359      System.arraycopy(axes.m_axes, 0, m_prevAxes.m_axes, 0, count);
360    }
361
362    @SuppressWarnings("PMD.AvoidArrayLoops")
363    void appendPOVs(HALJoystickPOVs povs, long timestamp) {
364      int count = povs.m_count;
365      if (m_sizedPOVs == null || m_sizedPOVs.length != count) {
366        m_sizedPOVs = new long[count];
367      }
368      for (int i = 0; i < count; i++) {
369        m_sizedPOVs[i] = povs.m_povs[i];
370      }
371      m_logPOVs.append(m_sizedPOVs, timestamp);
372      m_prevPOVs.m_count = count;
373      System.arraycopy(povs.m_povs, 0, m_prevPOVs.m_povs, 0, count);
374    }
375
376    final int m_stick;
377    boolean[] m_sizedButtons;
378    float[] m_sizedAxes;
379    long[] m_sizedPOVs;
380    final HALJoystickButtons m_prevButtons = new HALJoystickButtons();
381    final HALJoystickAxes m_prevAxes = new HALJoystickAxes(DriverStationJNI.kMaxJoystickAxes);
382    final HALJoystickPOVs m_prevPOVs = new HALJoystickPOVs(DriverStationJNI.kMaxJoystickPOVs);
383    final BooleanArrayLogEntry m_logButtons;
384    final FloatArrayLogEntry m_logAxes;
385    final IntegerArrayLogEntry m_logPOVs;
386  }
387
388  private static class DataLogSender {
389    DataLogSender(DataLog log, boolean logJoysticks, long timestamp) {
390      m_logEnabled = new BooleanLogEntry(log, "DS:enabled", timestamp);
391      m_logAutonomous = new BooleanLogEntry(log, "DS:autonomous", timestamp);
392      m_logTest = new BooleanLogEntry(log, "DS:test", timestamp);
393      m_logEstop = new BooleanLogEntry(log, "DS:estop", timestamp);
394
395      // append initial control word values
396      m_wasEnabled = m_controlWordCache.getEnabled();
397      m_wasAutonomous = m_controlWordCache.getAutonomous();
398      m_wasTest = m_controlWordCache.getTest();
399      m_wasEstop = m_controlWordCache.getEStop();
400
401      m_logEnabled.append(m_wasEnabled, timestamp);
402      m_logAutonomous.append(m_wasAutonomous, timestamp);
403      m_logTest.append(m_wasTest, timestamp);
404      m_logEstop.append(m_wasEstop, timestamp);
405
406      if (logJoysticks) {
407        m_joysticks = new JoystickLogSender[kJoystickPorts];
408        for (int i = 0; i < kJoystickPorts; i++) {
409          m_joysticks[i] = new JoystickLogSender(log, i, timestamp);
410        }
411      } else {
412        m_joysticks = new JoystickLogSender[0];
413      }
414    }
415
416    public void send(long timestamp) {
417      // append control word value changes
418      boolean enabled = m_controlWordCache.getEnabled();
419      if (enabled != m_wasEnabled) {
420        m_logEnabled.append(enabled, timestamp);
421      }
422      m_wasEnabled = enabled;
423
424      boolean autonomous = m_controlWordCache.getAutonomous();
425      if (autonomous != m_wasAutonomous) {
426        m_logAutonomous.append(autonomous, timestamp);
427      }
428      m_wasAutonomous = autonomous;
429
430      boolean test = m_controlWordCache.getTest();
431      if (test != m_wasTest) {
432        m_logTest.append(test, timestamp);
433      }
434      m_wasTest = test;
435
436      boolean estop = m_controlWordCache.getEStop();
437      if (estop != m_wasEstop) {
438        m_logEstop.append(estop, timestamp);
439      }
440      m_wasEstop = estop;
441
442      // append joystick value changes
443      for (JoystickLogSender joystick : m_joysticks) {
444        joystick.send(timestamp);
445      }
446    }
447
448    boolean m_wasEnabled;
449    boolean m_wasAutonomous;
450    boolean m_wasTest;
451    boolean m_wasEstop;
452    final BooleanLogEntry m_logEnabled;
453    final BooleanLogEntry m_logAutonomous;
454    final BooleanLogEntry m_logTest;
455    final BooleanLogEntry m_logEstop;
456
457    final JoystickLogSender[] m_joysticks;
458  }
459
460  // Joystick User Data
461  private static HALJoystickAxes[] m_joystickAxes = new HALJoystickAxes[kJoystickPorts];
462  private static HALJoystickAxesRaw[] m_joystickAxesRaw = new HALJoystickAxesRaw[kJoystickPorts];
463  private static HALJoystickPOVs[] m_joystickPOVs = new HALJoystickPOVs[kJoystickPorts];
464  private static HALJoystickButtons[] m_joystickButtons = new HALJoystickButtons[kJoystickPorts];
465  private static MatchInfoData m_matchInfo = new MatchInfoData();
466  private static ControlWord m_controlWord = new ControlWord();
467  private static EventVector m_refreshEvents = new EventVector();
468
469  // Joystick Cached Data
470  private static HALJoystickAxes[] m_joystickAxesCache = new HALJoystickAxes[kJoystickPorts];
471  private static HALJoystickAxesRaw[] m_joystickAxesRawCache =
472      new HALJoystickAxesRaw[kJoystickPorts];
473  private static HALJoystickPOVs[] m_joystickPOVsCache = new HALJoystickPOVs[kJoystickPorts];
474  private static HALJoystickButtons[] m_joystickButtonsCache =
475      new HALJoystickButtons[kJoystickPorts];
476  private static MatchInfoData m_matchInfoCache = new MatchInfoData();
477  private static ControlWord m_controlWordCache = new ControlWord();
478
479  // Joystick button rising/falling edge flags
480  private static int[] m_joystickButtonsPressed = new int[kJoystickPorts];
481  private static int[] m_joystickButtonsReleased = new int[kJoystickPorts];
482
483  // preallocated byte buffer for button count
484  private static final ByteBuffer m_buttonCountBuffer = ByteBuffer.allocateDirect(1);
485
486  private static final MatchDataSender m_matchDataSender;
487  private static DataLogSender m_dataLogSender;
488
489  private static final ReentrantLock m_cacheDataMutex = new ReentrantLock();
490
491  private static boolean m_silenceJoystickWarning;
492
493  /**
494   * DriverStation constructor.
495   *
496   * <p>The single DriverStation instance is created statically with the instance static member
497   * variable.
498   */
499  private DriverStation() {}
500
501  static {
502    HAL.initialize(500, 0);
503
504    for (int i = 0; i < kJoystickPorts; i++) {
505      m_joystickButtons[i] = new HALJoystickButtons();
506      m_joystickAxes[i] = new HALJoystickAxes(DriverStationJNI.kMaxJoystickAxes);
507      m_joystickAxesRaw[i] = new HALJoystickAxesRaw(DriverStationJNI.kMaxJoystickAxes);
508      m_joystickPOVs[i] = new HALJoystickPOVs(DriverStationJNI.kMaxJoystickPOVs);
509
510      m_joystickButtonsCache[i] = new HALJoystickButtons();
511      m_joystickAxesCache[i] = new HALJoystickAxes(DriverStationJNI.kMaxJoystickAxes);
512      m_joystickAxesRawCache[i] = new HALJoystickAxesRaw(DriverStationJNI.kMaxJoystickAxes);
513      m_joystickPOVsCache[i] = new HALJoystickPOVs(DriverStationJNI.kMaxJoystickPOVs);
514    }
515
516    m_matchDataSender = new MatchDataSender();
517  }
518
519  /**
520   * Report error to Driver Station. Optionally appends Stack trace to error message.
521   *
522   * @param error The error to report.
523   * @param printTrace If true, append stack trace to error string
524   */
525  public static void reportError(String error, boolean printTrace) {
526    reportErrorImpl(true, 1, error, printTrace);
527  }
528
529  /**
530   * Report error to Driver Station. Appends provided stack trace to error message.
531   *
532   * @param error The error to report.
533   * @param stackTrace The stack trace to append
534   */
535  public static void reportError(String error, StackTraceElement[] stackTrace) {
536    reportErrorImpl(true, 1, error, stackTrace);
537  }
538
539  /**
540   * Report warning to Driver Station. Optionally appends Stack trace to warning message.
541   *
542   * @param warning The warning to report.
543   * @param printTrace If true, append stack trace to warning string
544   */
545  public static void reportWarning(String warning, boolean printTrace) {
546    reportErrorImpl(false, 1, warning, printTrace);
547  }
548
549  /**
550   * Report warning to Driver Station. Appends provided stack trace to warning message.
551   *
552   * @param warning The warning to report.
553   * @param stackTrace The stack trace to append
554   */
555  public static void reportWarning(String warning, StackTraceElement[] stackTrace) {
556    reportErrorImpl(false, 1, warning, stackTrace);
557  }
558
559  private static void reportErrorImpl(boolean isError, int code, String error, boolean printTrace) {
560    reportErrorImpl(isError, code, error, printTrace, Thread.currentThread().getStackTrace(), 3);
561  }
562
563  private static void reportErrorImpl(
564      boolean isError, int code, String error, StackTraceElement[] stackTrace) {
565    reportErrorImpl(isError, code, error, true, stackTrace, 0);
566  }
567
568  private static void reportErrorImpl(
569      boolean isError,
570      int code,
571      String error,
572      boolean printTrace,
573      StackTraceElement[] stackTrace,
574      int stackTraceFirst) {
575    String locString;
576    if (stackTrace.length >= stackTraceFirst + 1) {
577      locString = stackTrace[stackTraceFirst].toString();
578    } else {
579      locString = "";
580    }
581    StringBuilder traceString = new StringBuilder();
582    if (printTrace) {
583      boolean haveLoc = false;
584      for (int i = stackTraceFirst; i < stackTrace.length; i++) {
585        String loc = stackTrace[i].toString();
586        traceString.append("\tat ").append(loc).append('\n');
587        // get first user function
588        if (!haveLoc && !loc.startsWith("edu.wpi.first")) {
589          locString = loc;
590          haveLoc = true;
591        }
592      }
593    }
594    DriverStationJNI.sendError(
595        isError, code, false, error, locString, traceString.toString(), true);
596  }
597
598  /**
599   * The state of one joystick button. Button indexes begin at 1.
600   *
601   * @param stick The joystick to read.
602   * @param button The button index, beginning at 1.
603   * @return The state of the joystick button.
604   */
605  public static boolean getStickButton(final int stick, final int button) {
606    if (stick < 0 || stick >= kJoystickPorts) {
607      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
608    }
609    if (button <= 0) {
610      reportJoystickUnpluggedError("Button indexes begin at 1 in WPILib for C++ and Java\n");
611      return false;
612    }
613
614    m_cacheDataMutex.lock();
615    try {
616      if (button <= m_joystickButtons[stick].m_count) {
617        return (m_joystickButtons[stick].m_buttons & 1 << (button - 1)) != 0;
618      }
619    } finally {
620      m_cacheDataMutex.unlock();
621    }
622
623    reportJoystickUnpluggedWarning(
624        "Joystick Button "
625            + button
626            + " on port "
627            + stick
628            + " not available, check if controller is plugged in");
629    return false;
630  }
631
632  /**
633   * Whether one joystick button was pressed since the last check. Button indexes begin at 1.
634   *
635   * @param stick The joystick to read.
636   * @param button The button index, beginning at 1.
637   * @return Whether the joystick button was pressed since the last check.
638   */
639  public static boolean getStickButtonPressed(final int stick, final int button) {
640    if (button <= 0) {
641      reportJoystickUnpluggedError("Button indexes begin at 1 in WPILib for C++ and Java\n");
642      return false;
643    }
644    if (stick < 0 || stick >= kJoystickPorts) {
645      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
646    }
647
648    m_cacheDataMutex.lock();
649    try {
650      if (button <= m_joystickButtons[stick].m_count) {
651        // If button was pressed, clear flag and return true
652        if ((m_joystickButtonsPressed[stick] & 1 << (button - 1)) != 0) {
653          m_joystickButtonsPressed[stick] &= ~(1 << (button - 1));
654          return true;
655        } else {
656          return false;
657        }
658      }
659    } finally {
660      m_cacheDataMutex.unlock();
661    }
662
663    reportJoystickUnpluggedWarning(
664        "Joystick Button "
665            + button
666            + " on port "
667            + stick
668            + " not available, check if controller is plugged in");
669    return false;
670  }
671
672  /**
673   * Whether one joystick button was released since the last check. Button indexes begin at 1.
674   *
675   * @param stick The joystick to read.
676   * @param button The button index, beginning at 1.
677   * @return Whether the joystick button was released since the last check.
678   */
679  public static boolean getStickButtonReleased(final int stick, final int button) {
680    if (button <= 0) {
681      reportJoystickUnpluggedError("Button indexes begin at 1 in WPILib for C++ and Java\n");
682      return false;
683    }
684    if (stick < 0 || stick >= kJoystickPorts) {
685      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
686    }
687
688    m_cacheDataMutex.lock();
689    try {
690      if (button <= m_joystickButtons[stick].m_count) {
691        // If button was released, clear flag and return true
692        if ((m_joystickButtonsReleased[stick] & 1 << (button - 1)) != 0) {
693          m_joystickButtonsReleased[stick] &= ~(1 << (button - 1));
694          return true;
695        } else {
696          return false;
697        }
698      }
699    } finally {
700      m_cacheDataMutex.unlock();
701    }
702
703    reportJoystickUnpluggedWarning(
704        "Joystick Button "
705            + button
706            + " on port "
707            + stick
708            + " not available, check if controller is plugged in");
709    return false;
710  }
711
712  /**
713   * Get the value of the axis on a joystick. This depends on the mapping of the joystick connected
714   * to the specified port.
715   *
716   * @param stick The joystick to read.
717   * @param axis The analog axis value to read from the joystick.
718   * @return The value of the axis on the joystick.
719   */
720  public static double getStickAxis(int stick, int axis) {
721    if (stick < 0 || stick >= kJoystickPorts) {
722      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
723    }
724    if (axis < 0 || axis >= DriverStationJNI.kMaxJoystickAxes) {
725      throw new IllegalArgumentException("Joystick axis is out of range");
726    }
727
728    m_cacheDataMutex.lock();
729    try {
730      if (axis < m_joystickAxes[stick].m_count) {
731        return m_joystickAxes[stick].m_axes[axis];
732      }
733    } finally {
734      m_cacheDataMutex.unlock();
735    }
736
737    reportJoystickUnpluggedWarning(
738        "Joystick axis "
739            + axis
740            + " on port "
741            + stick
742            + " not available, check if controller is plugged in");
743    return 0.0;
744  }
745
746  /**
747   * Get the state of a POV on the joystick.
748   *
749   * @param stick The joystick to read.
750   * @param pov The POV to read.
751   * @return the angle of the POV.
752   */
753  public static POVDirection getStickPOV(int stick, int pov) {
754    if (stick < 0 || stick >= kJoystickPorts) {
755      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
756    }
757    if (pov < 0 || pov >= DriverStationJNI.kMaxJoystickPOVs) {
758      throw new IllegalArgumentException("Joystick POV is out of range");
759    }
760
761    m_cacheDataMutex.lock();
762    try {
763      if (pov < m_joystickPOVs[stick].m_count) {
764        return POVDirection.of(m_joystickPOVs[stick].m_povs[pov]);
765      }
766    } finally {
767      m_cacheDataMutex.unlock();
768    }
769
770    reportJoystickUnpluggedWarning(
771        "Joystick POV "
772            + pov
773            + " on port "
774            + stick
775            + " not available, check if controller is plugged in");
776    return POVDirection.Center;
777  }
778
779  /**
780   * The state of the buttons on the joystick.
781   *
782   * @param stick The joystick to read.
783   * @return The state of the buttons on the joystick.
784   */
785  public static int getStickButtons(final int stick) {
786    if (stick < 0 || stick >= kJoystickPorts) {
787      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
788    }
789
790    m_cacheDataMutex.lock();
791    try {
792      return m_joystickButtons[stick].m_buttons;
793    } finally {
794      m_cacheDataMutex.unlock();
795    }
796  }
797
798  /**
799   * Returns the number of axes on a given joystick port.
800   *
801   * @param stick The joystick port number
802   * @return The number of axes on the indicated joystick
803   */
804  public static int getStickAxisCount(int stick) {
805    if (stick < 0 || stick >= kJoystickPorts) {
806      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
807    }
808
809    m_cacheDataMutex.lock();
810    try {
811      return m_joystickAxes[stick].m_count;
812    } finally {
813      m_cacheDataMutex.unlock();
814    }
815  }
816
817  /**
818   * Returns the number of povs on a given joystick port.
819   *
820   * @param stick The joystick port number
821   * @return The number of povs on the indicated joystick
822   */
823  public static int getStickPOVCount(int stick) {
824    if (stick < 0 || stick >= kJoystickPorts) {
825      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
826    }
827
828    m_cacheDataMutex.lock();
829    try {
830      return m_joystickPOVs[stick].m_count;
831    } finally {
832      m_cacheDataMutex.unlock();
833    }
834  }
835
836  /**
837   * Gets the number of buttons on a joystick.
838   *
839   * @param stick The joystick port number
840   * @return The number of buttons on the indicated joystick
841   */
842  public static int getStickButtonCount(int stick) {
843    if (stick < 0 || stick >= kJoystickPorts) {
844      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
845    }
846
847    m_cacheDataMutex.lock();
848    try {
849      return m_joystickButtons[stick].m_count;
850    } finally {
851      m_cacheDataMutex.unlock();
852    }
853  }
854
855  /**
856   * Gets the value of isGamepad on a joystick.
857   *
858   * @param stick The joystick port number
859   * @return A boolean that returns the value of isGamepad
860   */
861  public static boolean getJoystickIsGamepad(int stick) {
862    if (stick < 0 || stick >= kJoystickPorts) {
863      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
864    }
865
866    return DriverStationJNI.getJoystickIsGamepad((byte) stick) == 1;
867  }
868
869  /**
870   * Gets the value of type on a joystick.
871   *
872   * @param stick The joystick port number
873   * @return The value of type
874   */
875  public static int getJoystickType(int stick) {
876    if (stick < 0 || stick >= kJoystickPorts) {
877      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
878    }
879
880    return DriverStationJNI.getJoystickType((byte) stick);
881  }
882
883  /**
884   * Gets the name of the joystick at a port.
885   *
886   * @param stick The joystick port number
887   * @return The value of name
888   */
889  public static String getJoystickName(int stick) {
890    if (stick < 0 || stick >= kJoystickPorts) {
891      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
892    }
893
894    return DriverStationJNI.getJoystickName((byte) stick);
895  }
896
897  /**
898   * Returns the types of Axes on a given joystick port.
899   *
900   * @param stick The joystick port number
901   * @param axis The target axis
902   * @return What type of axis the axis is reporting to be
903   */
904  public static int getJoystickAxisType(int stick, int axis) {
905    if (stick < 0 || stick >= kJoystickPorts) {
906      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
907    }
908
909    return DriverStationJNI.getJoystickAxisType((byte) stick, (byte) axis);
910  }
911
912  /**
913   * Returns if a joystick is connected to the Driver Station.
914   *
915   * <p>This makes a best effort guess by looking at the reported number of axis, buttons, and POVs
916   * attached.
917   *
918   * @param stick The joystick port number
919   * @return true if a joystick is connected
920   */
921  public static boolean isJoystickConnected(int stick) {
922    return getStickAxisCount(stick) > 0
923        || getStickButtonCount(stick) > 0
924        || getStickPOVCount(stick) > 0;
925  }
926
927  /**
928   * Gets a value indicating whether the Driver Station requires the robot to be enabled.
929   *
930   * @return True if the robot is enabled, false otherwise.
931   */
932  public static boolean isEnabled() {
933    m_cacheDataMutex.lock();
934    try {
935      return m_controlWord.getEnabled() && m_controlWord.getDSAttached();
936    } finally {
937      m_cacheDataMutex.unlock();
938    }
939  }
940
941  /**
942   * Gets a value indicating whether the Driver Station requires the robot to be disabled.
943   *
944   * @return True if the robot should be disabled, false otherwise.
945   */
946  public static boolean isDisabled() {
947    return !isEnabled();
948  }
949
950  /**
951   * Gets a value indicating whether the Robot is e-stopped.
952   *
953   * @return True if the robot is e-stopped, false otherwise.
954   */
955  public static boolean isEStopped() {
956    m_cacheDataMutex.lock();
957    try {
958      return m_controlWord.getEStop();
959    } finally {
960      m_cacheDataMutex.unlock();
961    }
962  }
963
964  /**
965   * Gets a value indicating whether the Driver Station requires the robot to be running in
966   * autonomous mode.
967   *
968   * @return True if autonomous mode should be enabled, false otherwise.
969   */
970  public static boolean isAutonomous() {
971    m_cacheDataMutex.lock();
972    try {
973      return m_controlWord.getAutonomous();
974    } finally {
975      m_cacheDataMutex.unlock();
976    }
977  }
978
979  /**
980   * Gets a value indicating whether the Driver Station requires the robot to be running in
981   * autonomous mode and enabled.
982   *
983   * @return True if autonomous should be set and the robot should be enabled.
984   */
985  public static boolean isAutonomousEnabled() {
986    m_cacheDataMutex.lock();
987    try {
988      return m_controlWord.getAutonomous() && m_controlWord.getEnabled();
989    } finally {
990      m_cacheDataMutex.unlock();
991    }
992  }
993
994  /**
995   * Gets a value indicating whether the Driver Station requires the robot to be running in
996   * operator-controlled mode.
997   *
998   * @return True if operator-controlled mode should be enabled, false otherwise.
999   */
1000  public static boolean isTeleop() {
1001    return !(isAutonomous() || isTest());
1002  }
1003
1004  /**
1005   * Gets a value indicating whether the Driver Station requires the robot to be running in
1006   * operator-controller mode and enabled.
1007   *
1008   * @return True if operator-controlled mode should be set and the robot should be enabled.
1009   */
1010  public static boolean isTeleopEnabled() {
1011    m_cacheDataMutex.lock();
1012    try {
1013      return !m_controlWord.getAutonomous()
1014          && !m_controlWord.getTest()
1015          && m_controlWord.getEnabled();
1016    } finally {
1017      m_cacheDataMutex.unlock();
1018    }
1019  }
1020
1021  /**
1022   * Gets a value indicating whether the Driver Station requires the robot to be running in Test
1023   * mode.
1024   *
1025   * @return True if test mode should be enabled, false otherwise.
1026   */
1027  public static boolean isTest() {
1028    m_cacheDataMutex.lock();
1029    try {
1030      return m_controlWord.getTest();
1031    } finally {
1032      m_cacheDataMutex.unlock();
1033    }
1034  }
1035
1036  /**
1037   * Gets a value indicating whether the Driver Station requires the robot to be running in Test
1038   * mode and enabled.
1039   *
1040   * @return True if test mode should be set and the robot should be enabled.
1041   */
1042  public static boolean isTestEnabled() {
1043    m_cacheDataMutex.lock();
1044    try {
1045      return m_controlWord.getTest() && m_controlWord.getEnabled();
1046    } finally {
1047      m_cacheDataMutex.unlock();
1048    }
1049  }
1050
1051  /**
1052   * Gets a value indicating whether the Driver Station is attached.
1053   *
1054   * @return True if Driver Station is attached, false otherwise.
1055   */
1056  public static boolean isDSAttached() {
1057    m_cacheDataMutex.lock();
1058    try {
1059      return m_controlWord.getDSAttached();
1060    } finally {
1061      m_cacheDataMutex.unlock();
1062    }
1063  }
1064
1065  /**
1066   * Gets if the driver station attached to a Field Management System.
1067   *
1068   * @return true if the robot is competing on a field being controlled by a Field Management System
1069   */
1070  public static boolean isFMSAttached() {
1071    m_cacheDataMutex.lock();
1072    try {
1073      return m_controlWord.getFMSAttached();
1074    } finally {
1075      m_cacheDataMutex.unlock();
1076    }
1077  }
1078
1079  /**
1080   * Get the game specific message from the FMS.
1081   *
1082   * <p>If the FMS is not connected, it is set from the game data setting on the driver station.
1083   *
1084   * @return the game specific message
1085   */
1086  public static String getGameSpecificMessage() {
1087    m_cacheDataMutex.lock();
1088    try {
1089      return m_matchInfo.gameSpecificMessage;
1090    } finally {
1091      m_cacheDataMutex.unlock();
1092    }
1093  }
1094
1095  /**
1096   * Get the event name from the FMS.
1097   *
1098   * @return the event name
1099   */
1100  public static String getEventName() {
1101    m_cacheDataMutex.lock();
1102    try {
1103      return m_matchInfo.eventName;
1104    } finally {
1105      m_cacheDataMutex.unlock();
1106    }
1107  }
1108
1109  /**
1110   * Get the match type from the FMS.
1111   *
1112   * @return the match type
1113   */
1114  public static MatchType getMatchType() {
1115    int matchType;
1116    m_cacheDataMutex.lock();
1117    try {
1118      matchType = m_matchInfo.matchType;
1119    } finally {
1120      m_cacheDataMutex.unlock();
1121    }
1122    return switch (matchType) {
1123      case 1 -> MatchType.Practice;
1124      case 2 -> MatchType.Qualification;
1125      case 3 -> MatchType.Elimination;
1126      default -> MatchType.None;
1127    };
1128  }
1129
1130  /**
1131   * Get the match number from the FMS.
1132   *
1133   * @return the match number
1134   */
1135  public static int getMatchNumber() {
1136    m_cacheDataMutex.lock();
1137    try {
1138      return m_matchInfo.matchNumber;
1139    } finally {
1140      m_cacheDataMutex.unlock();
1141    }
1142  }
1143
1144  /**
1145   * Get the replay number from the FMS.
1146   *
1147   * @return the replay number
1148   */
1149  public static int getReplayNumber() {
1150    m_cacheDataMutex.lock();
1151    try {
1152      return m_matchInfo.replayNumber;
1153    } finally {
1154      m_cacheDataMutex.unlock();
1155    }
1156  }
1157
1158  private static Map<AllianceStationID, Optional<Alliance>> m_allianceMap =
1159      Map.of(
1160          AllianceStationID.Unknown, Optional.empty(),
1161          AllianceStationID.Red1, Optional.of(Alliance.Red),
1162          AllianceStationID.Red2, Optional.of(Alliance.Red),
1163          AllianceStationID.Red3, Optional.of(Alliance.Red),
1164          AllianceStationID.Blue1, Optional.of(Alliance.Blue),
1165          AllianceStationID.Blue2, Optional.of(Alliance.Blue),
1166          AllianceStationID.Blue3, Optional.of(Alliance.Blue));
1167
1168  private static Map<AllianceStationID, OptionalInt> m_stationMap =
1169      Map.of(
1170          AllianceStationID.Unknown, OptionalInt.empty(),
1171          AllianceStationID.Red1, OptionalInt.of(1),
1172          AllianceStationID.Red2, OptionalInt.of(2),
1173          AllianceStationID.Red3, OptionalInt.of(3),
1174          AllianceStationID.Blue1, OptionalInt.of(1),
1175          AllianceStationID.Blue2, OptionalInt.of(2),
1176          AllianceStationID.Blue3, OptionalInt.of(3));
1177
1178  /**
1179   * Get the current alliance from the FMS.
1180   *
1181   * <p>If the FMS is not connected, it is set from the team alliance setting on the driver station.
1182   *
1183   * @return The alliance (red or blue) or an empty optional if the alliance is invalid
1184   */
1185  public static Optional<Alliance> getAlliance() {
1186    AllianceStationID allianceStationID = DriverStationJNI.getAllianceStation();
1187    if (allianceStationID == null) {
1188      allianceStationID = AllianceStationID.Unknown;
1189    }
1190
1191    return m_allianceMap.get(allianceStationID);
1192  }
1193
1194  /**
1195   * Gets the location of the team's driver station controls from the FMS.
1196   *
1197   * <p>If the FMS is not connected, it is set from the team alliance setting on the driver station.
1198   *
1199   * @return the location of the team's driver station controls: 1, 2, or 3
1200   */
1201  public static OptionalInt getLocation() {
1202    AllianceStationID allianceStationID = DriverStationJNI.getAllianceStation();
1203    if (allianceStationID == null) {
1204      allianceStationID = AllianceStationID.Unknown;
1205    }
1206
1207    return m_stationMap.get(allianceStationID);
1208  }
1209
1210  /**
1211   * Gets the raw alliance station of the teams driver station.
1212   *
1213   * <p>This returns the raw low level value. Prefer getLocation or getAlliance unless necessary for
1214   * performance.
1215   *
1216   * @return The raw alliance station id.
1217   */
1218  public static AllianceStationID getRawAllianceStation() {
1219    return DriverStationJNI.getAllianceStation();
1220  }
1221
1222  /**
1223   * Wait for a DS connection.
1224   *
1225   * @param timeout timeout in seconds. 0 for infinite.
1226   * @return true if connected, false if timeout
1227   */
1228  public static boolean waitForDsConnection(double timeout) {
1229    int event = WPIUtilJNI.createEvent(true, false);
1230    DriverStationJNI.provideNewDataEventHandle(event);
1231    boolean result;
1232    try {
1233      if (timeout == 0) {
1234        WPIUtilJNI.waitForObject(event);
1235        result = true;
1236      } else {
1237        result = !WPIUtilJNI.waitForObjectTimeout(event, timeout);
1238      }
1239    } catch (InterruptedException ex) {
1240      Thread.currentThread().interrupt();
1241      result = false;
1242    } finally {
1243      DriverStationJNI.removeNewDataEventHandle(event);
1244      WPIUtilJNI.destroyEvent(event);
1245    }
1246    return result;
1247  }
1248
1249  /**
1250   * Return the approximate match time. The FMS does not send an official match time to the robots,
1251   * but does send an approximate match time. The value will count down the time remaining in the
1252   * current period (auto or teleop). Warning: This is not an official time (so it cannot be used to
1253   * dispute ref calls or guarantee that a function will trigger before the match ends).
1254   *
1255   * <p>When connected to the real field, this number only changes in full integer increments, and
1256   * always counts down.
1257   *
1258   * <p>When the DS is in practice mode, this number is a floating point number, and counts down.
1259   *
1260   * <p>When the DS is in teleop or autonomous mode, this number is a floating point number, and
1261   * counts up.
1262   *
1263   * <p>Simulation matches DS behavior without an FMS connected.
1264   *
1265   * @return Time remaining in current match period (auto or teleop) in seconds
1266   */
1267  public static double getMatchTime() {
1268    return DriverStationJNI.getMatchTime();
1269  }
1270
1271  /**
1272   * Allows the user to specify whether they want joystick connection warnings to be printed to the
1273   * console. This setting is ignored when the FMS is connected -- warnings will always be on in
1274   * that scenario.
1275   *
1276   * @param silence Whether warning messages should be silenced.
1277   */
1278  public static void silenceJoystickConnectionWarning(boolean silence) {
1279    m_silenceJoystickWarning = silence;
1280  }
1281
1282  /**
1283   * Returns whether joystick connection warnings are silenced. This will always return false when
1284   * connected to the FMS.
1285   *
1286   * @return Whether joystick connection warnings are silenced.
1287   */
1288  public static boolean isJoystickConnectionWarningSilenced() {
1289    return !isFMSAttached() && m_silenceJoystickWarning;
1290  }
1291
1292  /**
1293   * Refresh the passed in control word to contain the current control word cache.
1294   *
1295   * @param word Word to refresh.
1296   */
1297  public static void refreshControlWordFromCache(ControlWord word) {
1298    m_cacheDataMutex.lock();
1299    try {
1300      word.update(m_controlWord);
1301    } finally {
1302      m_cacheDataMutex.unlock();
1303    }
1304  }
1305
1306  /**
1307   * Copy data from the DS task for the user. If no new data exists, it will just be returned,
1308   * otherwise the data will be copied from the DS polling loop.
1309   */
1310  public static void refreshData() {
1311    DriverStationJNI.refreshDSData();
1312
1313    // Get the status of all the joysticks
1314    for (byte stick = 0; stick < kJoystickPorts; stick++) {
1315      m_joystickAxesCache[stick].m_count =
1316          DriverStationJNI.getJoystickAxes(stick, m_joystickAxesCache[stick].m_axes);
1317      m_joystickAxesRawCache[stick].m_count =
1318          DriverStationJNI.getJoystickAxesRaw(stick, m_joystickAxesRawCache[stick].m_axes);
1319      m_joystickPOVsCache[stick].m_count =
1320          DriverStationJNI.getJoystickPOVs(stick, m_joystickPOVsCache[stick].m_povs);
1321      m_joystickButtonsCache[stick].m_buttons =
1322          DriverStationJNI.getJoystickButtons(stick, m_buttonCountBuffer);
1323      m_joystickButtonsCache[stick].m_count = m_buttonCountBuffer.get(0);
1324    }
1325
1326    DriverStationJNI.getMatchInfo(m_matchInfoCache);
1327
1328    DriverStationJNI.getControlWord(m_controlWordCache);
1329
1330    DataLogSender dataLogSender;
1331    // lock joystick mutex to swap cache data
1332    m_cacheDataMutex.lock();
1333    try {
1334      for (int i = 0; i < kJoystickPorts; i++) {
1335        // If buttons weren't pressed and are now, set flags in m_buttonsPressed
1336        m_joystickButtonsPressed[i] |=
1337            ~m_joystickButtons[i].m_buttons & m_joystickButtonsCache[i].m_buttons;
1338
1339        // If buttons were pressed and aren't now, set flags in m_buttonsReleased
1340        m_joystickButtonsReleased[i] |=
1341            m_joystickButtons[i].m_buttons & ~m_joystickButtonsCache[i].m_buttons;
1342      }
1343
1344      // move cache to actual data
1345      HALJoystickAxes[] currentAxes = m_joystickAxes;
1346      m_joystickAxes = m_joystickAxesCache;
1347      m_joystickAxesCache = currentAxes;
1348
1349      HALJoystickAxesRaw[] currentAxesRaw = m_joystickAxesRaw;
1350      m_joystickAxesRaw = m_joystickAxesRawCache;
1351      m_joystickAxesRawCache = currentAxesRaw;
1352
1353      HALJoystickButtons[] currentButtons = m_joystickButtons;
1354      m_joystickButtons = m_joystickButtonsCache;
1355      m_joystickButtonsCache = currentButtons;
1356
1357      HALJoystickPOVs[] currentPOVs = m_joystickPOVs;
1358      m_joystickPOVs = m_joystickPOVsCache;
1359      m_joystickPOVsCache = currentPOVs;
1360
1361      MatchInfoData currentInfo = m_matchInfo;
1362      m_matchInfo = m_matchInfoCache;
1363      m_matchInfoCache = currentInfo;
1364
1365      ControlWord currentWord = m_controlWord;
1366      m_controlWord = m_controlWordCache;
1367      m_controlWordCache = currentWord;
1368
1369      dataLogSender = m_dataLogSender;
1370    } finally {
1371      m_cacheDataMutex.unlock();
1372    }
1373
1374    m_refreshEvents.wakeup();
1375
1376    m_matchDataSender.sendMatchData();
1377    if (dataLogSender != null) {
1378      dataLogSender.send(WPIUtilJNI.now());
1379    }
1380  }
1381
1382  /**
1383   * Registers the given handle for DS data refresh notifications.
1384   *
1385   * @param handle The event handle.
1386   */
1387  public static void provideRefreshedDataEventHandle(int handle) {
1388    m_refreshEvents.add(handle);
1389  }
1390
1391  /**
1392   * Unregisters the given handle from DS data refresh notifications.
1393   *
1394   * @param handle The event handle.
1395   */
1396  public static void removeRefreshedDataEventHandle(int handle) {
1397    m_refreshEvents.remove(handle);
1398  }
1399
1400  /**
1401   * Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm
1402   * the DS.
1403   */
1404  private static void reportJoystickUnpluggedError(String message) {
1405    double currentTime = Timer.getTimestamp();
1406    if (currentTime > m_nextMessageTime) {
1407      reportError(message, false);
1408      m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
1409    }
1410  }
1411
1412  /**
1413   * Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm
1414   * the DS.
1415   */
1416  private static void reportJoystickUnpluggedWarning(String message) {
1417    if (isFMSAttached() || !m_silenceJoystickWarning) {
1418      double currentTime = Timer.getTimestamp();
1419      if (currentTime > m_nextMessageTime) {
1420        reportWarning(message, false);
1421        m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
1422      }
1423    }
1424  }
1425
1426  /**
1427   * Starts logging DriverStation data to data log. Repeated calls are ignored.
1428   *
1429   * @param log data log
1430   * @param logJoysticks if true, log joystick data
1431   */
1432  @SuppressWarnings("PMD.NonThreadSafeSingleton")
1433  public static void startDataLog(DataLog log, boolean logJoysticks) {
1434    m_cacheDataMutex.lock();
1435    try {
1436      if (m_dataLogSender == null) {
1437        m_dataLogSender = new DataLogSender(log, logJoysticks, WPIUtilJNI.now());
1438      }
1439    } finally {
1440      m_cacheDataMutex.unlock();
1441    }
1442  }
1443
1444  /**
1445   * Starts logging DriverStation data to data log, including joystick data. Repeated calls are
1446   * ignored.
1447   *
1448   * @param log data log
1449   */
1450  public static void startDataLog(DataLog log) {
1451    startDataLog(log, true);
1452  }
1453}