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.wpilibj2.command.button; 006 007import edu.wpi.first.math.Pair; 008import edu.wpi.first.wpilibj.GenericHID; 009import edu.wpi.first.wpilibj.event.EventLoop; 010import edu.wpi.first.wpilibj2.command.CommandScheduler; 011import java.util.HashMap; 012import java.util.Map; 013 014/** 015 * A version of {@link GenericHID} with {@link Trigger} factories for command-based. 016 * 017 * @see GenericHID 018 */ 019public class CommandGenericHID { 020 private final GenericHID m_hid; 021 private final Map<EventLoop, Map<Integer, Trigger>> m_buttonCache = new HashMap<>(); 022 private final Map<EventLoop, Map<Pair<Integer, Double>, Trigger>> m_axisLessThanCache = 023 new HashMap<>(); 024 private final Map<EventLoop, Map<Pair<Integer, Double>, Trigger>> m_axisGreaterThanCache = 025 new HashMap<>(); 026 private final Map<EventLoop, Map<Pair<Integer, Double>, Trigger>> 027 m_axisMagnitudeGreaterThanCache = new HashMap<>(); 028 private final Map<EventLoop, Map<Integer, Trigger>> m_povCache = new HashMap<>(); 029 030 /** 031 * Construct an instance of a device. 032 * 033 * @param port The port index on the Driver Station that the device is plugged into. 034 */ 035 public CommandGenericHID(int port) { 036 m_hid = new GenericHID(port); 037 } 038 039 /** 040 * Get the underlying GenericHID object. 041 * 042 * @return the wrapped GenericHID object 043 */ 044 public GenericHID getHID() { 045 return m_hid; 046 } 047 048 /** 049 * Constructs an event instance around this button's digital signal. 050 * 051 * @param button the button index 052 * @return an event instance representing the button's digital signal attached to the {@link 053 * CommandScheduler#getDefaultButtonLoop() default scheduler button loop}. 054 * @see #button(int, EventLoop) 055 */ 056 public Trigger button(int button) { 057 return button(button, CommandScheduler.getInstance().getDefaultButtonLoop()); 058 } 059 060 /** 061 * Constructs an event instance around this button's digital signal. 062 * 063 * @param button the button index 064 * @param loop the event loop instance to attach the event to. 065 * @return an event instance representing the button's digital signal attached to the given loop. 066 */ 067 public Trigger button(int button, EventLoop loop) { 068 var cache = m_buttonCache.computeIfAbsent(loop, k -> new HashMap<>()); 069 return cache.computeIfAbsent(button, k -> new Trigger(loop, () -> m_hid.getRawButton(k))); 070 } 071 072 /** 073 * Constructs a Trigger instance based around this angle of the default (index 0) POV on the HID, 074 * attached to {@link CommandScheduler#getDefaultButtonLoop() the default command scheduler button 075 * loop}. 076 * 077 * <p>The POV angles start at 0 in the up direction, and increase clockwise (e.g. right is 90, 078 * upper-left is 315). 079 * 080 * @param angle POV angle in degrees, or -1 for the center / not pressed. 081 * @return a Trigger instance based around this angle of a POV on the HID. 082 */ 083 public Trigger pov(int angle) { 084 return pov(0, angle, CommandScheduler.getInstance().getDefaultButtonLoop()); 085 } 086 087 /** 088 * Constructs a Trigger instance based around this angle of a POV on the HID. 089 * 090 * <p>The POV angles start at 0 in the up direction, and increase clockwise (e.g. right is 90, 091 * upper-left is 315). 092 * 093 * @param pov index of the POV to read (starting at 0). Defaults to 0. 094 * @param angle POV angle in degrees, or -1 for the center / not pressed. 095 * @param loop the event loop instance to attach the event to. Defaults to {@link 096 * CommandScheduler#getDefaultButtonLoop() the default command scheduler button loop}. 097 * @return a Trigger instance based around this angle of a POV on the HID. 098 */ 099 public Trigger pov(int pov, int angle, EventLoop loop) { 100 var cache = m_povCache.computeIfAbsent(loop, k -> new HashMap<>()); 101 // angle can be -1, so use 3600 instead of 360 102 return cache.computeIfAbsent( 103 pov * 3600 + angle, k -> new Trigger(loop, () -> m_hid.getPOV(pov) == angle)); 104 } 105 106 /** 107 * Constructs a Trigger instance based around the 0 degree angle (up) of the default (index 0) POV 108 * on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command 109 * scheduler button loop}. 110 * 111 * @return a Trigger instance based around the 0 degree angle of a POV on the HID. 112 */ 113 public Trigger povUp() { 114 return pov(0); 115 } 116 117 /** 118 * Constructs a Trigger instance based around the 45 degree angle (right up) of the default (index 119 * 0) POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default 120 * command scheduler button loop}. 121 * 122 * @return a Trigger instance based around the 45 degree angle of a POV on the HID. 123 */ 124 public Trigger povUpRight() { 125 return pov(45); 126 } 127 128 /** 129 * Constructs a Trigger instance based around the 90 degree angle (right) of the default (index 0) 130 * POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command 131 * scheduler button loop}. 132 * 133 * @return a Trigger instance based around the 90 degree angle of a POV on the HID. 134 */ 135 public Trigger povRight() { 136 return pov(90); 137 } 138 139 /** 140 * Constructs a Trigger instance based around the 135 degree angle (right down) of the default 141 * (index 0) POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the 142 * default command scheduler button loop}. 143 * 144 * @return a Trigger instance based around the 135 degree angle of a POV on the HID. 145 */ 146 public Trigger povDownRight() { 147 return pov(135); 148 } 149 150 /** 151 * Constructs a Trigger instance based around the 180 degree angle (down) of the default (index 0) 152 * POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command 153 * scheduler button loop}. 154 * 155 * @return a Trigger instance based around the 180 degree angle of a POV on the HID. 156 */ 157 public Trigger povDown() { 158 return pov(180); 159 } 160 161 /** 162 * Constructs a Trigger instance based around the 225 degree angle (down left) of the default 163 * (index 0) POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the 164 * default command scheduler button loop}. 165 * 166 * @return a Trigger instance based around the 225 degree angle of a POV on the HID. 167 */ 168 public Trigger povDownLeft() { 169 return pov(225); 170 } 171 172 /** 173 * Constructs a Trigger instance based around the 270 degree angle (left) of the default (index 0) 174 * POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command 175 * scheduler button loop}. 176 * 177 * @return a Trigger instance based around the 270 degree angle of a POV on the HID. 178 */ 179 public Trigger povLeft() { 180 return pov(270); 181 } 182 183 /** 184 * Constructs a Trigger instance based around the 315 degree angle (left up) of the default (index 185 * 0) POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default 186 * command scheduler button loop}. 187 * 188 * @return a Trigger instance based around the 315 degree angle of a POV on the HID. 189 */ 190 public Trigger povUpLeft() { 191 return pov(315); 192 } 193 194 /** 195 * Constructs a Trigger instance based around the center (not pressed) position of the default 196 * (index 0) POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the 197 * default command scheduler button loop}. 198 * 199 * @return a Trigger instance based around the center position of a POV on the HID. 200 */ 201 public Trigger povCenter() { 202 return pov(-1); 203 } 204 205 /** 206 * Constructs a Trigger instance that is true when the axis value is less than {@code threshold}, 207 * attached to {@link CommandScheduler#getDefaultButtonLoop() the default command scheduler button 208 * loop}. 209 * 210 * @param axis The axis to read, starting at 0 211 * @param threshold The value below which this trigger should return true. 212 * @return a Trigger instance that is true when the axis value is less than the provided 213 * threshold. 214 */ 215 public Trigger axisLessThan(int axis, double threshold) { 216 return axisLessThan(axis, threshold, CommandScheduler.getInstance().getDefaultButtonLoop()); 217 } 218 219 /** 220 * Constructs a Trigger instance that is true when the axis value is less than {@code threshold}, 221 * attached to the given loop. 222 * 223 * @param axis The axis to read, starting at 0 224 * @param threshold The value below which this trigger should return true. 225 * @param loop the event loop instance to attach the trigger to 226 * @return a Trigger instance that is true when the axis value is less than the provided 227 * threshold. 228 */ 229 public Trigger axisLessThan(int axis, double threshold, EventLoop loop) { 230 var cache = m_axisLessThanCache.computeIfAbsent(loop, k -> new HashMap<>()); 231 return cache.computeIfAbsent( 232 Pair.of(axis, threshold), k -> new Trigger(loop, () -> getRawAxis(axis) < threshold)); 233 } 234 235 /** 236 * Constructs a Trigger instance that is true when the axis value is less than {@code threshold}, 237 * attached to {@link CommandScheduler#getDefaultButtonLoop() the default command scheduler button 238 * loop}. 239 * 240 * @param axis The axis to read, starting at 0 241 * @param threshold The value above which this trigger should return true. 242 * @return a Trigger instance that is true when the axis value is greater than the provided 243 * threshold. 244 */ 245 public Trigger axisGreaterThan(int axis, double threshold) { 246 return axisGreaterThan(axis, threshold, CommandScheduler.getInstance().getDefaultButtonLoop()); 247 } 248 249 /** 250 * Constructs a Trigger instance that is true when the axis value is greater than {@code 251 * threshold}, attached to the given loop. 252 * 253 * @param axis The axis to read, starting at 0 254 * @param threshold The value above which this trigger should return true. 255 * @param loop the event loop instance to attach the trigger to. 256 * @return a Trigger instance that is true when the axis value is greater than the provided 257 * threshold. 258 */ 259 public Trigger axisGreaterThan(int axis, double threshold, EventLoop loop) { 260 var cache = m_axisGreaterThanCache.computeIfAbsent(loop, k -> new HashMap<>()); 261 return cache.computeIfAbsent( 262 Pair.of(axis, threshold), k -> new Trigger(loop, () -> getRawAxis(axis) > threshold)); 263 } 264 265 /** 266 * Constructs a Trigger instance that is true when the axis magnitude value is greater than {@code 267 * threshold}, attached to the given loop. 268 * 269 * @param axis The axis to read, starting at 0 270 * @param threshold The value above which this trigger should return true. 271 * @param loop the event loop instance to attach the trigger to. 272 * @return a Trigger instance that is true when the axis magnitude value is greater than the 273 * provided threshold. 274 */ 275 public Trigger axisMagnitudeGreaterThan(int axis, double threshold, EventLoop loop) { 276 var cache = m_axisMagnitudeGreaterThanCache.computeIfAbsent(loop, k -> new HashMap<>()); 277 return cache.computeIfAbsent( 278 Pair.of(axis, threshold), 279 k -> new Trigger(loop, () -> Math.abs(getRawAxis(axis)) > threshold)); 280 } 281 282 /** 283 * Constructs a Trigger instance that is true when the axis magnitude value is greater than {@code 284 * threshold}, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command 285 * scheduler button loop}. 286 * 287 * @param axis The axis to read, starting at 0 288 * @param threshold The value above which this trigger should return true. 289 * @return a Trigger instance that is true when the deadbanded axis value is active (non-zero). 290 */ 291 public Trigger axisMagnitudeGreaterThan(int axis, double threshold) { 292 return axisMagnitudeGreaterThan( 293 axis, threshold, CommandScheduler.getInstance().getDefaultButtonLoop()); 294 } 295 296 /** 297 * Get the value of the axis. 298 * 299 * @param axis The axis to read, starting at 0. 300 * @return The value of the axis. 301 */ 302 public double getRawAxis(int axis) { 303 return m_hid.getRawAxis(axis); 304 } 305 306 /** 307 * Set the rumble output for the HID. The DS currently supports 2 rumble values, left rumble and 308 * right rumble. 309 * 310 * @param type Which rumble value to set 311 * @param value The normalized value (0 to 1) to set the rumble to 312 */ 313 public void setRumble(GenericHID.RumbleType type, double value) { 314 m_hid.setRumble(type, value); 315 } 316 317 /** 318 * Get if the HID is connected. 319 * 320 * @return true if the HID is connected 321 */ 322 public boolean isConnected() { 323 return m_hid.isConnected(); 324 } 325}