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