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 package edu.wpi.first.wpilibj.command;
008
009 import edu.wpi.first.wpilibj.NamedSendable;
010 import edu.wpi.first.wpilibj.buttons.Trigger.ButtonScheduler;
011 import edu.wpi.first.wpilibj.communication.UsageReporting;
012 import edu.wpi.first.wpilibj.networktables2.type.NumberArray;
013 import edu.wpi.first.wpilibj.networktables2.type.StringArray;
014 import edu.wpi.first.wpilibj.tables.ITable;
015 import java.util.Enumeration;
016 import java.util.Hashtable;
017 import java.util.Vector;
018
019 /**
020 * The {@link Scheduler} is a singleton which holds the top-level running
021 * commands. It is in charge of both calling the command's
022 * {@link Command#run() run()} method and to make sure that there are no two
023 * commands with conflicting requirements running.
024 *
025 * <p>It is fine if teams wish to take control of the {@link Scheduler}
026 * themselves, all that needs to be done is to call
027 * {@link Scheduler#getInstance() Scheduler.getInstance()}.{@link Scheduler#run() run()}
028 * often to have {@link Command Commands} function correctly. However, this is
029 * already done for you if you use the CommandBased Robot template.</p>
030 *
031 * @author Joe Grinstead
032 * @see Command
033 */
034 public class Scheduler implements NamedSendable {
035
036 /**
037 * The Singleton Instance
038 */
039 private static Scheduler instance;
040
041 /**
042 * Returns the {@link Scheduler}, creating it if one does not exist.
043 *
044 * @return the {@link Scheduler}
045 */
046 public synchronized static Scheduler getInstance() {
047 return instance == null ? instance = new Scheduler() : instance;
048 }
049 /**
050 * A hashtable of active {@link Command Commands} to their
051 * {@link LinkedListElement}
052 */
053 private Hashtable commandTable = new Hashtable();
054 /**
055 * The {@link Set} of all {@link Subsystem Subsystems}
056 */
057 private Set subsystems = new Set();
058 /**
059 * The first {@link Command} in the list
060 */
061 private LinkedListElement firstCommand;
062 /**
063 * The last {@link Command} in the list
064 */
065 private LinkedListElement lastCommand;
066 /**
067 * Whether or not we are currently adding a command
068 */
069 private boolean adding = false;
070 /**
071 * Whether or not we are currently disabled
072 */
073 private boolean disabled = false;
074 /**
075 * A list of all {@link Command Commands} which need to be added
076 */
077 private Vector additions = new Vector();
078 private ITable m_table;
079 /**
080 * A list of all
081 * {@link edu.wpi.first.wpilibj.buttons.Trigger.ButtonScheduler Buttons}. It
082 * is created lazily.
083 */
084 private Vector buttons;
085 private boolean m_runningCommandsChanged;
086
087 /**
088 * Instantiates a {@link Scheduler}.
089 */
090 private Scheduler() {
091 UsageReporting.report(UsageReporting.kResourceType_Command, UsageReporting.kCommand_Scheduler);
092 }
093
094 /**
095 * Adds the command to the {@link Scheduler}. This will not add the
096 * {@link Command} immediately, but will instead wait for the proper time in
097 * the {@link Scheduler#run()} loop before doing so. The command returns
098 * immediately and does nothing if given null.
099 *
100 * <p>Adding a {@link Command} to the {@link Scheduler} involves the
101 * {@link Scheduler} removing any {@link Command} which has shared
102 * requirements.</p>
103 *
104 * @param command the command to add
105 */
106 public void add(Command command) {
107 if (command != null) {
108 additions.addElement(command);
109 }
110 }
111
112 /**
113 * Adds a button to the {@link Scheduler}. The {@link Scheduler} will poll
114 * the button during its {@link Scheduler#run()}.
115 *
116 * @param button the button to add
117 */
118 public void addButton(ButtonScheduler button) {
119 if (buttons == null) {
120 buttons = new Vector();
121 }
122 buttons.addElement(button);
123 }
124
125 /**
126 * Adds a command immediately to the {@link Scheduler}. This should only be
127 * called in the {@link Scheduler#run()} loop. Any command with conflicting
128 * requirements will be removed, unless it is uninterruptable. Giving
129 * <code>null</code> does nothing.
130 *
131 * @param command the {@link Command} to add
132 */
133 private void _add(Command command) {
134 if (command == null) {
135 return;
136 }
137
138 // Check to make sure no adding during adding
139 if (adding) {
140 System.err.println("WARNING: Can not start command from cancel method. Ignoring:" + command);
141 return;
142 }
143
144 // Only add if not already in
145 if (!commandTable.containsKey(command)) {
146
147 // Check that the requirements can be had
148 Enumeration requirements = command.getRequirements();
149 while (requirements.hasMoreElements()) {
150 Subsystem lock = (Subsystem) requirements.nextElement();
151 if (lock.getCurrentCommand() != null && !lock.getCurrentCommand().isInterruptible()) {
152 return;
153 }
154 }
155
156 // Give it the requirements
157 adding = true;
158 requirements = command.getRequirements();
159 while (requirements.hasMoreElements()) {
160 Subsystem lock = (Subsystem) requirements.nextElement();
161 if (lock.getCurrentCommand() != null) {
162 lock.getCurrentCommand().cancel();
163 remove(lock.getCurrentCommand());
164 }
165 lock.setCurrentCommand(command);
166 }
167 adding = false;
168
169 // Add it to the list
170 LinkedListElement element = new LinkedListElement();
171 element.setData(command);
172 if (firstCommand == null) {
173 firstCommand = lastCommand = element;
174 } else {
175 lastCommand.add(element);
176 lastCommand = element;
177 }
178 commandTable.put(command, element);
179
180 m_runningCommandsChanged = true;
181
182 command.startRunning();
183 }
184 }
185
186 /**
187 * Runs a single iteration of the loop. This method should be called often
188 * in order to have a functioning {@link Command} system. The loop has five
189 * stages:
190 *
191 * <ol> <li> Poll the Buttons </li> <li> Execute/Remove the Commands </li>
192 * <li> Send values to SmartDashboard </li> <li> Add Commands </li> <li> Add
193 * Defaults </li> </ol>
194 */
195 public void run() {
196
197 m_runningCommandsChanged = false;
198
199 if (disabled) {
200 return;
201 } // Don't run when disabled
202
203 // Get button input (going backwards preserves button priority)
204 if (buttons != null) {
205 for (int i = buttons.size() - 1; i >= 0; i--) {
206 ((ButtonScheduler) buttons.elementAt(i)).execute();
207 }
208 }
209 // Loop through the commands
210 LinkedListElement e = firstCommand;
211 while (e != null) {
212 Command c = e.getData();
213 e = e.getNext();
214 if (!c.run()) {
215 remove(c);
216 m_runningCommandsChanged = true;
217 }
218 }
219
220 // Add the new things
221 for (int i = 0; i < additions.size(); i++) {
222 _add((Command) additions.elementAt(i));
223 }
224 additions.removeAllElements();
225
226 // Add in the defaults
227 Enumeration locks = subsystems.getElements();
228 while (locks.hasMoreElements()) {
229 Subsystem lock = (Subsystem) locks.nextElement();
230 if (lock.getCurrentCommand() == null) {
231 _add(lock.getDefaultCommand());
232 }
233 lock.confirmCommand();
234 }
235
236 updateTable();
237 }
238
239 /**
240 * Registers a {@link Subsystem} to this {@link Scheduler}, so that the
241 * {@link Scheduler} might know if a default {@link Command} needs to be
242 * run. All {@link Subsystem Subsystems} should call this.
243 *
244 * @param system the system
245 */
246 void registerSubsystem(Subsystem system) {
247 if (system != null) {
248 subsystems.add(system);
249 }
250 }
251
252 /**
253 * Removes the {@link Command} from the {@link Scheduler}.
254 *
255 * @param command the command to remove
256 */
257 void remove(Command command) {
258 if (command == null || !commandTable.containsKey(command)) {
259 return;
260 }
261 LinkedListElement e = (LinkedListElement) commandTable.get(command);
262 commandTable.remove(command);
263
264 if (e.equals(lastCommand)) {
265 lastCommand = e.getPrevious();
266 }
267 if (e.equals(firstCommand)) {
268 firstCommand = e.getNext();
269 }
270 e.remove();
271
272 Enumeration requirements = command.getRequirements();
273 while (requirements.hasMoreElements()) {
274 ((Subsystem) requirements.nextElement()).setCurrentCommand(null);
275 }
276
277 command.removed();
278 }
279
280 /**
281 * Removes all commands
282 */
283 public void removeAll() {
284 // TODO: Confirm that this works with "uninteruptible" commands
285 while (firstCommand != null) {
286 remove(firstCommand.getData());
287 }
288 }
289
290 /**
291 * Disable the command scheduler.
292 */
293 public void disable() {
294 disabled = true;
295 }
296
297 /**
298 * Enable the command scheduler.
299 */
300 public void enable() {
301 disabled = false;
302 }
303
304 public String getName() {
305 return "Scheduler";
306 }
307
308 public String getType() {
309 return "Scheduler";
310 }
311 private StringArray commands;
312 private NumberArray ids, toCancel;
313
314 /**
315 * {@inheritDoc}
316 */
317 public void initTable(ITable subtable) {
318 m_table = subtable;
319 commands = new StringArray();
320 ids = new NumberArray();
321 toCancel = new NumberArray();
322
323 m_table.putValue("Names", commands);
324 m_table.putValue("Ids", ids);
325 m_table.putValue("Cancel", toCancel);
326 }
327
328 private void updateTable() {
329 if (m_table != null) {
330 // Get the commands to cancel
331 m_table.retrieveValue("Cancel", toCancel);
332 if (toCancel.size() > 0) {
333 for (LinkedListElement e = firstCommand; e != null; e = e.getNext()) {
334 for (int i = 0; i < toCancel.size(); i++) {
335 if (e.getData().hashCode() == toCancel.get(i)) {
336 e.getData().cancel();
337 }
338 }
339 }
340 toCancel.setSize(0);
341 m_table.putValue("Cancel", toCancel);
342 }
343
344 if (m_runningCommandsChanged) {
345 commands.setSize(0);
346 ids.setSize(0);
347 // Set the the running commands
348 for (LinkedListElement e = firstCommand; e != null; e = e.getNext()) {
349 commands.add(e.getData().getName());
350 ids.add(e.getData().hashCode());
351 }
352 m_table.putValue("Names", commands);
353 m_table.putValue("Ids", ids);
354 }
355 }
356 }
357
358 /**
359 * {@inheritDoc}
360 */
361 public ITable getTable() {
362 return m_table;
363 }
364
365 public String getSmartDashboardType() {
366 return "Scheduler";
367 }
368 }