Source code

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