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}