/**
* Removes the {@link Command} from the {@link Scheduler}.
*
* @param command the command to remove
*/
void Scheduler::Remove(Command* command) {
if (command == nullptr) {
wpi_setWPIErrorWithContext(NullParameter, "command");
return;
}
if (!m_commands.erase(command)) return;
Command::SubsystemSet requirements = command->GetRequirements();
auto iter = requirements.begin();
for (; iter != requirements.end(); iter++) {
Subsystem* lock = *iter;
lock->SetCurrentCommand(nullptr);
}
command->Removed();
}
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;
}
}