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();
}