mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
bb50f4b134
Made a toplevel directory for C++ and C++ tests Change-Id: I4bc2074a7036ec7fe79568b411637a5bee9eb5b3 Added the C++ testing framework and one test Change-Id: I1e80a1e16b251a49666820a9d4c8caa025da9785
384 lines
10 KiB
C++
384 lines
10 KiB
C++
/*----------------------------------------------------------------------------*/
|
|
/* Copyright (c) FIRST 2011. All Rights Reserved. */
|
|
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
|
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
|
|
/*----------------------------------------------------------------------------*/
|
|
|
|
#include "Commands/CommandGroup.h"
|
|
#include "WPIErrors.h"
|
|
|
|
/**
|
|
* Creates a new {@link CommandGroup CommandGroup}.
|
|
*/
|
|
CommandGroup::CommandGroup()
|
|
{
|
|
m_currentCommandIndex = -1;
|
|
}
|
|
|
|
/**
|
|
* Creates a new {@link CommandGroup CommandGroup} with the given name.
|
|
* @param name the name for this command group
|
|
*/
|
|
CommandGroup::CommandGroup(const char *name) :
|
|
Command(name)
|
|
{
|
|
m_currentCommandIndex = -1;
|
|
}
|
|
|
|
CommandGroup::~CommandGroup()
|
|
{
|
|
}
|
|
|
|
/**
|
|
* 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 == NULL)
|
|
{
|
|
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();
|
|
Command::SubsystemSet::iterator iter = requirements.begin();
|
|
for (; iter != requirements.end(); iter++)
|
|
Requires(*iter);
|
|
}
|
|
|
|
/**
|
|
* Adds a new {@link Command Command} to the group with a given timeout.
|
|
* The {@link Command Command} will be started after all the previously added commands.
|
|
*
|
|
* <p>Once the {@link Command Command} is started, it will be run until it finishes 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>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
|
|
* @param timeout The timeout (in seconds)
|
|
*/
|
|
void CommandGroup::AddSequential(Command *command, double timeout)
|
|
{
|
|
if (command == NULL)
|
|
{
|
|
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_InSequence, timeout));
|
|
// Iterate through command->GetRequirements() and call Requires() on each required subsystem
|
|
Command::SubsystemSet requirements = command->GetRequirements();
|
|
Command::SubsystemSet::iterator iter = requirements.begin();
|
|
for (; iter != requirements.end(); iter++)
|
|
Requires(*iter);
|
|
}
|
|
|
|
/**
|
|
* Adds a new child {@link Command} to the group. The {@link Command} will be started after
|
|
* all the previously added {@link Command Commands}.
|
|
*
|
|
* <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, 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
|
|
*/
|
|
void CommandGroup::AddParallel(Command *command)
|
|
{
|
|
if (command == NULL)
|
|
{
|
|
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_BranchChild));
|
|
// Iterate through command->GetRequirements() and call Requires() on each required subsystem
|
|
Command::SubsystemSet requirements = command->GetRequirements();
|
|
Command::SubsystemSet::iterator iter = requirements.begin();
|
|
for (; iter != requirements.end(); iter++)
|
|
Requires(*iter);
|
|
}
|
|
|
|
/**
|
|
* 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 == NULL)
|
|
{
|
|
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();
|
|
Command::SubsystemSet::iterator iter = requirements.begin();
|
|
for (; iter != requirements.end(); iter++)
|
|
Requires(*iter);
|
|
}
|
|
|
|
void CommandGroup::_Initialize()
|
|
{
|
|
m_currentCommandIndex = -1;
|
|
}
|
|
|
|
void CommandGroup::_Execute()
|
|
{
|
|
CommandGroupEntry entry;
|
|
Command *cmd = NULL;
|
|
bool firstRun = false;
|
|
|
|
if (m_currentCommandIndex == -1)
|
|
{
|
|
firstRun = true;
|
|
m_currentCommandIndex = 0;
|
|
}
|
|
|
|
while ((unsigned)m_currentCommandIndex < m_commands.size())
|
|
{
|
|
if (cmd != NULL)
|
|
{
|
|
if (entry.IsTimedOut())
|
|
cmd->_Cancel();
|
|
|
|
if (cmd->Run())
|
|
{
|
|
break;
|
|
}
|
|
else
|
|
{
|
|
cmd->Removed();
|
|
m_currentCommandIndex++;
|
|
firstRun = true;
|
|
cmd = NULL;
|
|
continue;
|
|
}
|
|
}
|
|
|
|
entry = m_commands[m_currentCommandIndex];
|
|
cmd = NULL;
|
|
|
|
switch (entry.m_state)
|
|
{
|
|
case CommandGroupEntry::kSequence_InSequence:
|
|
cmd = entry.m_command;
|
|
if (firstRun)
|
|
{
|
|
cmd->StartRunning();
|
|
CancelConflicts(cmd);
|
|
firstRun = false;
|
|
}
|
|
break;
|
|
|
|
case CommandGroupEntry::kSequence_BranchPeer:
|
|
m_currentCommandIndex++;
|
|
entry.m_command->Start();
|
|
break;
|
|
|
|
case CommandGroupEntry::kSequence_BranchChild:
|
|
m_currentCommandIndex++;
|
|
CancelConflicts(entry.m_command);
|
|
entry.m_command->StartRunning();
|
|
m_children.push_back(entry);
|
|
break;
|
|
}
|
|
}
|
|
|
|
// Run Children
|
|
CommandList::iterator iter = m_children.begin();
|
|
for (; iter != m_children.end();)
|
|
{
|
|
entry = *iter;
|
|
Command *child = entry.m_command;
|
|
if (entry.IsTimedOut())
|
|
child->_Cancel();
|
|
|
|
if (!child->Run())
|
|
{
|
|
child->Removed();
|
|
iter = m_children.erase(iter);
|
|
}
|
|
else
|
|
{
|
|
iter++;
|
|
}
|
|
}
|
|
}
|
|
|
|
void CommandGroup::_End()
|
|
{
|
|
// Theoretically, we don't have to check this, but we do if teams override the IsFinished method
|
|
if (m_currentCommandIndex != -1 && (unsigned)m_currentCommandIndex < m_commands.size())
|
|
{
|
|
Command *cmd = m_commands[m_currentCommandIndex].m_command;
|
|
cmd->_Cancel();
|
|
cmd->Removed();
|
|
}
|
|
|
|
CommandList::iterator iter = m_children.begin();
|
|
for (; iter != m_children.end(); iter++)
|
|
{
|
|
Command *cmd = iter->m_command;
|
|
cmd->_Cancel();
|
|
cmd->Removed();
|
|
}
|
|
m_children.clear();
|
|
}
|
|
|
|
void CommandGroup::_Interrupted()
|
|
{
|
|
_End();
|
|
}
|
|
|
|
// Can be overwritten by teams
|
|
void CommandGroup::Initialize()
|
|
{
|
|
}
|
|
|
|
// Can be overwritten by teams
|
|
void CommandGroup::Execute()
|
|
{
|
|
}
|
|
|
|
// Can be overwritten by teams
|
|
void CommandGroup::End()
|
|
{
|
|
}
|
|
|
|
// Can be overwritten by teams
|
|
void CommandGroup::Interrupted()
|
|
{
|
|
}
|
|
|
|
bool CommandGroup::IsFinished()
|
|
{
|
|
return (unsigned)m_currentCommandIndex >= m_commands.size() && m_children.empty();
|
|
}
|
|
|
|
bool CommandGroup::IsInterruptible()
|
|
{
|
|
if (!Command::IsInterruptible())
|
|
return false;
|
|
|
|
if (m_currentCommandIndex != -1 && (unsigned)m_currentCommandIndex < m_commands.size())
|
|
{
|
|
Command *cmd = m_commands[m_currentCommandIndex].m_command;
|
|
if (!cmd->IsInterruptible())
|
|
return false;
|
|
}
|
|
|
|
CommandList::iterator iter = m_children.begin();
|
|
for (; iter != m_children.end(); iter++)
|
|
{
|
|
if (!iter->m_command->IsInterruptible())
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
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++;
|
|
}
|
|
}
|
|
|
|
int CommandGroup::GetSize()
|
|
{
|
|
return m_children.size();
|
|
}
|