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 static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
008
009import edu.wpi.first.math.filter.Debouncer;
010import edu.wpi.first.wpilibj2.command.Command;
011import edu.wpi.first.wpilibj2.command.CommandScheduler;
012import edu.wpi.first.wpilibj2.command.InstantCommand;
013import edu.wpi.first.wpilibj2.command.Subsystem;
014import java.util.function.BooleanSupplier;
015
016/**
017 * This class provides an easy way to link commands to inputs.
018 *
019 * <p>It is very easy to link a button to a command. For instance, you could link the trigger button
020 * of a joystick to a "score" command.
021 *
022 * <p>It is encouraged that teams write a subclass of Trigger if they want to have something unusual
023 * (for instance, if they want to react to the user holding a button while the robot is reading a
024 * certain sensor input). For this, they only have to write the {@link Trigger#get()} method to get
025 * the full functionality of the Trigger class.
026 */
027public class Trigger implements BooleanSupplier {
028  private final BooleanSupplier m_isActive;
029
030  /**
031   * Creates a new trigger with the given condition determining whether it is active.
032   *
033   * @param isActive returns whether or not the trigger should be active
034   */
035  public Trigger(BooleanSupplier isActive) {
036    m_isActive = isActive;
037  }
038
039  /**
040   * Creates a new trigger that is always inactive. Useful only as a no-arg constructor for
041   * subclasses that will be overriding {@link Trigger#get()} anyway.
042   */
043  public Trigger() {
044    m_isActive = () -> false;
045  }
046
047  /**
048   * Returns whether or not the trigger is active.
049   *
050   * <p>This method will be called repeatedly a command is linked to the Trigger.
051   *
052   * <p>Functionally identical to {@link Trigger#getAsBoolean()}.
053   *
054   * @return whether or not the trigger condition is active.
055   */
056  public boolean get() {
057    return this.getAsBoolean();
058  }
059
060  /**
061   * Returns whether or not the trigger is active.
062   *
063   * <p>This method will be called repeatedly a command is linked to the Trigger.
064   *
065   * <p>Functionally identical to {@link Trigger#get()}.
066   *
067   * @return whether or not the trigger condition is active.
068   */
069  @Override
070  public boolean getAsBoolean() {
071    return m_isActive.getAsBoolean();
072  }
073
074  /**
075   * Starts the given command whenever the trigger just becomes active.
076   *
077   * @param command the command to start
078   * @param interruptible whether the command is interruptible
079   * @return this trigger, so calls can be chained
080   */
081  public Trigger whenActive(final Command command, boolean interruptible) {
082    requireNonNullParam(command, "command", "whenActive");
083
084    CommandScheduler.getInstance()
085        .addButton(
086            new Runnable() {
087              private boolean m_pressedLast = get();
088
089              @Override
090              public void run() {
091                boolean pressed = get();
092
093                if (!m_pressedLast && pressed) {
094                  command.schedule(interruptible);
095                }
096
097                m_pressedLast = pressed;
098              }
099            });
100
101    return this;
102  }
103
104  /**
105   * Starts the given command whenever the trigger just becomes active. The command is set to be
106   * interruptible.
107   *
108   * @param command the command to start
109   * @return this trigger, so calls can be chained
110   */
111  public Trigger whenActive(final Command command) {
112    return whenActive(command, true);
113  }
114
115  /**
116   * Runs the given runnable whenever the trigger just becomes active.
117   *
118   * @param toRun the runnable to run
119   * @param requirements the required subsystems
120   * @return this trigger, so calls can be chained
121   */
122  public Trigger whenActive(final Runnable toRun, Subsystem... requirements) {
123    return whenActive(new InstantCommand(toRun, requirements));
124  }
125
126  /**
127   * Constantly starts the given command while the button is held.
128   *
129   * <p>{@link Command#schedule(boolean)} will be called repeatedly while the trigger is active, and
130   * will be canceled when the trigger becomes inactive.
131   *
132   * @param command the command to start
133   * @param interruptible whether the command is interruptible
134   * @return this trigger, so calls can be chained
135   */
136  public Trigger whileActiveContinuous(final Command command, boolean interruptible) {
137    requireNonNullParam(command, "command", "whileActiveContinuous");
138
139    CommandScheduler.getInstance()
140        .addButton(
141            new Runnable() {
142              private boolean m_pressedLast = get();
143
144              @Override
145              public void run() {
146                boolean pressed = get();
147
148                if (pressed) {
149                  command.schedule(interruptible);
150                } else if (m_pressedLast) {
151                  command.cancel();
152                }
153
154                m_pressedLast = pressed;
155              }
156            });
157    return this;
158  }
159
160  /**
161   * Constantly starts the given command while the button is held.
162   *
163   * <p>{@link Command#schedule(boolean)} will be called repeatedly while the trigger is active, and
164   * will be canceled when the trigger becomes inactive. The command is set to be interruptible.
165   *
166   * @param command the command to start
167   * @return this trigger, so calls can be chained
168   */
169  public Trigger whileActiveContinuous(final Command command) {
170    return whileActiveContinuous(command, true);
171  }
172
173  /**
174   * Constantly runs the given runnable while the button is held.
175   *
176   * @param toRun the runnable to run
177   * @param requirements the required subsystems
178   * @return this trigger, so calls can be chained
179   */
180  public Trigger whileActiveContinuous(final Runnable toRun, Subsystem... requirements) {
181    return whileActiveContinuous(new InstantCommand(toRun, requirements));
182  }
183
184  /**
185   * Starts the given command when the trigger initially becomes active, and ends it when it becomes
186   * inactive, but does not re-start it in-between.
187   *
188   * @param command the command to start
189   * @param interruptible whether the command is interruptible
190   * @return this trigger, so calls can be chained
191   */
192  public Trigger whileActiveOnce(final Command command, boolean interruptible) {
193    requireNonNullParam(command, "command", "whileActiveOnce");
194
195    CommandScheduler.getInstance()
196        .addButton(
197            new Runnable() {
198              private boolean m_pressedLast = get();
199
200              @Override
201              public void run() {
202                boolean pressed = get();
203
204                if (!m_pressedLast && pressed) {
205                  command.schedule(interruptible);
206                } else if (m_pressedLast && !pressed) {
207                  command.cancel();
208                }
209
210                m_pressedLast = pressed;
211              }
212            });
213    return this;
214  }
215
216  /**
217   * Starts the given command when the trigger initially becomes active, and ends it when it becomes
218   * inactive, but does not re-start it in-between. The command is set to be interruptible.
219   *
220   * @param command the command to start
221   * @return this trigger, so calls can be chained
222   */
223  public Trigger whileActiveOnce(final Command command) {
224    return whileActiveOnce(command, true);
225  }
226
227  /**
228   * Starts the command when the trigger becomes inactive.
229   *
230   * @param command the command to start
231   * @param interruptible whether the command is interruptible
232   * @return this trigger, so calls can be chained
233   */
234  public Trigger whenInactive(final Command command, boolean interruptible) {
235    requireNonNullParam(command, "command", "whenInactive");
236
237    CommandScheduler.getInstance()
238        .addButton(
239            new Runnable() {
240              private boolean m_pressedLast = get();
241
242              @Override
243              public void run() {
244                boolean pressed = get();
245
246                if (m_pressedLast && !pressed) {
247                  command.schedule(interruptible);
248                }
249
250                m_pressedLast = pressed;
251              }
252            });
253    return this;
254  }
255
256  /**
257   * Starts the command when the trigger becomes inactive. The command is set to be interruptible.
258   *
259   * @param command the command to start
260   * @return this trigger, so calls can be chained
261   */
262  public Trigger whenInactive(final Command command) {
263    return whenInactive(command, true);
264  }
265
266  /**
267   * Runs the given runnable when the trigger becomes inactive.
268   *
269   * @param toRun the runnable to run
270   * @param requirements the required subsystems
271   * @return this trigger, so calls can be chained
272   */
273  public Trigger whenInactive(final Runnable toRun, Subsystem... requirements) {
274    return whenInactive(new InstantCommand(toRun, requirements));
275  }
276
277  /**
278   * Toggles a command when the trigger becomes active.
279   *
280   * @param command the command to toggle
281   * @param interruptible whether the command is interruptible
282   * @return this trigger, so calls can be chained
283   */
284  public Trigger toggleWhenActive(final Command command, boolean interruptible) {
285    requireNonNullParam(command, "command", "toggleWhenActive");
286
287    CommandScheduler.getInstance()
288        .addButton(
289            new Runnable() {
290              private boolean m_pressedLast = get();
291
292              @Override
293              public void run() {
294                boolean pressed = get();
295
296                if (!m_pressedLast && pressed) {
297                  if (command.isScheduled()) {
298                    command.cancel();
299                  } else {
300                    command.schedule(interruptible);
301                  }
302                }
303
304                m_pressedLast = pressed;
305              }
306            });
307    return this;
308  }
309
310  /**
311   * Toggles a command when the trigger becomes active. The command is set to be interruptible.
312   *
313   * @param command the command to toggle
314   * @return this trigger, so calls can be chained
315   */
316  public Trigger toggleWhenActive(final Command command) {
317    return toggleWhenActive(command, true);
318  }
319
320  /**
321   * Cancels a command when the trigger becomes active.
322   *
323   * @param command the command to cancel
324   * @return this trigger, so calls can be chained
325   */
326  public Trigger cancelWhenActive(final Command command) {
327    requireNonNullParam(command, "command", "cancelWhenActive");
328
329    CommandScheduler.getInstance()
330        .addButton(
331            new Runnable() {
332              private boolean m_pressedLast = get();
333
334              @Override
335              public void run() {
336                boolean pressed = get();
337
338                if (!m_pressedLast && pressed) {
339                  command.cancel();
340                }
341
342                m_pressedLast = pressed;
343              }
344            });
345    return this;
346  }
347
348  /**
349   * Composes this trigger with another trigger, returning a new trigger that is active when both
350   * triggers are active.
351   *
352   * @param trigger the trigger to compose with
353   * @return the trigger that is active when both triggers are active
354   */
355  public Trigger and(Trigger trigger) {
356    return new Trigger(() -> get() && trigger.get());
357  }
358
359  /**
360   * Composes this trigger with another trigger, returning a new trigger that is active when either
361   * trigger is active.
362   *
363   * @param trigger the trigger to compose with
364   * @return the trigger that is active when either trigger is active
365   */
366  public Trigger or(Trigger trigger) {
367    return new Trigger(() -> get() || trigger.get());
368  }
369
370  /**
371   * Creates a new trigger that is active when this trigger is inactive, i.e. that acts as the
372   * negation of this trigger.
373   *
374   * @return the negated trigger
375   */
376  public Trigger negate() {
377    return new Trigger(() -> !get());
378  }
379
380  /**
381   * Creates a new debounced trigger from this trigger - it will become active when this trigger has
382   * been active for longer than the specified period.
383   *
384   * @param seconds The debounce period.
385   * @return The debounced trigger (rising edges debounced only)
386   */
387  public Trigger debounce(double seconds) {
388    return debounce(seconds, Debouncer.DebounceType.kRising);
389  }
390
391  /**
392   * Creates a new debounced trigger from this trigger - it will become active when this trigger has
393   * been active for longer than the specified period.
394   *
395   * @param seconds The debounce period.
396   * @param type The debounce type.
397   * @return The debounced trigger.
398   */
399  public Trigger debounce(double seconds, Debouncer.DebounceType type) {
400    return new Trigger(
401        new BooleanSupplier() {
402          Debouncer m_debouncer = new Debouncer(seconds, type);
403
404          @Override
405          public boolean getAsBoolean() {
406            return m_debouncer.calculate(get());
407          }
408        });
409  }
410}