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