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;
006
007import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
008
009import edu.wpi.first.util.function.BooleanConsumer;
010import edu.wpi.first.util.sendable.Sendable;
011import edu.wpi.first.util.sendable.SendableBuilder;
012import edu.wpi.first.util.sendable.SendableRegistry;
013import java.util.HashSet;
014import java.util.Set;
015import java.util.function.BooleanSupplier;
016
017/**
018 * A state machine representing a complete action to be performed by the robot. Commands are run by
019 * the {@link CommandScheduler}, and can be composed into CommandGroups to allow users to build
020 * complicated multistep actions without the need to roll the state machine logic themselves.
021 *
022 * <p>Commands are run synchronously from the main robot loop; no multithreading is used, unless
023 * specified explicitly from the command implementation.
024 *
025 * <p>This class is provided by the NewCommands VendorDep
026 */
027public abstract class Command implements Sendable {
028  /** Requirements set. */
029  protected Set<Subsystem> m_requirements = new HashSet<>();
030
031  /** Default constructor. */
032  @SuppressWarnings("this-escape")
033  protected Command() {
034    String name = getClass().getName();
035    SendableRegistry.add(this, name.substring(name.lastIndexOf('.') + 1));
036  }
037
038  /** The initial subroutine of a command. Called once when the command is initially scheduled. */
039  public void initialize() {}
040
041  /** The main body of a command. Called repeatedly while the command is scheduled. */
042  public void execute() {}
043
044  /**
045   * The action to take when the command ends. Called when either the command finishes normally, or
046   * when it interrupted/canceled.
047   *
048   * <p>Do not schedule commands here that share requirements with this command. Use {@link
049   * #andThen(Command...)} instead.
050   *
051   * @param interrupted whether the command was interrupted/canceled
052   */
053  public void end(boolean interrupted) {}
054
055  /**
056   * Whether the command has finished. Once a command finishes, the scheduler will call its end()
057   * method and un-schedule it.
058   *
059   * @return whether the command has finished.
060   */
061  public boolean isFinished() {
062    return false;
063  }
064
065  /**
066   * Specifies the set of subsystems used by this command. Two commands cannot use the same
067   * subsystem at the same time. If another command is scheduled that shares a requirement, {@link
068   * #getInterruptionBehavior()} will be checked and followed. If no subsystems are required, return
069   * an empty set.
070   *
071   * <p>Note: it is recommended that user implementations contain the requirements as a field, and
072   * return that field here, rather than allocating a new set every time this is called.
073   *
074   * @return the set of subsystems that are required
075   * @see InterruptionBehavior
076   */
077  public Set<Subsystem> getRequirements() {
078    return m_requirements;
079  }
080
081  /**
082   * Adds the specified subsystems to the requirements of the command. The scheduler will prevent
083   * two commands that require the same subsystem from being scheduled simultaneously.
084   *
085   * <p>Note that the scheduler determines the requirements of a command when it is scheduled, so
086   * this method should normally be called from the command's constructor.
087   *
088   * @param requirements the requirements to add
089   */
090  public final void addRequirements(Subsystem... requirements) {
091    for (Subsystem requirement : requirements) {
092      m_requirements.add(requireNonNullParam(requirement, "requirement", "addRequirements"));
093    }
094  }
095
096  /**
097   * Gets the name of this Command.
098   *
099   * <p>By default, the simple class name is used. This can be changed with {@link
100   * #setName(String)}.
101   *
102   * @return The display name of the Command
103   */
104  public String getName() {
105    return SendableRegistry.getName(this);
106  }
107
108  /**
109   * Sets the name of this Command.
110   *
111   * @param name The display name of the Command.
112   */
113  public void setName(String name) {
114    SendableRegistry.setName(this, name);
115  }
116
117  /**
118   * Gets the subsystem name of this Command.
119   *
120   * @return Subsystem name
121   */
122  public String getSubsystem() {
123    return SendableRegistry.getSubsystem(this);
124  }
125
126  /**
127   * Sets the subsystem name of this Command.
128   *
129   * @param subsystem subsystem name
130   */
131  public void setSubsystem(String subsystem) {
132    SendableRegistry.setSubsystem(this, subsystem);
133  }
134
135  /**
136   * Decorates this command with a timeout. If the specified timeout is exceeded before the command
137   * finishes normally, the command will be interrupted and un-scheduled.
138   *
139   * <p>Note: This decorator works by adding this command to a composition. The command the
140   * decorator was called on cannot be scheduled independently or be added to a different
141   * composition (namely, decorators), unless it is manually cleared from the list of composed
142   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
143   * returned from this method can be further decorated without issue.
144   *
145   * @param seconds the timeout duration
146   * @return the command with the timeout added
147   */
148  public ParallelRaceGroup withTimeout(double seconds) {
149    return raceWith(new WaitCommand(seconds));
150  }
151
152  /**
153   * Decorates this command with an interrupt condition. If the specified condition becomes true
154   * before the command finishes normally, the command will be interrupted and un-scheduled.
155   *
156   * <p>Note: This decorator works by adding this command to a composition. The command the
157   * decorator was called on cannot be scheduled independently or be added to a different
158   * composition (namely, decorators), unless it is manually cleared from the list of composed
159   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
160   * returned from this method can be further decorated without issue.
161   *
162   * @param condition the interrupt condition
163   * @return the command with the interrupt condition added
164   * @see #onlyWhile(BooleanSupplier)
165   */
166  public ParallelRaceGroup until(BooleanSupplier condition) {
167    return raceWith(new WaitUntilCommand(condition));
168  }
169
170  /**
171   * Decorates this command with a run condition. If the specified condition becomes false before
172   * the command finishes normally, the command will be interrupted and un-scheduled.
173   *
174   * <p>Note: This decorator works by adding this command to a composition. The command the
175   * decorator was called on cannot be scheduled independently or be added to a different
176   * composition (namely, decorators), unless it is manually cleared from the list of composed
177   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
178   * returned from this method can be further decorated without issue.
179   *
180   * @param condition the run condition
181   * @return the command with the run condition added
182   * @see #until(BooleanSupplier)
183   */
184  public ParallelRaceGroup onlyWhile(BooleanSupplier condition) {
185    return until(() -> !condition.getAsBoolean());
186  }
187
188  /**
189   * Decorates this command with a runnable to run before this command starts.
190   *
191   * <p>Note: This decorator works by adding this command to a composition. The command the
192   * decorator was called on cannot be scheduled independently or be added to a different
193   * composition (namely, decorators), unless it is manually cleared from the list of composed
194   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
195   * returned from this method can be further decorated without issue.
196   *
197   * @param toRun the Runnable to run
198   * @param requirements the required subsystems
199   * @return the decorated command
200   */
201  public SequentialCommandGroup beforeStarting(Runnable toRun, Subsystem... requirements) {
202    return beforeStarting(new InstantCommand(toRun, requirements));
203  }
204
205  /**
206   * Decorates this command with another command to run before this command starts.
207   *
208   * <p>Note: This decorator works by adding this command to a composition. The command the
209   * decorator was called on cannot be scheduled independently or be added to a different
210   * composition (namely, decorators), unless it is manually cleared from the list of composed
211   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
212   * returned from this method can be further decorated without issue.
213   *
214   * @param before the command to run before this one
215   * @return the decorated command
216   */
217  public SequentialCommandGroup beforeStarting(Command before) {
218    return new SequentialCommandGroup(before, this);
219  }
220
221  /**
222   * Decorates this command with a runnable to run after the command finishes.
223   *
224   * <p>Note: This decorator works by adding this command to a composition. The command the
225   * decorator was called on cannot be scheduled independently or be added to a different
226   * composition (namely, decorators), unless it is manually cleared from the list of composed
227   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
228   * returned from this method can be further decorated without issue.
229   *
230   * @param toRun the Runnable to run
231   * @param requirements the required subsystems
232   * @return the decorated command
233   */
234  public SequentialCommandGroup andThen(Runnable toRun, Subsystem... requirements) {
235    return andThen(new InstantCommand(toRun, requirements));
236  }
237
238  /**
239   * Decorates this command with a set of commands to run after it in sequence. Often more
240   * convenient/less-verbose than constructing a new {@link SequentialCommandGroup} explicitly.
241   *
242   * <p>Note: This decorator works by adding this command to a composition. The command the
243   * decorator was called on cannot be scheduled independently or be added to a different
244   * composition (namely, decorators), unless it is manually cleared from the list of composed
245   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
246   * returned from this method can be further decorated without issue.
247   *
248   * @param next the commands to run next
249   * @return the decorated command
250   */
251  public SequentialCommandGroup andThen(Command... next) {
252    SequentialCommandGroup group = new SequentialCommandGroup(this);
253    group.addCommands(next);
254    return group;
255  }
256
257  /**
258   * Decorates this command with a set of commands to run parallel to it, ending when the calling
259   * command ends and interrupting all the others. Often more convenient/less-verbose than
260   * constructing a new {@link ParallelDeadlineGroup} explicitly.
261   *
262   * <p>Note: This decorator works by adding this command to a composition. The command the
263   * decorator was called on cannot be scheduled independently or be added to a different
264   * composition (namely, decorators), unless it is manually cleared from the list of composed
265   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
266   * returned from this method can be further decorated without issue.
267   *
268   * @param parallel the commands to run in parallel
269   * @return the decorated command
270   */
271  public ParallelDeadlineGroup deadlineWith(Command... parallel) {
272    return new ParallelDeadlineGroup(this, parallel);
273  }
274
275  /**
276   * Decorates this command with a set of commands to run parallel to it, ending when the last
277   * command ends. Often more convenient/less-verbose than constructing a new {@link
278   * ParallelCommandGroup} explicitly.
279   *
280   * <p>Note: This decorator works by adding this command to a composition. The command the
281   * decorator was called on cannot be scheduled independently or be added to a different
282   * composition (namely, decorators), unless it is manually cleared from the list of composed
283   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
284   * returned from this method can be further decorated without issue.
285   *
286   * @param parallel the commands to run in parallel
287   * @return the decorated command
288   */
289  public ParallelCommandGroup alongWith(Command... parallel) {
290    ParallelCommandGroup group = new ParallelCommandGroup(this);
291    group.addCommands(parallel);
292    return group;
293  }
294
295  /**
296   * Decorates this command with a set of commands to run parallel to it, ending when the first
297   * command ends. Often more convenient/less-verbose than constructing a new {@link
298   * ParallelRaceGroup} explicitly.
299   *
300   * <p>Note: This decorator works by adding this command to a composition. The command the
301   * decorator was called on cannot be scheduled independently or be added to a different
302   * composition (namely, decorators), unless it is manually cleared from the list of composed
303   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
304   * returned from this method can be further decorated without issue.
305   *
306   * @param parallel the commands to run in parallel
307   * @return the decorated command
308   */
309  public ParallelRaceGroup raceWith(Command... parallel) {
310    ParallelRaceGroup group = new ParallelRaceGroup(this);
311    group.addCommands(parallel);
312    return group;
313  }
314
315  /**
316   * Decorates this command to run repeatedly, restarting it when it ends, until this command is
317   * interrupted. The decorated command can still be canceled.
318   *
319   * <p>Note: This decorator works by adding this command to a composition. The command the
320   * decorator was called on cannot be scheduled independently or be added to a different
321   * composition (namely, decorators), unless it is manually cleared from the list of composed
322   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
323   * returned from this method can be further decorated without issue.
324   *
325   * @return the decorated command
326   */
327  public RepeatCommand repeatedly() {
328    return new RepeatCommand(this);
329  }
330
331  /**
332   * Decorates this command to run "by proxy" by wrapping it in a {@link ProxyCommand}. This is
333   * useful for "forking off" from command compositions when the user does not wish to extend the
334   * command's requirements to the entire command composition.
335   *
336   * @return the decorated command
337   */
338  public ProxyCommand asProxy() {
339    return new ProxyCommand(this);
340  }
341
342  /**
343   * Decorates this command to only run if this condition is not met. If the command is already
344   * running and the condition changes to true, the command will not stop running. The requirements
345   * of this command will be kept for the new conditional command.
346   *
347   * <p>Note: This decorator works by adding this command to a composition. The command the
348   * decorator was called on cannot be scheduled independently or be added to a different
349   * composition (namely, decorators), unless it is manually cleared from the list of composed
350   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
351   * returned from this method can be further decorated without issue.
352   *
353   * @param condition the condition that will prevent the command from running
354   * @return the decorated command
355   * @see #onlyIf(BooleanSupplier)
356   */
357  public ConditionalCommand unless(BooleanSupplier condition) {
358    return new ConditionalCommand(new InstantCommand(), this, condition);
359  }
360
361  /**
362   * Decorates this command to only run if this condition is met. If the command is already running
363   * and the condition changes to false, the command will not stop running. The requirements of this
364   * command will be kept for the new conditional command.
365   *
366   * <p>Note: This decorator works by adding this command to a composition. The command the
367   * decorator was called on cannot be scheduled independently or be added to a different
368   * composition (namely, decorators), unless it is manually cleared from the list of composed
369   * commands with {@link CommandScheduler#removeComposedCommand(Command)}. The command composition
370   * returned from this method can be further decorated without issue.
371   *
372   * @param condition the condition that will allow the command to run
373   * @return the decorated command
374   * @see #unless(BooleanSupplier)
375   */
376  public ConditionalCommand onlyIf(BooleanSupplier condition) {
377    return unless(() -> !condition.getAsBoolean());
378  }
379
380  /**
381   * Decorates this command to run or stop when disabled.
382   *
383   * @param doesRunWhenDisabled true to run when disabled.
384   * @return the decorated command
385   */
386  public WrapperCommand ignoringDisable(boolean doesRunWhenDisabled) {
387    return new WrapperCommand(this) {
388      @Override
389      public boolean runsWhenDisabled() {
390        return doesRunWhenDisabled;
391      }
392    };
393  }
394
395  /**
396   * Decorates this command to have a different {@link InterruptionBehavior interruption behavior}.
397   *
398   * @param interruptBehavior the desired interrupt behavior
399   * @return the decorated command
400   */
401  public WrapperCommand withInterruptBehavior(InterruptionBehavior interruptBehavior) {
402    return new WrapperCommand(this) {
403      @Override
404      public InterruptionBehavior getInterruptionBehavior() {
405        return interruptBehavior;
406      }
407    };
408  }
409
410  /**
411   * Decorates this command with a lambda to call on interrupt or end, following the command's
412   * inherent {@link #end(boolean)} method.
413   *
414   * @param end a lambda accepting a boolean parameter specifying whether the command was
415   *     interrupted.
416   * @return the decorated command
417   */
418  public WrapperCommand finallyDo(BooleanConsumer end) {
419    requireNonNullParam(end, "end", "Command.finallyDo()");
420    return new WrapperCommand(this) {
421      @Override
422      public void end(boolean interrupted) {
423        super.end(interrupted);
424        end.accept(interrupted);
425      }
426    };
427  }
428
429  /**
430   * Decorates this command with a lambda to call on interrupt or end, following the command's
431   * inherent {@link #end(boolean)} method. The provided lambda will run identically in both
432   * interrupt and end cases.
433   *
434   * @param end a lambda to run when the command ends, whether or not it was interrupted.
435   * @return the decorated command
436   */
437  public WrapperCommand finallyDo(Runnable end) {
438    return finallyDo(interrupted -> end.run());
439  }
440
441  /**
442   * Decorates this command with a lambda to call on interrupt, following the command's inherent
443   * {@link #end(boolean)} method.
444   *
445   * @param handler a lambda to run when the command is interrupted
446   * @return the decorated command
447   */
448  public WrapperCommand handleInterrupt(Runnable handler) {
449    requireNonNullParam(handler, "handler", "Command.handleInterrupt()");
450    return finallyDo(
451        interrupted -> {
452          if (interrupted) {
453            handler.run();
454          }
455        });
456  }
457
458  /** Schedules this command. */
459  public void schedule() {
460    CommandScheduler.getInstance().schedule(this);
461  }
462
463  /**
464   * Cancels this command. Will call {@link #end(boolean) end(true)}. Commands will be canceled
465   * regardless of {@link InterruptionBehavior interruption behavior}.
466   *
467   * @see CommandScheduler#cancel(Command...)
468   */
469  public void cancel() {
470    CommandScheduler.getInstance().cancel(this);
471  }
472
473  /**
474   * Whether the command is currently scheduled. Note that this does not detect whether the command
475   * is in a composition, only whether it is directly being run by the scheduler.
476   *
477   * @return Whether the command is scheduled.
478   */
479  public boolean isScheduled() {
480    return CommandScheduler.getInstance().isScheduled(this);
481  }
482
483  /**
484   * Whether the command requires a given subsystem.
485   *
486   * @param requirement the subsystem to inquire about
487   * @return whether the subsystem is required
488   */
489  public boolean hasRequirement(Subsystem requirement) {
490    return getRequirements().contains(requirement);
491  }
492
493  /**
494   * How the command behaves when another command with a shared requirement is scheduled.
495   *
496   * @return a variant of {@link InterruptionBehavior}, defaulting to {@link
497   *     InterruptionBehavior#kCancelSelf kCancelSelf}.
498   */
499  public InterruptionBehavior getInterruptionBehavior() {
500    return InterruptionBehavior.kCancelSelf;
501  }
502
503  /**
504   * Whether the given command should run when the robot is disabled. Override to return true if the
505   * command should run when disabled.
506   *
507   * @return whether the command should run when the robot is disabled
508   */
509  public boolean runsWhenDisabled() {
510    return false;
511  }
512
513  /**
514   * Decorates this Command with a name.
515   *
516   * @param name name
517   * @return the decorated Command
518   */
519  public WrapperCommand withName(String name) {
520    WrapperCommand wrapper = new WrapperCommand(Command.this) {};
521    wrapper.setName(name);
522    return wrapper;
523  }
524
525  @Override
526  public void initSendable(SendableBuilder builder) {
527    builder.setSmartDashboardType("Command");
528    builder.addStringProperty(".name", this::getName, null);
529    builder.addBooleanProperty(
530        "running",
531        this::isScheduled,
532        value -> {
533          if (value) {
534            if (!isScheduled()) {
535              schedule();
536            }
537          } else {
538            if (isScheduled()) {
539              cancel();
540            }
541          }
542        });
543    builder.addBooleanProperty(
544        ".isParented", () -> CommandScheduler.getInstance().isComposed(this), null);
545    builder.addStringProperty(
546        "interruptBehavior", () -> getInterruptionBehavior().toString(), null);
547    builder.addBooleanProperty("runsWhenDisabled", this::runsWhenDisabled, null);
548  }
549
550  /**
551   * An enum describing the command's behavior when another command with a shared requirement is
552   * scheduled.
553   */
554  public enum InterruptionBehavior {
555    /**
556     * This command ends, {@link #end(boolean) end(true)} is called, and the incoming command is
557     * scheduled normally.
558     *
559     * <p>This is the default behavior.
560     */
561    kCancelSelf,
562    /** This command continues, and the incoming command is not scheduled. */
563    kCancelIncoming
564  }
565}