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.DriverStationJNI; 008import edu.wpi.first.math.MathUtil; 009import edu.wpi.first.math.Pair; 010import edu.wpi.first.wpilibj.DriverStation.POVDirection; 011import edu.wpi.first.wpilibj.event.BooleanEvent; 012import edu.wpi.first.wpilibj.event.EventLoop; 013import java.util.HashMap; 014import java.util.Map; 015 016/** 017 * Handle input from standard HID devices connected to the Driver Station. 018 * 019 * <p>This class handles standard input that comes from the Driver Station. Each time a value is 020 * requested the most recent value is returned. There is a single class instance for each device and 021 * the mapping of ports to hardware buttons depends on the code in the Driver Station. 022 */ 023public class GenericHID { 024 /** Represents a rumble output on the Joystick. */ 025 public enum RumbleType { 026 /** Left rumble motor. */ 027 kLeftRumble, 028 /** Right rumble motor. */ 029 kRightRumble, 030 /** Both left and right rumble motors. */ 031 kBothRumble 032 } 033 034 /** USB HID interface type. */ 035 public enum HIDType { 036 /** Unknown. */ 037 kUnknown(-1), 038 /** XInputUnknown. */ 039 kXInputUnknown(0), 040 /** XInputGamepad. */ 041 kXInputGamepad(1), 042 /** XInputWheel. */ 043 kXInputWheel(2), 044 /** XInputArcadeStick. */ 045 kXInputArcadeStick(3), 046 /** XInputFlightStick. */ 047 kXInputFlightStick(4), 048 /** XInputDancePad. */ 049 kXInputDancePad(5), 050 /** XInputGuitar. */ 051 kXInputGuitar(6), 052 /** XInputGuitar2. */ 053 kXInputGuitar2(7), 054 /** XInputDrumKit. */ 055 kXInputDrumKit(8), 056 /** XInputGuitar3. */ 057 kXInputGuitar3(11), 058 /** XInputArcadePad. */ 059 kXInputArcadePad(19), 060 /** HIDJoystick. */ 061 kHIDJoystick(20), 062 /** HIDGamepad. */ 063 kHIDGamepad(21), 064 /** HIDDriving. */ 065 kHIDDriving(22), 066 /** HIDFlight. */ 067 kHIDFlight(23), 068 /** HID1stPerson. */ 069 kHID1stPerson(24); 070 071 /** HIDType value. */ 072 public final int value; 073 074 @SuppressWarnings("PMD.UseConcurrentHashMap") 075 private static final Map<Integer, HIDType> map = new HashMap<>(); 076 077 HIDType(int value) { 078 this.value = value; 079 } 080 081 static { 082 for (HIDType hidType : HIDType.values()) { 083 map.put(hidType.value, hidType); 084 } 085 } 086 087 /** 088 * Creates an HIDType with the given value. 089 * 090 * @param value HIDType's value. 091 * @return HIDType with the given value. 092 */ 093 public static HIDType of(int value) { 094 return map.get(value); 095 } 096 } 097 098 private final int m_port; 099 private int m_outputs; 100 private int m_leftRumble; 101 private int m_rightRumble; 102 private final Map<EventLoop, Map<Integer, BooleanEvent>> m_buttonCache = new HashMap<>(); 103 private final Map<EventLoop, Map<Pair<Integer, Double>, BooleanEvent>> m_axisLessThanCache = 104 new HashMap<>(); 105 private final Map<EventLoop, Map<Pair<Integer, Double>, BooleanEvent>> m_axisGreaterThanCache = 106 new HashMap<>(); 107 private final Map<EventLoop, Map<Integer, BooleanEvent>> m_povCache = new HashMap<>(); 108 109 /** 110 * Construct an instance of a device. 111 * 112 * @param port The port index on the Driver Station that the device is plugged into. 113 */ 114 public GenericHID(int port) { 115 m_port = port; 116 } 117 118 /** 119 * Get the button value (starting at button 1). 120 * 121 * <p>The buttons are returned in a single 16 bit value with one bit representing the state of 122 * each button. The appropriate button is returned as a boolean value. 123 * 124 * <p>This method returns true if the button is being held down at the time that this method is 125 * being called. 126 * 127 * @param button The button number to be read (starting at 1) 128 * @return The state of the button. 129 */ 130 public boolean getRawButton(int button) { 131 return DriverStation.getStickButton(m_port, (byte) button); 132 } 133 134 /** 135 * Whether the button was pressed since the last check. Button indexes begin at 1. 136 * 137 * <p>This method returns true if the button went from not pressed to held down since the last 138 * time this method was called. This is useful if you only want to call a function once when you 139 * press the button. 140 * 141 * @param button The button index, beginning at 1. 142 * @return Whether the button was pressed since the last check. 143 */ 144 public boolean getRawButtonPressed(int button) { 145 return DriverStation.getStickButtonPressed(m_port, (byte) button); 146 } 147 148 /** 149 * Whether the button was released since the last check. Button indexes begin at 1. 150 * 151 * <p>This method returns true if the button went from held down to not pressed since the last 152 * time this method was called. This is useful if you only want to call a function once when you 153 * release the button. 154 * 155 * @param button The button index, beginning at 1. 156 * @return Whether the button was released since the last check. 157 */ 158 public boolean getRawButtonReleased(int button) { 159 return DriverStation.getStickButtonReleased(m_port, button); 160 } 161 162 /** 163 * Constructs an event instance around this button's digital signal. 164 * 165 * @param button the button index 166 * @param loop the event loop instance to attach the event to. 167 * @return an event instance representing the button's digital signal attached to the given loop. 168 */ 169 public BooleanEvent button(int button, EventLoop loop) { 170 synchronized (m_buttonCache) { 171 var cache = m_buttonCache.computeIfAbsent(loop, k -> new HashMap<>()); 172 return cache.computeIfAbsent(button, k -> new BooleanEvent(loop, () -> getRawButton(k))); 173 } 174 } 175 176 /** 177 * Get the value of the axis. 178 * 179 * @param axis The axis to read, starting at 0. 180 * @return The value of the axis. 181 */ 182 public double getRawAxis(int axis) { 183 return DriverStation.getStickAxis(m_port, axis); 184 } 185 186 /** 187 * Get the angle of a POV on the HID. 188 * 189 * @param pov The index of the POV to read (starting at 0). Defaults to 0. 190 * @return the angle of the POV. 191 */ 192 public POVDirection getPOV(int pov) { 193 return DriverStation.getStickPOV(m_port, pov); 194 } 195 196 /** 197 * Get the angle of the default POV (index 0) on the HID. 198 * 199 * @return the angle of the POV. 200 */ 201 public POVDirection getPOV() { 202 return getPOV(0); 203 } 204 205 /** 206 * Constructs a BooleanEvent instance based around this angle of a POV on the HID. 207 * 208 * @param angle POV angle. 209 * @param loop the event loop instance to attach the event to. 210 * @return a BooleanEvent instance based around this angle of a POV on the HID. 211 */ 212 public BooleanEvent pov(POVDirection angle, EventLoop loop) { 213 return pov(0, angle, loop); 214 } 215 216 /** 217 * Constructs a BooleanEvent instance based around this angle of a POV on the HID. 218 * 219 * @param pov index of the POV to read (starting at 0). Defaults to 0. 220 * @param angle POV angle. 221 * @param loop the event loop instance to attach the event to. 222 * @return a BooleanEvent instance based around this angle of a POV on the HID. 223 */ 224 public BooleanEvent pov(int pov, POVDirection angle, EventLoop loop) { 225 synchronized (m_povCache) { 226 var cache = m_povCache.computeIfAbsent(loop, k -> new HashMap<>()); 227 // angle.value is a 4 bit bitfield 228 return cache.computeIfAbsent( 229 pov * 16 + angle.value, k -> new BooleanEvent(loop, () -> getPOV(pov) == angle)); 230 } 231 } 232 233 /** 234 * Constructs a BooleanEvent instance based around the up direction of the default (index 0) POV 235 * on the HID. 236 * 237 * @param loop the event loop instance to attach the event to. 238 * @return a BooleanEvent instance based around the up direction a POV on the HID. 239 */ 240 public BooleanEvent povUp(EventLoop loop) { 241 return pov(POVDirection.Up, loop); 242 } 243 244 /** 245 * Constructs a BooleanEvent instance based around the up right direction of the default (index 0) 246 * POV on the HID. 247 * 248 * @param loop the event loop instance to attach the event to. 249 * @return a BooleanEvent instance based around the up right direction of a POV on the HID. 250 */ 251 public BooleanEvent povUpRight(EventLoop loop) { 252 return pov(POVDirection.UpRight, loop); 253 } 254 255 /** 256 * Constructs a BooleanEvent instance based around the right direction of the default (index 0) 257 * POV on the HID. 258 * 259 * @param loop the event loop instance to attach the event to. 260 * @return a BooleanEvent instance based around the right direction of a POV on the HID. 261 */ 262 public BooleanEvent povRight(EventLoop loop) { 263 return pov(POVDirection.Right, loop); 264 } 265 266 /** 267 * Constructs a BooleanEvent instance based around the down right direction of the default (index 268 * 0) POV on the HID. 269 * 270 * @param loop the event loop instance to attach the event to. 271 * @return a BooleanEvent instance based around the down right direction of a POV on the HID. 272 */ 273 public BooleanEvent povDownRight(EventLoop loop) { 274 return pov(POVDirection.DownRight, loop); 275 } 276 277 /** 278 * Constructs a BooleanEvent instance based around the down direction of the default (index 0) POV 279 * on the HID. 280 * 281 * @param loop the event loop instance to attach the event to. 282 * @return a BooleanEvent instance based around the down direction of a POV on the HID. 283 */ 284 public BooleanEvent povDown(EventLoop loop) { 285 return pov(POVDirection.Down, loop); 286 } 287 288 /** 289 * Constructs a BooleanEvent instance based around the down left direction of the default (index 290 * 0) POV on the HID. 291 * 292 * @param loop the event loop instance to attach the event to. 293 * @return a BooleanEvent instance based around the down left direction of a POV on the HID. 294 */ 295 public BooleanEvent povDownLeft(EventLoop loop) { 296 return pov(POVDirection.DownLeft, loop); 297 } 298 299 /** 300 * Constructs a BooleanEvent instance based around the left direction of the default (index 0) POV 301 * on the HID. 302 * 303 * @param loop the event loop instance to attach the event to. 304 * @return a BooleanEvent instance based around the left direction of a POV on the HID. 305 */ 306 public BooleanEvent povLeft(EventLoop loop) { 307 return pov(POVDirection.Left, loop); 308 } 309 310 /** 311 * Constructs a BooleanEvent instance based around the up left direction of the default (index 0) 312 * POV on the HID. 313 * 314 * @param loop the event loop instance to attach the event to. 315 * @return a BooleanEvent instance based around the up left direction of a POV on the HID. 316 */ 317 public BooleanEvent povUpLeft(EventLoop loop) { 318 return pov(POVDirection.UpLeft, loop); 319 } 320 321 /** 322 * Constructs a BooleanEvent instance based around the center (not pressed) of the default (index 323 * 0) POV on the HID. 324 * 325 * @param loop the event loop instance to attach the event to. 326 * @return a BooleanEvent instance based around the center of a POV on the HID. 327 */ 328 public BooleanEvent povCenter(EventLoop loop) { 329 return pov(POVDirection.Center, loop); 330 } 331 332 /** 333 * Constructs an event instance that is true when the axis value is less than {@code threshold}, 334 * attached to the given loop. 335 * 336 * @param axis The axis to read, starting at 0 337 * @param threshold The value below which this event should return true. 338 * @param loop the event loop instance to attach the event to. 339 * @return an event instance that is true when the axis value is less than the provided threshold. 340 */ 341 public BooleanEvent axisLessThan(int axis, double threshold, EventLoop loop) { 342 synchronized (m_axisLessThanCache) { 343 var cache = m_axisLessThanCache.computeIfAbsent(loop, k -> new HashMap<>()); 344 return cache.computeIfAbsent( 345 Pair.of(axis, threshold), 346 k -> new BooleanEvent(loop, () -> getRawAxis(axis) < threshold)); 347 } 348 } 349 350 /** 351 * Constructs an event instance that is true when the axis value is greater than {@code 352 * threshold}, attached to the given loop. 353 * 354 * @param axis The axis to read, starting at 0 355 * @param threshold The value above which this event should return true. 356 * @param loop the event loop instance to attach the event to. 357 * @return an event instance that is true when the axis value is greater than the provided 358 * threshold. 359 */ 360 public BooleanEvent axisGreaterThan(int axis, double threshold, EventLoop loop) { 361 synchronized (m_axisGreaterThanCache) { 362 var cache = m_axisGreaterThanCache.computeIfAbsent(loop, k -> new HashMap<>()); 363 return cache.computeIfAbsent( 364 Pair.of(axis, threshold), 365 k -> new BooleanEvent(loop, () -> getRawAxis(axis) > threshold)); 366 } 367 } 368 369 /** 370 * Get the number of axes for the HID. 371 * 372 * @return the number of axis for the current HID 373 */ 374 public int getAxisCount() { 375 return DriverStation.getStickAxisCount(m_port); 376 } 377 378 /** 379 * For the current HID, return the number of POVs. 380 * 381 * @return the number of POVs for the current HID 382 */ 383 public int getPOVCount() { 384 return DriverStation.getStickPOVCount(m_port); 385 } 386 387 /** 388 * For the current HID, return the number of buttons. 389 * 390 * @return the number of buttons for the current HID 391 */ 392 public int getButtonCount() { 393 return DriverStation.getStickButtonCount(m_port); 394 } 395 396 /** 397 * Get if the HID is connected. 398 * 399 * @return true if the HID is connected 400 */ 401 public boolean isConnected() { 402 return DriverStation.isJoystickConnected(m_port); 403 } 404 405 /** 406 * Get the type of the HID. 407 * 408 * @return the type of the HID. 409 */ 410 public HIDType getType() { 411 return HIDType.of(DriverStation.getJoystickType(m_port)); 412 } 413 414 /** 415 * Get the name of the HID. 416 * 417 * @return the name of the HID. 418 */ 419 public String getName() { 420 return DriverStation.getJoystickName(m_port); 421 } 422 423 /** 424 * Get the axis type of the provided joystick axis. 425 * 426 * @param axis The axis to read, starting at 0. 427 * @return the axis type of the given joystick axis 428 */ 429 public int getAxisType(int axis) { 430 return DriverStation.getJoystickAxisType(m_port, axis); 431 } 432 433 /** 434 * Get the port number of the HID. 435 * 436 * @return The port number of the HID. 437 */ 438 public int getPort() { 439 return m_port; 440 } 441 442 /** 443 * Set a single HID output value for the HID. 444 * 445 * @param outputNumber The index of the output to set (1-32) 446 * @param value The value to set the output to 447 */ 448 public void setOutput(int outputNumber, boolean value) { 449 m_outputs = (m_outputs & ~(1 << (outputNumber - 1))) | ((value ? 1 : 0) << (outputNumber - 1)); 450 DriverStationJNI.setJoystickOutputs((byte) m_port, m_outputs, m_leftRumble, m_rightRumble); 451 } 452 453 /** 454 * Set all HID output values for the HID. 455 * 456 * @param value The 32 bit output value (1 bit for each output) 457 */ 458 public void setOutputs(int value) { 459 m_outputs = value; 460 DriverStationJNI.setJoystickOutputs((byte) m_port, m_outputs, m_leftRumble, m_rightRumble); 461 } 462 463 /** 464 * Set the rumble output for the HID. The DS currently supports 2 rumble values, left rumble and 465 * right rumble. 466 * 467 * @param type Which rumble value to set 468 * @param value The normalized value (0 to 1) to set the rumble to 469 */ 470 public void setRumble(RumbleType type, double value) { 471 value = MathUtil.clamp(value, 0, 1); 472 int rumbleValue = (int) (value * 65535); 473 switch (type) { 474 case kLeftRumble -> this.m_leftRumble = rumbleValue; 475 case kRightRumble -> this.m_rightRumble = rumbleValue; 476 default -> { 477 this.m_leftRumble = rumbleValue; 478 this.m_rightRumble = rumbleValue; 479 } 480 } 481 482 DriverStationJNI.setJoystickOutputs( 483 (byte) this.m_port, this.m_outputs, this.m_leftRumble, this.m_rightRumble); 484 } 485}