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