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 static java.util.Objects.requireNonNull;
008
009import java.util.Enumeration;
010import java.util.Vector;
011
012/**
013 * A {@link CommandGroup} is a list of commands which are executed in sequence.
014 *
015 * <p>Commands in a {@link CommandGroup} are added using the {@link
016 * CommandGroup#addSequential(Command) addSequential(...)} method and are called sequentially.
017 * {@link CommandGroup CommandGroups} are themselves {@link Command commands} and can be given to
018 * other {@link CommandGroup CommandGroups}.
019 *
020 * <p>{@link CommandGroup CommandGroups} will carry all of the requirements of their {@link Command
021 * subcommands}. Additional requirements can be specified by calling {@link
022 * CommandGroup#requires(Subsystem) requires(...)} normally in the constructor.
023 *
024 * <p>CommandGroups can also execute commands in parallel, simply by adding them using {@link
025 * CommandGroup#addParallel(Command) addParallel(...)}.
026 *
027 * @see Command
028 * @see Subsystem
029 * @see IllegalUseOfCommandException
030 */
031public class CommandGroup extends Command {
032  /** The commands in this group (stored in entries). */
033  @SuppressWarnings("PMD.UseArrayListInsteadOfVector")
034  private final Vector<Entry> m_commands = new Vector<>();
035  /*
036   * Intentionally package private
037   */
038  /** The active children in this group (stored in entries). */
039  @SuppressWarnings("PMD.UseArrayListInsteadOfVector")
040  final Vector<Entry> m_children = new Vector<>();
041  /** The current command, -1 signifies that none have been run. */
042  private int m_currentCommandIndex = -1;
043
044  /**
045   * Creates a new {@link CommandGroup CommandGroup}. The name of this command will be set to its
046   * class name.
047   */
048  public CommandGroup() {}
049
050  /**
051   * Creates a new {@link CommandGroup CommandGroup} with the given name.
052   *
053   * @param name the name for this command group
054   * @throws IllegalArgumentException if name is null
055   */
056  public CommandGroup(String name) {
057    super(name);
058  }
059
060  /**
061   * Adds a new {@link Command Command} to the group. The {@link Command Command} will be started
062   * after all the previously added {@link Command Commands}.
063   *
064   * <p>Note that any requirements the given {@link Command Command} has will be added to the group.
065   * For this reason, a {@link Command Command's} requirements can not be changed after being added
066   * to a group.
067   *
068   * <p>It is recommended that this method be called in the constructor.
069   *
070   * @param command The {@link Command Command} to be added
071   * @throws IllegalUseOfCommandException if the group has been started before or been given to
072   *     another group
073   * @throws IllegalArgumentException if command is null
074   */
075  public final synchronized void addSequential(Command command) {
076    validate("Can not add new command to command group");
077    if (command == null) {
078      throw new IllegalArgumentException("Given null command");
079    }
080
081    command.setParent(this);
082
083    m_commands.addElement(new Entry(command, Entry.IN_SEQUENCE));
084    for (Enumeration<?> e = command.getRequirements(); e.hasMoreElements(); ) {
085      requires((Subsystem) e.nextElement());
086    }
087  }
088
089  /**
090   * Adds a new {@link Command Command} to the group with a given timeout. The {@link Command
091   * Command} will be started after all the previously added commands.
092   *
093   * <p>Once the {@link Command Command} is started, it will be run until it finishes or the time
094   * expires, whichever is sooner. Note that the given {@link Command Command} will have no
095   * knowledge that it is on a timer.
096   *
097   * <p>Note that any requirements the given {@link Command Command} has will be added to the group.
098   * For this reason, a {@link Command Command's} requirements can not be changed after being added
099   * to a group.
100   *
101   * <p>It is recommended that this method be called in the constructor.
102   *
103   * @param command The {@link Command Command} to be added
104   * @param timeout The timeout (in seconds)
105   * @throws IllegalUseOfCommandException if the group has been started before or been given to
106   *     another group or if the {@link Command Command} has been started before or been given to
107   *     another group
108   * @throws IllegalArgumentException if command is null or timeout is negative
109   */
110  public final synchronized void addSequential(Command command, double timeout) {
111    validate("Can not add new command to command group");
112    if (command == null) {
113      throw new IllegalArgumentException("Given null command");
114    }
115    if (timeout < 0) {
116      throw new IllegalArgumentException("Can not be given a negative timeout");
117    }
118
119    command.setParent(this);
120
121    m_commands.addElement(new Entry(command, Entry.IN_SEQUENCE, timeout));
122    for (Enumeration<?> e = command.getRequirements(); e.hasMoreElements(); ) {
123      requires((Subsystem) e.nextElement());
124    }
125  }
126
127  /**
128   * Adds a new child {@link Command} to the group. The {@link Command} will be started after all
129   * the previously added {@link Command Commands}.
130   *
131   * <p>Instead of waiting for the child to finish, a {@link CommandGroup} will have it run at the
132   * same time as the subsequent {@link Command Commands}. The child will run until either it
133   * finishes, a new child with conflicting requirements is started, or the main sequence runs a
134   * {@link Command} with conflicting requirements. In the latter two cases, the child will be
135   * canceled even if it says it can't be interrupted.
136   *
137   * <p>Note that any requirements the given {@link Command Command} has will be added to the group.
138   * For this reason, a {@link Command Command's} requirements can not be changed after being added
139   * to a group.
140   *
141   * <p>It is recommended that this method be called in the constructor.
142   *
143   * @param command The command to be added
144   * @throws IllegalUseOfCommandException if the group has been started before or been given to
145   *     another command group
146   * @throws IllegalArgumentException if command is null
147   */
148  public final synchronized void addParallel(Command command) {
149    requireNonNull(command, "Provided command was null");
150    validate("Can not add new command to command group");
151
152    command.setParent(this);
153
154    m_commands.addElement(new Entry(command, Entry.BRANCH_CHILD));
155    for (Enumeration<?> e = command.getRequirements(); e.hasMoreElements(); ) {
156      requires((Subsystem) e.nextElement());
157    }
158  }
159
160  /**
161   * Adds a new child {@link Command} to the group with the given timeout. The {@link Command} will
162   * be started after all the previously added {@link Command Commands}.
163   *
164   * <p>Once the {@link Command Command} is started, it will run until it finishes, is interrupted,
165   * or the time expires, whichever is sooner. Note that the given {@link Command Command} will have
166   * no knowledge that it is on a timer.
167   *
168   * <p>Instead of waiting for the child to finish, a {@link CommandGroup} will have it run at the
169   * same time as the subsequent {@link Command Commands}. The child will run until either it
170   * finishes, the timeout expires, a new child with conflicting requirements is started, or the
171   * main sequence runs a {@link Command} with conflicting requirements. In the latter two cases,
172   * the child will be canceled even if it says it can't be interrupted.
173   *
174   * <p>Note that any requirements the given {@link Command Command} has will be added to the group.
175   * For this reason, a {@link Command Command's} requirements can not be changed after being added
176   * to a group.
177   *
178   * <p>It is recommended that this method be called in the constructor.
179   *
180   * @param command The command to be added
181   * @param timeout The timeout (in seconds)
182   * @throws IllegalUseOfCommandException if the group has been started before or been given to
183   *     another command group
184   * @throws IllegalArgumentException if command is null
185   */
186  public final synchronized void addParallel(Command command, double timeout) {
187    requireNonNull(command, "Provided command was null");
188    if (timeout < 0) {
189      throw new IllegalArgumentException("Can not be given a negative timeout");
190    }
191    validate("Can not add new command to command group");
192
193    command.setParent(this);
194
195    m_commands.addElement(new Entry(command, Entry.BRANCH_CHILD, timeout));
196    for (Enumeration<?> e = command.getRequirements(); e.hasMoreElements(); ) {
197      requires((Subsystem) e.nextElement());
198    }
199  }
200
201  @Override
202  @SuppressWarnings("MethodName")
203  void _initialize() {
204    m_currentCommandIndex = -1;
205  }
206
207  @Override
208  @SuppressWarnings({"MethodName", "PMD.AvoidReassigningLoopVariables"})
209  void _execute() {
210    Entry entry = null;
211    Command cmd = null;
212    boolean firstRun = false;
213    if (m_currentCommandIndex == -1) {
214      firstRun = true;
215      m_currentCommandIndex = 0;
216    }
217
218    while (m_currentCommandIndex < m_commands.size()) {
219      if (cmd != null) {
220        if (entry.isTimedOut()) {
221          cmd._cancel();
222        }
223        if (cmd.run()) {
224          break;
225        } else {
226          cmd.removed();
227          m_currentCommandIndex++;
228          firstRun = true;
229          cmd = null;
230          continue;
231        }
232      }
233
234      entry = m_commands.elementAt(m_currentCommandIndex);
235      cmd = null;
236
237      switch (entry.m_state) {
238        case Entry.IN_SEQUENCE:
239          cmd = entry.m_command;
240          if (firstRun) {
241            cmd.startRunning();
242            cancelConflicts(cmd);
243          }
244          firstRun = false;
245          break;
246        case Entry.BRANCH_PEER:
247          m_currentCommandIndex++;
248          entry.m_command.start();
249          break;
250        case Entry.BRANCH_CHILD:
251          m_currentCommandIndex++;
252          cancelConflicts(entry.m_command);
253          entry.m_command.startRunning();
254          m_children.addElement(entry);
255          break;
256        default:
257          break;
258      }
259    }
260
261    // Run Children
262    for (int i = 0; i < m_children.size(); i++) {
263      entry = m_children.elementAt(i);
264      Command child = entry.m_command;
265      if (entry.isTimedOut()) {
266        child._cancel();
267      }
268      if (!child.run()) {
269        child.removed();
270        m_children.removeElementAt(i--);
271      }
272    }
273  }
274
275  @Override
276  @SuppressWarnings("MethodName")
277  void _end() {
278    // Theoretically, we don't have to check this, but we do if teams override
279    // the isFinished method
280    if (m_currentCommandIndex != -1 && m_currentCommandIndex < m_commands.size()) {
281      Command cmd = m_commands.elementAt(m_currentCommandIndex).m_command;
282      cmd._cancel();
283      cmd.removed();
284    }
285
286    Enumeration<?> children = m_children.elements();
287    while (children.hasMoreElements()) {
288      Command cmd = ((Entry) children.nextElement()).m_command;
289      cmd._cancel();
290      cmd.removed();
291    }
292    m_children.removeAllElements();
293  }
294
295  @Override
296  @SuppressWarnings("MethodName")
297  void _interrupted() {
298    _end();
299  }
300
301  /**
302   * Returns true if all the {@link Command Commands} in this group have been started and have
303   * finished.
304   *
305   * <p>Teams may override this method, although they should probably reference super.isFinished()
306   * if they do.
307   *
308   * @return whether this {@link CommandGroup} is finished
309   */
310  @Override
311  protected boolean isFinished() {
312    return m_currentCommandIndex >= m_commands.size() && m_children.isEmpty();
313  }
314
315  // Can be overwritten by teams
316  @Override
317  protected void initialize() {}
318
319  // Can be overwritten by teams
320  @Override
321  protected void execute() {}
322
323  // Can be overwritten by teams
324  @Override
325  protected void end() {}
326
327  // Can be overwritten by teams
328  @Override
329  protected void interrupted() {}
330
331  /**
332   * Returns whether or not this group is interruptible. A command group will be uninterruptible if
333   * {@link CommandGroup#setInterruptible(boolean) setInterruptible(false)} was called or if it is
334   * currently running an uninterruptible command or child.
335   *
336   * @return whether or not this {@link CommandGroup} is interruptible.
337   */
338  @Override
339  public synchronized boolean isInterruptible() {
340    if (!super.isInterruptible()) {
341      return false;
342    }
343
344    if (m_currentCommandIndex != -1 && m_currentCommandIndex < m_commands.size()) {
345      Command cmd = m_commands.elementAt(m_currentCommandIndex).m_command;
346      if (!cmd.isInterruptible()) {
347        return false;
348      }
349    }
350
351    for (int i = 0; i < m_children.size(); i++) {
352      if (!m_children.elementAt(i).m_command.isInterruptible()) {
353        return false;
354      }
355    }
356
357    return true;
358  }
359
360  @SuppressWarnings("PMD.AvoidReassigningLoopVariables")
361  private void cancelConflicts(Command command) {
362    for (int i = 0; i < m_children.size(); i++) {
363      Command child = m_children.elementAt(i).m_command;
364
365      Enumeration<?> requirements = command.getRequirements();
366
367      while (requirements.hasMoreElements()) {
368        Object requirement = requirements.nextElement();
369        if (child.doesRequire((Subsystem) requirement)) {
370          child._cancel();
371          child.removed();
372          m_children.removeElementAt(i--);
373          break;
374        }
375      }
376    }
377  }
378
379  private static class Entry {
380    private static final int IN_SEQUENCE = 0;
381    private static final int BRANCH_PEER = 1;
382    private static final int BRANCH_CHILD = 2;
383    private final Command m_command;
384    private final int m_state;
385    private final double m_timeout;
386
387    Entry(Command command, int state) {
388      m_command = command;
389      m_state = state;
390      m_timeout = -1;
391    }
392
393    Entry(Command command, int state, double timeout) {
394      m_command = command;
395      m_state = state;
396      m_timeout = timeout;
397    }
398
399    boolean isTimedOut() {
400      if (m_timeout == -1) {
401        return false;
402      } else {
403        double time = m_command.timeSinceInitialized();
404        return time != 0 && time >= m_timeout;
405      }
406    }
407  }
408}