void Scheduler::ProcessCommandAddition(Command* command) { if (command == nullptr) return; // Check to make sure no adding during adding if (m_adding) { wpi_setWPIErrorWithContext(IncompatibleState, "Can not start command from cancel method"); return; } // Only add if not already in auto found = m_commands.find(command); if (found == m_commands.end()) { // Check that the requirements can be had Command::SubsystemSet requirements = command->GetRequirements(); Command::SubsystemSet::iterator iter; for (iter = requirements.begin(); iter != requirements.end(); iter++) { Subsystem* lock = *iter; if (lock->GetCurrentCommand() != nullptr && !lock->GetCurrentCommand()->IsInterruptible()) return; } // Give it the requirements m_adding = true; for (iter = requirements.begin(); iter != requirements.end(); iter++) { Subsystem* lock = *iter; if (lock->GetCurrentCommand() != nullptr) { lock->GetCurrentCommand()->Cancel(); Remove(lock->GetCurrentCommand()); } lock->SetCurrentCommand(command); } m_adding = false; m_commands.insert(command); command->StartRunning(); m_runningCommandsChanged = true; } }
/** * Runs a single iteration of the loop. * * This method should be called often in order to have a functioning * {@link Command} system. The loop has five stages: * * <ol> * <li> Poll the Buttons </li> * <li> Execute/Remove the Commands </li> * <li> Send values to SmartDashboard </li> * <li> Add Commands </li> * <li> Add Defaults </li> * </ol> */ void Scheduler::Run() { // Get button input (going backwards preserves button priority) { if (!m_enabled) return; std::lock_guard<priority_mutex> sync(m_buttonsLock); auto rButtonIter = m_buttons.rbegin(); for (; rButtonIter != m_buttons.rend(); rButtonIter++) { (*rButtonIter)->Execute(); } } m_runningCommandsChanged = false; // Loop through the commands auto commandIter = m_commands.begin(); for (; commandIter != m_commands.end();) { Command* command = *commandIter; // Increment before potentially removing to keep the iterator valid commandIter++; if (!command->Run()) { Remove(command); m_runningCommandsChanged = true; } } // Add the new things { std::lock_guard<priority_mutex> sync(m_additionsLock); auto additionsIter = m_additions.begin(); for (; additionsIter != m_additions.end(); additionsIter++) { ProcessCommandAddition(*additionsIter); } m_additions.clear(); } // Add in the defaults auto subsystemIter = m_subsystems.begin(); for (; subsystemIter != m_subsystems.end(); subsystemIter++) { Subsystem* lock = *subsystemIter; if (lock->GetCurrentCommand() == nullptr) { ProcessCommandAddition(lock->GetDefaultCommand()); } lock->ConfirmCommand(); } UpdateTable(); }