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}