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