Source code

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.wpilibj.command;
006
007import edu.wpi.first.util.sendable.Sendable;
008import edu.wpi.first.util.sendable.SendableBuilder;
009import edu.wpi.first.util.sendable.SendableRegistry;
010import edu.wpi.first.wpilibj.RobotState;
011import edu.wpi.first.wpilibj.Timer;
012import java.util.Enumeration;
013
014/**
015 * The Command class is at the very core of the entire command framework. Every command can be
016 * started with a call to {@link Command#start() start()}. Once a command is started it will call
017 * {@link Command#initialize() initialize()}, and then will repeatedly call {@link Command#execute()
018 * execute()} until the {@link Command#isFinished() isFinished()} returns true. Once it does, {@link
019 * Command#end() end()} will be called.
020 *
021 * <p>However, if at any point while it is running {@link Command#cancel() cancel()} is called, then
022 * the command will be stopped and {@link Command#interrupted() interrupted()} will be called.
023 *
024 * <p>If a command uses a {@link Subsystem}, then it should specify that it does so by calling the
025 * {@link Command#requires(Subsystem) requires(...)} method in its constructor. Note that a Command
026 * may have multiple requirements, and {@link Command#requires(Subsystem) requires(...)} should be
027 * called for each one.
028 *
029 * <p>If a command is running and a new command with shared requirements is started, then one of two
030 * things will happen. If the active command is interruptible, then {@link Command#cancel()
031 * cancel()} will be called and the command will be removed to make way for the new one. If the
032 * active command is not interruptible, the other one will not even be started, and the active one
033 * will continue functioning.
034 *
035 * @see Subsystem
036 * @see CommandGroup
037 * @see IllegalUseOfCommandException
038 */
039public abstract class Command implements Sendable, AutoCloseable {
040  /** The time since this command was initialized. */
041  private double m_startTime = -1;
042
043  /** The time (in seconds) before this command "times out" (or -1 if no timeout). */
044  private double m_timeout = -1;
045
046  /** Whether or not this command has been initialized. */
047  private boolean m_initialized;
048
049  /** The required subsystems. */
050  private final Set m_requirements = new Set();
051
052  /** Whether or not it is running. */
053  private boolean m_running;
054
055  /** Whether or not it is interruptible. */
056  private boolean m_interruptible = true;
057
058  /** Whether or not it has been canceled. */
059  private boolean m_canceled;
060
061  /** Whether or not it has been locked. */
062  private boolean m_locked;
063
064  /** Whether this command should run when the robot is disabled. */
065  private boolean m_runWhenDisabled;
066
067  /** Whether or not this command has completed running. */
068  private boolean m_completed;
069
070  /** The {@link CommandGroup} this is in. */
071  private CommandGroup m_parent;
072
073  /** Creates a new command. The name of this command will be set to its class name. */
074  public Command() {
075    String name = getClass().getName();
076    SendableRegistry.add(this, name.substring(name.lastIndexOf('.') + 1));
077  }
078
079  /**
080   * Creates a new command with the given name.
081   *
082   * @param name the name for this command
083   * @throws IllegalArgumentException if name is null
084   */
085  public Command(String name) {
086    if (name == null) {
087      throw new IllegalArgumentException("Name must not be null.");
088    }
089    SendableRegistry.add(this, name);
090  }
091
092  /**
093   * Creates a new command with the given timeout and a default name. The default name is the name
094   * of the class.
095   *
096   * @param timeout the time (in seconds) before this command "times out"
097   * @throws IllegalArgumentException if given a negative timeout
098   * @see Command#isTimedOut() isTimedOut()
099   */
100  public Command(double timeout) {
101    this();
102    if (timeout < 0) {
103      throw new IllegalArgumentException("Timeout must not be negative.  Given:" + timeout);
104    }
105    m_timeout = timeout;
106  }
107
108  /**
109   * Creates a new command with the given timeout and a default name. The default name is the name
110   * of the class.
111   *
112   * @param subsystem the subsystem that this command requires
113   * @throws IllegalArgumentException if given a negative timeout
114   * @see Command#isTimedOut() isTimedOut()
115   */
116  public Command(Subsystem subsystem) {
117    this();
118    requires(subsystem);
119  }
120
121  /**
122   * Creates a new command with the given name.
123   *
124   * @param name the name for this command
125   * @param subsystem the subsystem that this command requires
126   * @throws IllegalArgumentException if name is null
127   */
128  public Command(String name, Subsystem subsystem) {
129    this(name);
130    requires(subsystem);
131  }
132
133  /**
134   * Creates a new command with the given timeout and a default name. The default name is the name
135   * of the class.
136   *
137   * @param timeout the time (in seconds) before this command "times out"
138   * @param subsystem the subsystem that this command requires
139   * @throws IllegalArgumentException if given a negative timeout
140   * @see Command#isTimedOut() isTimedOut()
141   */
142  public Command(double timeout, Subsystem subsystem) {
143    this(timeout);
144    requires(subsystem);
145  }
146
147  /**
148   * Creates a new command with the given name and timeout.
149   *
150   * @param name the name of the command
151   * @param timeout the time (in seconds) before this command "times out"
152   * @throws IllegalArgumentException if given a negative timeout or name was null.
153   * @see Command#isTimedOut() isTimedOut()
154   */
155  public Command(String name, double timeout) {
156    this(name);
157    if (timeout < 0) {
158      throw new IllegalArgumentException("Timeout must not be negative.  Given:" + timeout);
159    }
160    m_timeout = timeout;
161  }
162
163  /**
164   * Creates a new command with the given name and timeout.
165   *
166   * @param name the name of the command
167   * @param timeout the time (in seconds) before this command "times out"
168   * @param subsystem the subsystem that this command requires
169   * @throws IllegalArgumentException if given a negative timeout
170   * @throws IllegalArgumentException if given a negative timeout or name was null.
171   * @see Command#isTimedOut() isTimedOut()
172   */
173  public Command(String name, double timeout, Subsystem subsystem) {
174    this(name, timeout);
175    requires(subsystem);
176  }
177
178  @Override
179  public void close() {
180    SendableRegistry.remove(this);
181  }
182
183  /**
184   * Sets the timeout of this command.
185   *
186   * @param seconds the timeout (in seconds)
187   * @throws IllegalArgumentException if seconds is negative
188   * @see Command#isTimedOut() isTimedOut()
189   */
190  protected final synchronized void setTimeout(double seconds) {
191    if (seconds < 0) {
192      throw new IllegalArgumentException("Seconds must be positive.  Given:" + seconds);
193    }
194    m_timeout = seconds;
195  }
196
197  /**
198   * Returns the time since this command was initialized (in seconds). This function will work even
199   * if there is no specified timeout.
200   *
201   * @return the time since this command was initialized (in seconds).
202   */
203  public final synchronized double timeSinceInitialized() {
204    return m_startTime < 0 ? 0 : Timer.getFPGATimestamp() - m_startTime;
205  }
206
207  /**
208   * This method specifies that the given {@link Subsystem} is used by this command. This method is
209   * crucial to the functioning of the Command System in general.
210   *
211   * <p>Note that the recommended way to call this method is in the constructor.
212   *
213   * @param subsystem the {@link Subsystem} required
214   * @throws IllegalArgumentException if subsystem is null
215   * @throws IllegalUseOfCommandException if this command has started before or if it has been given
216   *     to a {@link CommandGroup}
217   * @see Subsystem
218   */
219  protected synchronized void requires(Subsystem subsystem) {
220    validate("Can not add new requirement to command");
221    if (subsystem != null) {
222      m_requirements.add(subsystem);
223    } else {
224      throw new IllegalArgumentException("Subsystem must not be null.");
225    }
226  }
227
228  /**
229   * Called when the command has been removed. This will call {@link Command#interrupted()
230   * interrupted()} or {@link Command#end() end()}.
231   */
232  synchronized void removed() {
233    if (m_initialized) {
234      if (isCanceled()) {
235        interrupted();
236        _interrupted();
237      } else {
238        end();
239        _end();
240      }
241    }
242    m_initialized = false;
243    m_canceled = false;
244    m_running = false;
245    m_completed = true;
246  }
247
248  /**
249   * The run method is used internally to actually run the commands.
250   *
251   * @return whether or not the command should stay within the {@link Scheduler}.
252   */
253  synchronized boolean run() {
254    if (!m_runWhenDisabled && m_parent == null && RobotState.isDisabled()) {
255      cancel();
256    }
257    if (isCanceled()) {
258      return false;
259    }
260    if (!m_initialized) {
261      m_initialized = true;
262      startTiming();
263      _initialize();
264      initialize();
265    }
266    _execute();
267    execute();
268    return !isFinished();
269  }
270
271  /** The initialize method is called the first time this Command is run after being started. */
272  protected void initialize() {}
273
274  /** A shadow method called before {@link Command#initialize() initialize()}. */
275  @SuppressWarnings("MethodName")
276  void _initialize() {}
277
278  /** The execute method is called repeatedly until this Command either finishes or is canceled. */
279  @SuppressWarnings("MethodName")
280  protected void execute() {}
281
282  /** A shadow method called before {@link Command#execute() execute()}. */
283  @SuppressWarnings("MethodName")
284  void _execute() {}
285
286  /**
287   * Returns whether this command is finished. If it is, then the command will be removed and {@link
288   * Command#end() end()} will be called.
289   *
290   * <p>It may be useful for a team to reference the {@link Command#isTimedOut() isTimedOut()}
291   * method for time-sensitive commands.
292   *
293   * <p>Returning false will result in the command never ending automatically. It may still be
294   * canceled manually or interrupted by another command. Returning true will result in the command
295   * executing once and finishing immediately. We recommend using {@link InstantCommand} for this.
296   *
297   * @return whether this command is finished.
298   * @see Command#isTimedOut() isTimedOut()
299   */
300  protected abstract boolean isFinished();
301
302  /**
303   * Called when the command ended peacefully. This is where you may want to wrap up loose ends,
304   * like shutting off a motor that was being used in the command.
305   */
306  protected void end() {}
307
308  /** A shadow method called after {@link Command#end() end()}. */
309  @SuppressWarnings("MethodName")
310  void _end() {}
311
312  /**
313   * Called when the command ends because somebody called {@link Command#cancel() cancel()} or
314   * another command shared the same requirements as this one, and booted it out.
315   *
316   * <p>This is where you may want to wrap up loose ends, like shutting off a motor that was being
317   * used in the command.
318   *
319   * <p>Generally, it is useful to simply call the {@link Command#end() end()} method within this
320   * method, as done here.
321   */
322  protected void interrupted() {
323    end();
324  }
325
326  /** A shadow method called after {@link Command#interrupted() interrupted()}. */
327  @SuppressWarnings("MethodName")
328  void _interrupted() {}
329
330  /**
331   * Called to indicate that the timer should start. This is called right before {@link
332   * Command#initialize() initialize()} is, inside the {@link Command#run() run()} method.
333   */
334  private void startTiming() {
335    m_startTime = Timer.getFPGATimestamp();
336  }
337
338  /**
339   * Returns whether or not the {@link Command#timeSinceInitialized() timeSinceInitialized()} method
340   * returns a number which is greater than or equal to the timeout for the command. If there is no
341   * timeout, this will always return false.
342   *
343   * @return whether the time has expired
344   */
345  protected synchronized boolean isTimedOut() {
346    return m_timeout != -1 && timeSinceInitialized() >= m_timeout;
347  }
348
349  /**
350   * Returns the requirements (as an {@link Enumeration Enumeration} of {@link Subsystem
351   * Subsystems}) of this command.
352   *
353   * @return the requirements (as an {@link Enumeration Enumeration} of {@link Subsystem
354   *     Subsystems}) of this command
355   */
356  synchronized Enumeration<?> getRequirements() {
357    return m_requirements.getElements();
358  }
359
360  /** Prevents further changes from being made. */
361  synchronized void lockChanges() {
362    m_locked = true;
363  }
364
365  /**
366   * If changes are locked, then this will throw an {@link IllegalUseOfCommandException}.
367   *
368   * @param message the message to say (it is appended by a default message)
369   */
370  synchronized void validate(String message) {
371    if (m_locked) {
372      throw new IllegalUseOfCommandException(
373          message + " after being started or being added to a command group");
374    }
375  }
376
377  /**
378   * Sets the parent of this command. No actual change is made to the group.
379   *
380   * @param parent the parent
381   * @throws IllegalUseOfCommandException if this {@link Command} already is already in a group
382   */
383  synchronized void setParent(CommandGroup parent) {
384    if (m_parent != null) {
385      throw new IllegalUseOfCommandException(
386          "Can not give command to a command group after already being put in a command group");
387    }
388    lockChanges();
389    m_parent = parent;
390  }
391
392  /**
393   * Returns whether the command has a parent.
394   *
395   * @return true if the command has a parent.
396   */
397  synchronized boolean isParented() {
398    return m_parent != null;
399  }
400
401  /**
402   * Clears list of subsystem requirements. This is only used by {@link ConditionalCommand} so
403   * canceling the chosen command works properly in {@link CommandGroup}.
404   */
405  protected void clearRequirements() {
406    m_requirements.clear();
407  }
408
409  /**
410   * Starts up the command. Gets the command ready to start.
411   *
412   * <p>Note that the command will eventually start, however it will not necessarily do so
413   * immediately, and may in fact be canceled before initialize is even called.
414   *
415   * @throws IllegalUseOfCommandException if the command is a part of a CommandGroup
416   */
417  public synchronized void start() {
418    lockChanges();
419    if (m_parent != null) {
420      throw new IllegalUseOfCommandException(
421          "Can not start a command that is a part of a command group");
422    }
423    Scheduler.getInstance().add(this);
424    m_completed = false;
425  }
426
427  /**
428   * This is used internally to mark that the command has been started. The lifecycle of a command
429   * is:
430   *
431   * <p>startRunning() is called. run() is called (multiple times potentially) removed() is called.
432   *
433   * <p>It is very important that startRunning and removed be called in order or some assumptions of
434   * the code will be broken.
435   */
436  synchronized void startRunning() {
437    m_running = true;
438    m_startTime = -1;
439  }
440
441  /**
442   * Returns whether or not the command is running. This may return true even if the command has
443   * just been canceled, as it may not have yet called {@link Command#interrupted()}.
444   *
445   * @return whether or not the command is running
446   */
447  public synchronized boolean isRunning() {
448    return m_running;
449  }
450
451  /**
452   * This will cancel the current command.
453   *
454   * <p>This will cancel the current command eventually. It can be called multiple times. And it can
455   * be called when the command is not running. If the command is running though, then the command
456   * will be marked as canceled and eventually removed.
457   *
458   * <p>A command can not be canceled if it is a part of a command group, you must cancel the
459   * command group instead.
460   *
461   * @throws IllegalUseOfCommandException if this command is a part of a command group
462   */
463  public synchronized void cancel() {
464    if (m_parent != null) {
465      throw new IllegalUseOfCommandException(
466          "Can not manually cancel a command in a command " + "group");
467    }
468    _cancel();
469  }
470
471  /**
472   * This works like cancel(), except that it doesn't throw an exception if it is a part of a
473   * command group. Should only be called by the parent command group.
474   */
475  @SuppressWarnings("MethodName")
476  synchronized void _cancel() {
477    if (isRunning()) {
478      m_canceled = true;
479    }
480  }
481
482  /**
483   * Returns whether or not this has been canceled.
484   *
485   * @return whether or not this has been canceled
486   */
487  public synchronized boolean isCanceled() {
488    return m_canceled;
489  }
490
491  /**
492   * Whether or not this command has completed running.
493   *
494   * @return whether or not this command has completed running.
495   */
496  public synchronized boolean isCompleted() {
497    return m_completed;
498  }
499
500  /**
501   * Returns whether or not this command can be interrupted.
502   *
503   * @return whether or not this command can be interrupted
504   */
505  public synchronized boolean isInterruptible() {
506    return m_interruptible;
507  }
508
509  /**
510   * Sets whether or not this command can be interrupted.
511   *
512   * @param interruptible whether or not this command can be interrupted
513   */
514  protected synchronized void setInterruptible(boolean interruptible) {
515    m_interruptible = interruptible;
516  }
517
518  /**
519   * Checks if the command requires the given {@link Subsystem}.
520   *
521   * @param system the system
522   * @return whether or not the subsystem is required, or false if given null
523   */
524  public synchronized boolean doesRequire(Subsystem system) {
525    return m_requirements.contains(system);
526  }
527
528  /**
529   * Returns the {@link CommandGroup} that this command is a part of. Will return null if this
530   * {@link Command} is not in a group.
531   *
532   * @return the {@link CommandGroup} that this command is a part of (or null if not in group)
533   */
534  public synchronized CommandGroup getGroup() {
535    return m_parent;
536  }
537
538  /**
539   * Sets whether or not this {@link Command} should run when the robot is disabled.
540   *
541   * <p>By default a command will not run when the robot is disabled, and will in fact be canceled.
542   *
543   * @param run whether or not this command should run when the robot is disabled
544   */
545  public void setRunWhenDisabled(boolean run) {
546    m_runWhenDisabled = run;
547  }
548
549  /**
550   * Returns whether or not this {@link Command} will run when the robot is disabled, or if it will
551   * cancel itself.
552   *
553   * @return True if this command will run when the robot is disabled.
554   */
555  public boolean willRunWhenDisabled() {
556    return m_runWhenDisabled;
557  }
558
559  /**
560   * Gets the name of this Command.
561   *
562   * @return Name
563   */
564  public String getName() {
565    return SendableRegistry.getName(this);
566  }
567
568  /**
569   * Sets the name of this Command.
570   *
571   * @param name name
572   */
573  public void setName(String name) {
574    SendableRegistry.setName(this, name);
575  }
576
577  /**
578   * Gets the subsystem name of this Command.
579   *
580   * @return Subsystem name
581   */
582  public String getSubsystem() {
583    return SendableRegistry.getSubsystem(this);
584  }
585
586  /**
587   * Sets the subsystem name of this Command.
588   *
589   * @param subsystem subsystem name
590   */
591  public void setSubsystem(String subsystem) {
592    SendableRegistry.setSubsystem(this, subsystem);
593  }
594
595  /**
596   * The string representation for a {@link Command} is by default its name.
597   *
598   * @return the string representation of this object
599   */
600  @Override
601  public String toString() {
602    return getName();
603  }
604
605  @Override
606  public void initSendable(SendableBuilder builder) {
607    builder.setSmartDashboardType("Command");
608    builder.addStringProperty(".name", this::getName, null);
609    builder.addBooleanProperty(
610        "running",
611        this::isRunning,
612        value -> {
613          if (value) {
614            if (!isRunning()) {
615              start();
616            }
617          } else {
618            if (isRunning()) {
619              cancel();
620            }
621          }
622        });
623    builder.addBooleanProperty(".isParented", this::isParented, null);
624  }
625}