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