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