void CommandGroup::CancelConflicts(Command *command)
{
CommandList::iterator childIter = m_children.begin();
for (; childIter != m_children.end();)
{
Command *child = childIter->m_command;
bool erased = false;
Command::SubsystemSet requirements = command->GetRequirements();
Command::SubsystemSet::iterator requirementIter = requirements.begin();
for (; requirementIter != requirements.end(); requirementIter++)
{
if (child->DoesRequire(*requirementIter))
{
child->_Cancel();
child->Removed();
childIter = m_children.erase(childIter);
erased = true;
break;
}
}
if (!erased)
childIter++;
}
}
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;
}
}
/**
* Adds a new {@link Command Command} to the group. The {@link Command Command}
* will be started after all the previously added {@link Command Commands}.
*
* <p>Note that any requirements the given {@link Command Command} has will be
* added to the group. For this reason, a {@link Command Command's}
* requirements can not be changed after being added to a group.</p>
*
* <p>It is recommended that this method be called in the constructor.</p>
*
* @param command The {@link Command Command} to be added
*/
void CommandGroup::AddSequential(Command* command) {
if (command == nullptr) {
wpi_setWPIErrorWithContext(NullParameter, "command");
return;
}
if (!AssertUnlocked("Cannot add new command to command group")) return;
command->SetParent(this);
m_commands.push_back(
CommandGroupEntry(command, CommandGroupEntry::kSequence_InSequence));
// Iterate through command->GetRequirements() and call Requires() on each
// required subsystem
Command::SubsystemSet requirements = command->GetRequirements();
auto iter = requirements.begin();
for (; iter != requirements.end(); iter++) Requires(*iter);
}
/**
* 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();
}
/**
* Sets the default command. If this is not called or is called with null,
* then there will be no default command for the subsystem.
*
* <p><b>WARNING:</b> This should <b>NOT</b> be called in a constructor if the subsystem is a
* singleton.</p>
*
* @param command the default command (or null if there should be none)
*/
void Subsystem::SetDefaultCommand(Command *command)
{
if (command == NULL)
{
m_defaultCommand = NULL;
}
else
{
bool found = false;
Command::SubsystemSet requirements = command->GetRequirements();
Command::SubsystemSet::iterator iter = requirements.begin();
for (; iter != requirements.end(); iter++)
{
if (*iter == this)
{
found = true;
break;
}
}
if (!found)
{
wpi_setWPIErrorWithContext(CommandIllegalUse, "A default command must require the subsystem");
return;
}
m_defaultCommand = command;
}
if (m_table != NULL)
{
if (m_defaultCommand != 0)
{
m_table->PutBoolean("hasDefault", true);
m_table->PutSubTable("default", m_defaultCommand->GetTable());
}
else
{
m_table->PutBoolean("hasDefault", false);
}
}
}
/**
* Adds a new child {@link Command} to the group with the given timeout. The
* {@link Command} will be started after all the previously added
* {@link Command Commands}.
*
* <p>Once the {@link Command Command} is started, it will run until it
* finishes, is interrupted, or the time expires, whichever is sooner. Note
* that the given {@link Command Command} will have no knowledge that it is on
* a timer.</p>
*
* <p>Instead of waiting for the child to finish, a {@link CommandGroup} will
* have it run at the same time as the subsequent {@link Command Commands}.
* The child will run until either it finishes, the timeout expires, a new
* child with conflicting requirements is started, or the main sequence runs a
* {@link Command} with conflicting requirements. In the latter two cases, the
* child will be canceled even if it says it can't be interrupted.</p>
*
* <p>Note that any requirements the given {@link Command Command} has will be
* added to the group. For this reason, a {@link Command Command's}
* requirements can not be changed after being added to a group.</p>
*
* <p>It is recommended that this method be called in the constructor.</p>
*
* @param command The command to be added
* @param timeout The timeout (in seconds)
*/
void CommandGroup::AddParallel(Command* command, double timeout) {
if (command == nullptr) {
wpi_setWPIErrorWithContext(NullParameter, "command");
return;
}
if (!AssertUnlocked("Cannot add new command to command group")) return;
if (timeout < 0.0) {
wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
return;
}
command->SetParent(this);
m_commands.push_back(CommandGroupEntry(
command, CommandGroupEntry::kSequence_BranchChild, timeout));
// Iterate through command->GetRequirements() and call Requires() on each
// required subsystem
Command::SubsystemSet requirements = command->GetRequirements();
auto iter = requirements.begin();
for (; iter != requirements.end(); iter++) Requires(*iter);
}