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