mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-07-01 02:41:48 +00:00
Clean up Command container iteration code (#73)
This commit is contained in:
Tyler Veness
committed by
Peter Johnson
Peter Johnson
parent
ea7d11b1db
commit
b7807bf9d2
@@ -114,7 +114,7 @@ bool Command::DoesRequire(Subsystem* system) const {
|
||||
return m_requirements.count(system) > 0;
|
||||
}
|
||||
|
||||
Command::SubsystemSet Command::GetRequirements() const {
|
||||
const Command::SubsystemSet& Command::GetRequirements() const {
|
||||
return m_requirements;
|
||||
}
|
||||
|
||||
|
||||
@@ -20,15 +20,13 @@ void CommandGroup::AddSequential(Command* command) {
|
||||
}
|
||||
if (!AssertUnlocked("Cannot add new command to command group")) return;
|
||||
|
||||
m_commands.emplace_back(command, CommandGroupEntry::kSequence_InSequence);
|
||||
|
||||
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();
|
||||
for (auto iter = requirements.begin(); iter != requirements.end(); iter++)
|
||||
Requires(*iter);
|
||||
for (auto& requirement : command->GetRequirements()) Requires(requirement);
|
||||
}
|
||||
|
||||
void CommandGroup::AddSequential(Command* command, double timeout) {
|
||||
@@ -42,15 +40,14 @@ void CommandGroup::AddSequential(Command* command, double timeout) {
|
||||
return;
|
||||
}
|
||||
|
||||
m_commands.emplace_back(command, CommandGroupEntry::kSequence_InSequence,
|
||||
timeout);
|
||||
|
||||
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();
|
||||
for (auto iter = requirements.begin(); iter != requirements.end(); iter++)
|
||||
Requires(*iter);
|
||||
for (auto& requirement : command->GetRequirements()) Requires(requirement);
|
||||
}
|
||||
|
||||
void CommandGroup::AddParallel(Command* command) {
|
||||
@@ -60,15 +57,13 @@ void CommandGroup::AddParallel(Command* command) {
|
||||
}
|
||||
if (!AssertUnlocked("Cannot add new command to command group")) return;
|
||||
|
||||
m_commands.emplace_back(command, CommandGroupEntry::kSequence_BranchChild);
|
||||
|
||||
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();
|
||||
for (auto iter = requirements.begin(); iter != requirements.end(); iter++)
|
||||
Requires(*iter);
|
||||
for (auto& requirement : command->GetRequirements()) Requires(requirement);
|
||||
}
|
||||
|
||||
void CommandGroup::AddParallel(Command* command, double timeout) {
|
||||
@@ -82,15 +77,14 @@ void CommandGroup::AddParallel(Command* command, double timeout) {
|
||||
return;
|
||||
}
|
||||
|
||||
m_commands.emplace_back(command, CommandGroupEntry::kSequence_BranchChild,
|
||||
timeout);
|
||||
|
||||
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();
|
||||
for (auto iter = requirements.begin(); iter != requirements.end(); iter++)
|
||||
Requires(*iter);
|
||||
for (auto& requirement : command->GetRequirements()) Requires(requirement);
|
||||
}
|
||||
|
||||
bool CommandGroup::IsInterruptible() const {
|
||||
@@ -102,8 +96,8 @@ bool CommandGroup::IsInterruptible() const {
|
||||
if (!cmd->IsInterruptible()) return false;
|
||||
}
|
||||
|
||||
for (auto iter = m_children.cbegin(); iter != m_children.cend(); iter++) {
|
||||
if (!iter->m_command->IsInterruptible()) return false;
|
||||
for (const auto& child : m_children) {
|
||||
if (!child->m_command->IsInterruptible()) return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
@@ -127,7 +121,7 @@ void CommandGroup::Interrupted() {}
|
||||
void CommandGroup::_Initialize() { m_currentCommandIndex = -1; }
|
||||
|
||||
void CommandGroup::_Execute() {
|
||||
CommandGroupEntry entry;
|
||||
CommandGroupEntry* entry;
|
||||
Command* cmd = nullptr;
|
||||
bool firstRun = false;
|
||||
|
||||
@@ -136,14 +130,20 @@ void CommandGroup::_Execute() {
|
||||
m_currentCommandIndex = 0;
|
||||
}
|
||||
|
||||
// While there are still commands in this group to run
|
||||
while (static_cast<size_t>(m_currentCommandIndex) < m_commands.size()) {
|
||||
// If a command is prepared to run
|
||||
if (cmd != nullptr) {
|
||||
if (entry.IsTimedOut()) cmd->_Cancel();
|
||||
// If command timed out, cancel it so it's removed from the Scheduler
|
||||
if (entry->IsTimedOut()) cmd->_Cancel();
|
||||
|
||||
// If command finished or was cancelled, remove it from Scheduler
|
||||
if (cmd->Run()) {
|
||||
break;
|
||||
} else {
|
||||
cmd->Removed();
|
||||
|
||||
// Advance to next command in group
|
||||
m_currentCommandIndex++;
|
||||
firstRun = true;
|
||||
cmd = nullptr;
|
||||
@@ -151,12 +151,12 @@ void CommandGroup::_Execute() {
|
||||
}
|
||||
}
|
||||
|
||||
entry = m_commands[m_currentCommandIndex];
|
||||
entry = &m_commands[m_currentCommandIndex];
|
||||
cmd = nullptr;
|
||||
|
||||
switch (entry.m_state) {
|
||||
switch (entry->m_state) {
|
||||
case CommandGroupEntry::kSequence_InSequence:
|
||||
cmd = entry.m_command;
|
||||
cmd = entry->m_command;
|
||||
if (firstRun) {
|
||||
cmd->StartRunning();
|
||||
CancelConflicts(cmd);
|
||||
@@ -165,32 +165,43 @@ void CommandGroup::_Execute() {
|
||||
break;
|
||||
|
||||
case CommandGroupEntry::kSequence_BranchPeer:
|
||||
// Start executing a parallel command and advance to next entry in group
|
||||
m_currentCommandIndex++;
|
||||
entry.m_command->Start();
|
||||
entry->m_command->Start();
|
||||
break;
|
||||
|
||||
case CommandGroupEntry::kSequence_BranchChild:
|
||||
m_currentCommandIndex++;
|
||||
CancelConflicts(entry.m_command);
|
||||
entry.m_command->StartRunning();
|
||||
|
||||
/* Causes scheduler to skip children of current command which require
|
||||
* the same subsystems as it
|
||||
*/
|
||||
CancelConflicts(entry->m_command);
|
||||
entry->m_command->StartRunning();
|
||||
|
||||
// Add current command entry to list of children of this group
|
||||
m_children.push_back(entry);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Run Children
|
||||
for (auto iter = m_children.begin(); iter != m_children.end();) {
|
||||
entry = *iter;
|
||||
Command* child = entry.m_command;
|
||||
if (entry.IsTimedOut()) child->_Cancel();
|
||||
for (auto& entry : m_children) {
|
||||
auto child = entry->m_command;
|
||||
if (entry->IsTimedOut()) {
|
||||
child->_Cancel();
|
||||
}
|
||||
|
||||
// If child finished or was cancelled, set it to nullptr. nullptr entries
|
||||
// are removed later.
|
||||
if (!child->Run()) {
|
||||
child->Removed();
|
||||
iter = m_children.erase(iter);
|
||||
} else {
|
||||
iter++;
|
||||
entry = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
m_children.erase(std::remove(m_children.begin(), m_children.end(), nullptr),
|
||||
m_children.end());
|
||||
}
|
||||
|
||||
void CommandGroup::_End() {
|
||||
@@ -203,8 +214,8 @@ void CommandGroup::_End() {
|
||||
cmd->Removed();
|
||||
}
|
||||
|
||||
for (auto iter = m_children.begin(); iter != m_children.end(); iter++) {
|
||||
Command* cmd = iter->m_command;
|
||||
for (auto& child : m_children) {
|
||||
Command* cmd = child->m_command;
|
||||
cmd->_Cancel();
|
||||
cmd->Removed();
|
||||
}
|
||||
@@ -215,13 +226,11 @@ void CommandGroup::_Interrupted() { _End(); }
|
||||
|
||||
void CommandGroup::CancelConflicts(Command* command) {
|
||||
for (auto childIter = m_children.begin(); childIter != m_children.end();) {
|
||||
Command* child = childIter->m_command;
|
||||
Command* child = (*childIter)->m_command;
|
||||
bool erased = false;
|
||||
|
||||
Command::SubsystemSet requirements = command->GetRequirements();
|
||||
for (auto requirementIter = requirements.begin();
|
||||
requirementIter != requirements.end(); requirementIter++) {
|
||||
if (child->DoesRequire(*requirementIter)) {
|
||||
for (auto& requirement : command->GetRequirements()) {
|
||||
if (child->DoesRequire(requirement)) {
|
||||
child->_Cancel();
|
||||
child->Removed();
|
||||
childIter = m_children.erase(childIter);
|
||||
|
||||
@@ -33,7 +33,7 @@ void Scheduler::AddCommand(Command* command) {
|
||||
|
||||
void Scheduler::AddButton(ButtonScheduler* button) {
|
||||
std::lock_guard<wpi::mutex> lock(m_buttonsMutex);
|
||||
m_buttons.push_back(button);
|
||||
m_buttons.emplace_back(button);
|
||||
}
|
||||
|
||||
void Scheduler::RegisterSubsystem(Subsystem* subsystem) {
|
||||
@@ -50,27 +50,23 @@ void Scheduler::Run() {
|
||||
if (!m_enabled) return;
|
||||
|
||||
std::lock_guard<wpi::mutex> lock(m_buttonsMutex);
|
||||
for (auto rButtonIter = m_buttons.rbegin(); rButtonIter != m_buttons.rend();
|
||||
rButtonIter++) {
|
||||
(*rButtonIter)->Execute();
|
||||
for (auto& button : m_buttons) {
|
||||
button->Execute();
|
||||
}
|
||||
}
|
||||
|
||||
// Call every subsystem's periodic method
|
||||
for (auto subsystemIter = m_subsystems.begin();
|
||||
subsystemIter != m_subsystems.end(); subsystemIter++) {
|
||||
Subsystem* subsystem = *subsystemIter;
|
||||
for (auto& subsystem : m_subsystems) {
|
||||
subsystem->Periodic();
|
||||
}
|
||||
|
||||
m_runningCommandsChanged = false;
|
||||
|
||||
// Loop through the commands
|
||||
for (auto commandIter = m_commands.begin();
|
||||
commandIter != m_commands.end();) {
|
||||
Command* command = *commandIter;
|
||||
for (auto cmdIter = m_commands.begin(); cmdIter != m_commands.end();) {
|
||||
Command* command = *cmdIter;
|
||||
// Increment before potentially removing to keep the iterator valid
|
||||
++commandIter;
|
||||
++cmdIter;
|
||||
if (!command->Run()) {
|
||||
Remove(command);
|
||||
m_runningCommandsChanged = true;
|
||||
@@ -80,21 +76,18 @@ void Scheduler::Run() {
|
||||
// Add the new things
|
||||
{
|
||||
std::lock_guard<wpi::mutex> lock(m_additionsMutex);
|
||||
for (auto additionsIter = m_additions.begin();
|
||||
additionsIter != m_additions.end(); additionsIter++) {
|
||||
ProcessCommandAddition(*additionsIter);
|
||||
for (auto& addition : m_additions) {
|
||||
ProcessCommandAddition(addition);
|
||||
}
|
||||
m_additions.clear();
|
||||
}
|
||||
|
||||
// Add in the defaults
|
||||
for (auto subsystemIter = m_subsystems.begin();
|
||||
subsystemIter != m_subsystems.end(); subsystemIter++) {
|
||||
Subsystem* lock = *subsystemIter;
|
||||
if (lock->GetCurrentCommand() == nullptr) {
|
||||
ProcessCommandAddition(lock->GetDefaultCommand());
|
||||
for (auto& subsystem : m_subsystems) {
|
||||
if (subsystem->GetCurrentCommand() == nullptr) {
|
||||
ProcessCommandAddition(subsystem->GetDefaultCommand());
|
||||
}
|
||||
lock->ConfirmCommand();
|
||||
subsystem->ConfirmCommand();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -106,10 +99,8 @@ void Scheduler::Remove(Command* command) {
|
||||
|
||||
if (!m_commands.erase(command)) return;
|
||||
|
||||
Command::SubsystemSet requirements = command->GetRequirements();
|
||||
for (auto iter = requirements.begin(); iter != requirements.end(); iter++) {
|
||||
Subsystem* lock = *iter;
|
||||
lock->SetCurrentCommand(nullptr);
|
||||
for (auto& requirement : command->GetRequirements()) {
|
||||
requirement->SetCurrentCommand(nullptr);
|
||||
}
|
||||
|
||||
command->Removed();
|
||||
@@ -149,12 +140,10 @@ void Scheduler::InitSendable(SendableBuilder& builder) {
|
||||
|
||||
// Cancel commands whose cancel buttons were pressed on the SmartDashboard
|
||||
if (!toCancel.empty()) {
|
||||
for (auto commandIter = m_commands.begin();
|
||||
commandIter != m_commands.end(); ++commandIter) {
|
||||
for (size_t i = 0; i < toCancel.size(); i++) {
|
||||
Command* c = *commandIter;
|
||||
if (c->GetID() == toCancel[i]) {
|
||||
c->Cancel();
|
||||
for (auto& command : m_commands) {
|
||||
for (const auto& cancelled : toCancel) {
|
||||
if (command->GetID() == cancelled) {
|
||||
command->Cancel();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -166,11 +155,9 @@ void Scheduler::InitSendable(SendableBuilder& builder) {
|
||||
if (m_runningCommandsChanged) {
|
||||
commands.resize(0);
|
||||
ids.resize(0);
|
||||
for (auto commandIter = m_commands.begin();
|
||||
commandIter != m_commands.end(); ++commandIter) {
|
||||
Command* c = *commandIter;
|
||||
commands.push_back(c->GetName());
|
||||
ids.push_back(c->GetID());
|
||||
for (const auto& command : m_commands) {
|
||||
commands.emplace_back(command->GetName());
|
||||
ids.emplace_back(command->GetID());
|
||||
}
|
||||
m_namesEntry.SetStringArray(commands);
|
||||
m_idsEntry.SetDoubleArray(ids);
|
||||
@@ -198,24 +185,20 @@ void Scheduler::ProcessCommandAddition(Command* command) {
|
||||
if (found == m_commands.end()) {
|
||||
// Check that the requirements can be had
|
||||
Command::SubsystemSet requirements = command->GetRequirements();
|
||||
for (Command::SubsystemSet::iterator iter = requirements.begin();
|
||||
iter != requirements.end(); iter++) {
|
||||
Subsystem* lock = *iter;
|
||||
if (lock->GetCurrentCommand() != nullptr &&
|
||||
!lock->GetCurrentCommand()->IsInterruptible())
|
||||
for (const auto& requirement : requirements) {
|
||||
if (requirement->GetCurrentCommand() != nullptr &&
|
||||
!requirement->GetCurrentCommand()->IsInterruptible())
|
||||
return;
|
||||
}
|
||||
|
||||
// Give it the requirements
|
||||
m_adding = true;
|
||||
for (Command::SubsystemSet::iterator iter = requirements.begin();
|
||||
iter != requirements.end(); iter++) {
|
||||
Subsystem* lock = *iter;
|
||||
if (lock->GetCurrentCommand() != nullptr) {
|
||||
lock->GetCurrentCommand()->Cancel();
|
||||
Remove(lock->GetCurrentCommand());
|
||||
for (auto& requirement : requirements) {
|
||||
if (requirement->GetCurrentCommand() != nullptr) {
|
||||
requirement->GetCurrentCommand()->Cancel();
|
||||
Remove(requirement->GetCurrentCommand());
|
||||
}
|
||||
lock->SetCurrentCommand(command);
|
||||
requirement->SetCurrentCommand(command);
|
||||
}
|
||||
m_adding = false;
|
||||
|
||||
|
||||
@@ -24,16 +24,8 @@ void Subsystem::SetDefaultCommand(Command* command) {
|
||||
if (command == nullptr) {
|
||||
m_defaultCommand = nullptr;
|
||||
} else {
|
||||
bool found = false;
|
||||
Command::SubsystemSet requirements = command->GetRequirements();
|
||||
for (auto iter = requirements.begin(); iter != requirements.end(); iter++) {
|
||||
if (*iter == this) {
|
||||
found = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!found) {
|
||||
const auto& reqs = command->GetRequirements();
|
||||
if (std::find(reqs.begin(), reqs.end(), this) == reqs.end()) {
|
||||
wpi_setWPIErrorWithContext(
|
||||
CommandIllegalUse, "A default command must require the subsystem");
|
||||
return;
|
||||
|
||||
@@ -195,7 +195,7 @@ class Command : public ErrorBase, public SendableBase {
|
||||
* @return The requirements (as an std::set of Subsystem pointers) of this
|
||||
* command
|
||||
*/
|
||||
SubsystemSet GetRequirements() const;
|
||||
const SubsystemSet& GetRequirements() const;
|
||||
|
||||
/**
|
||||
* Returns the CommandGroup that this command is a part of.
|
||||
|
||||
@@ -7,7 +7,6 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <list>
|
||||
#include <vector>
|
||||
|
||||
#include <wpi/Twine.h>
|
||||
@@ -169,7 +168,7 @@ class CommandGroup : public Command {
|
||||
std::vector<CommandGroupEntry> m_commands;
|
||||
|
||||
// The active children in this group (stored in entries)
|
||||
std::list<CommandGroupEntry> m_children;
|
||||
std::vector<CommandGroupEntry*> m_children;
|
||||
|
||||
// The current command, -1 signifies that none have been run
|
||||
int m_currentCommandIndex = -1;
|
||||
|
||||
@@ -97,7 +97,7 @@ class Scheduler : public ErrorBase, public SendableBase {
|
||||
|
||||
Command::SubsystemSet m_subsystems;
|
||||
wpi::mutex m_buttonsMutex;
|
||||
typedef std::vector<ButtonScheduler*> ButtonVector;
|
||||
typedef std::vector<std::unique_ptr<ButtonScheduler>> ButtonVector;
|
||||
ButtonVector m_buttons;
|
||||
typedef std::vector<Command*> CommandVector;
|
||||
wpi::mutex m_additionsMutex;
|
||||
|
||||
@@ -34,14 +34,14 @@ class CommandTest : public testing::Test {
|
||||
*/
|
||||
void TeardownScheduler() { Scheduler::GetInstance()->ResetAll(); }
|
||||
|
||||
void AssertCommandState(MockCommand& command, int32_t initialize,
|
||||
void AssertCommandState(MockCommand* command, int32_t initialize,
|
||||
int32_t execute, int32_t isFinished, int32_t end,
|
||||
int32_t interrupted) {
|
||||
EXPECT_EQ(initialize, command.GetInitializeCount());
|
||||
EXPECT_EQ(execute, command.GetExecuteCount());
|
||||
EXPECT_EQ(isFinished, command.GetIsFinishedCount());
|
||||
EXPECT_EQ(end, command.GetEndCount());
|
||||
EXPECT_EQ(interrupted, command.GetInterruptedCount());
|
||||
EXPECT_EQ(initialize, command->GetInitializeCount());
|
||||
EXPECT_EQ(execute, command->GetExecuteCount());
|
||||
EXPECT_EQ(isFinished, command->GetIsFinishedCount());
|
||||
EXPECT_EQ(end, command->GetEndCount());
|
||||
EXPECT_EQ(interrupted, command->GetInterruptedCount());
|
||||
}
|
||||
};
|
||||
|
||||
@@ -63,12 +63,12 @@ class ASubsystem : public Subsystem {
|
||||
|
||||
// CommandParallelGroupTest ported from CommandParallelGroupTest.java
|
||||
TEST_F(CommandTest, ParallelCommands) {
|
||||
MockCommand command1;
|
||||
MockCommand command2;
|
||||
auto command1 = new MockCommand;
|
||||
auto command2 = new MockCommand;
|
||||
CommandGroup commandGroup;
|
||||
|
||||
commandGroup.AddParallel(&command1);
|
||||
commandGroup.AddParallel(&command2);
|
||||
commandGroup.AddParallel(command1);
|
||||
commandGroup.AddParallel(command2);
|
||||
|
||||
AssertCommandState(command1, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(command2, 0, 0, 0, 0, 0);
|
||||
@@ -84,14 +84,14 @@ TEST_F(CommandTest, ParallelCommands) {
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command1, 1, 2, 2, 0, 0);
|
||||
AssertCommandState(command2, 1, 2, 2, 0, 0);
|
||||
command1.SetHasFinished(true);
|
||||
command1->SetHasFinished(true);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command1, 1, 3, 3, 1, 0);
|
||||
AssertCommandState(command2, 1, 3, 3, 0, 0);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command1, 1, 3, 3, 1, 0);
|
||||
AssertCommandState(command2, 1, 4, 4, 0, 0);
|
||||
command2.SetHasFinished(true);
|
||||
command2->SetHasFinished(true);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command1, 1, 3, 3, 1, 0);
|
||||
AssertCommandState(command2, 1, 5, 5, 1, 0);
|
||||
@@ -104,19 +104,19 @@ TEST_F(CommandTest, ParallelCommands) {
|
||||
TEST_F(CommandTest, RunAndTerminate) {
|
||||
MockCommand command;
|
||||
command.Start();
|
||||
AssertCommandState(command, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&command, 0, 0, 0, 0, 0);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&command, 0, 0, 0, 0, 0);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command, 1, 1, 1, 0, 0);
|
||||
AssertCommandState(&command, 1, 1, 1, 0, 0);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command, 1, 2, 2, 0, 0);
|
||||
AssertCommandState(&command, 1, 2, 2, 0, 0);
|
||||
command.SetHasFinished(true);
|
||||
AssertCommandState(command, 1, 2, 2, 0, 0);
|
||||
AssertCommandState(&command, 1, 2, 2, 0, 0);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command, 1, 3, 3, 1, 0);
|
||||
AssertCommandState(&command, 1, 3, 3, 1, 0);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command, 1, 3, 3, 1, 0);
|
||||
AssertCommandState(&command, 1, 3, 3, 1, 0);
|
||||
|
||||
TeardownScheduler();
|
||||
}
|
||||
@@ -124,21 +124,21 @@ TEST_F(CommandTest, RunAndTerminate) {
|
||||
TEST_F(CommandTest, RunAndCancel) {
|
||||
MockCommand command;
|
||||
command.Start();
|
||||
AssertCommandState(command, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&command, 0, 0, 0, 0, 0);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&command, 0, 0, 0, 0, 0);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command, 1, 1, 1, 0, 0);
|
||||
AssertCommandState(&command, 1, 1, 1, 0, 0);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command, 1, 2, 2, 0, 0);
|
||||
AssertCommandState(&command, 1, 2, 2, 0, 0);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command, 1, 3, 3, 0, 0);
|
||||
AssertCommandState(&command, 1, 3, 3, 0, 0);
|
||||
command.Cancel();
|
||||
AssertCommandState(command, 1, 3, 3, 0, 0);
|
||||
AssertCommandState(&command, 1, 3, 3, 0, 0);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command, 1, 3, 3, 0, 1);
|
||||
AssertCommandState(&command, 1, 3, 3, 0, 1);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command, 1, 3, 3, 0, 1);
|
||||
AssertCommandState(&command, 1, 3, 3, 0, 1);
|
||||
|
||||
TeardownScheduler();
|
||||
}
|
||||
@@ -147,17 +147,17 @@ TEST_F(CommandTest, RunAndCancel) {
|
||||
// CommandSequentialGroupTest ported from CommandSequentialGroupTest.java
|
||||
TEST_F(CommandTest, ThreeCommandOnSubSystem) {
|
||||
ASubsystem subsystem("Three Command Test Subsystem");
|
||||
MockCommand command1;
|
||||
command1.Requires(&subsystem);
|
||||
MockCommand command2;
|
||||
command2.Requires(&subsystem);
|
||||
MockCommand command3;
|
||||
command3.Requires(&subsystem);
|
||||
auto command1 = new MockCommand;
|
||||
command1->Requires(&subsystem);
|
||||
auto command2 = new MockCommand;
|
||||
command2->Requires(&subsystem);
|
||||
auto command3 = new MockCommand;
|
||||
command3->Requires(&subsystem);
|
||||
|
||||
CommandGroup commandGroup;
|
||||
commandGroup.AddSequential(&command1, 1.0);
|
||||
commandGroup.AddSequential(&command2, 2.0);
|
||||
commandGroup.AddSequential(&command3);
|
||||
commandGroup.AddSequential(command1, 1.0);
|
||||
commandGroup.AddSequential(command2, 2.0);
|
||||
commandGroup.AddSequential(command3);
|
||||
|
||||
AssertCommandState(command1, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(command2, 0, 0, 0, 0, 0);
|
||||
@@ -199,7 +199,7 @@ TEST_F(CommandTest, ThreeCommandOnSubSystem) {
|
||||
AssertCommandState(command1, 1, 1, 1, 0, 1);
|
||||
AssertCommandState(command2, 1, 2, 2, 0, 1);
|
||||
AssertCommandState(command3, 1, 2, 2, 0, 0);
|
||||
command3.SetHasFinished(true);
|
||||
command3->SetHasFinished(true);
|
||||
AssertCommandState(command1, 1, 1, 1, 0, 1);
|
||||
AssertCommandState(command2, 1, 2, 2, 0, 1);
|
||||
AssertCommandState(command3, 1, 2, 2, 0, 0);
|
||||
@@ -220,54 +220,54 @@ TEST_F(CommandTest, ThreeCommandOnSubSystem) {
|
||||
|
||||
// CommandSequentialGroupTest ported from CommandSequentialGroupTest.java
|
||||
TEST_F(CommandTest, OneCommandSupersedingAnotherBecauseOfDependencies) {
|
||||
auto subsystem = new ASubsystem("Command Superseding Test Subsystem");
|
||||
ASubsystem subsystem("Command Superseding Test Subsystem");
|
||||
MockCommand command1;
|
||||
command1.Requires(subsystem);
|
||||
command1.Requires(&subsystem);
|
||||
MockCommand command2;
|
||||
command2.Requires(subsystem);
|
||||
command2.Requires(&subsystem);
|
||||
|
||||
AssertCommandState(command1, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(command2, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&command1, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&command2, 0, 0, 0, 0, 0);
|
||||
|
||||
command1.Start();
|
||||
AssertCommandState(command1, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(command2, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&command1, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&command2, 0, 0, 0, 0, 0);
|
||||
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command1, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(command2, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&command1, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&command2, 0, 0, 0, 0, 0);
|
||||
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command1, 1, 1, 1, 0, 0);
|
||||
AssertCommandState(command2, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&command1, 1, 1, 1, 0, 0);
|
||||
AssertCommandState(&command2, 0, 0, 0, 0, 0);
|
||||
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command1, 1, 2, 2, 0, 0);
|
||||
AssertCommandState(command2, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&command1, 1, 2, 2, 0, 0);
|
||||
AssertCommandState(&command2, 0, 0, 0, 0, 0);
|
||||
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command1, 1, 3, 3, 0, 0);
|
||||
AssertCommandState(command2, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&command1, 1, 3, 3, 0, 0);
|
||||
AssertCommandState(&command2, 0, 0, 0, 0, 0);
|
||||
|
||||
command2.Start();
|
||||
AssertCommandState(command1, 1, 3, 3, 0, 0);
|
||||
AssertCommandState(command2, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&command1, 1, 3, 3, 0, 0);
|
||||
AssertCommandState(&command2, 0, 0, 0, 0, 0);
|
||||
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command1, 1, 4, 4, 0, 1);
|
||||
AssertCommandState(command2, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&command1, 1, 4, 4, 0, 1);
|
||||
AssertCommandState(&command2, 0, 0, 0, 0, 0);
|
||||
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command1, 1, 4, 4, 0, 1);
|
||||
AssertCommandState(command2, 1, 1, 1, 0, 0);
|
||||
AssertCommandState(&command1, 1, 4, 4, 0, 1);
|
||||
AssertCommandState(&command2, 1, 1, 1, 0, 0);
|
||||
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command1, 1, 4, 4, 0, 1);
|
||||
AssertCommandState(command2, 1, 2, 2, 0, 0);
|
||||
AssertCommandState(&command1, 1, 4, 4, 0, 1);
|
||||
AssertCommandState(&command2, 1, 2, 2, 0, 0);
|
||||
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command1, 1, 4, 4, 0, 1);
|
||||
AssertCommandState(command2, 1, 3, 3, 0, 0);
|
||||
AssertCommandState(&command1, 1, 4, 4, 0, 1);
|
||||
AssertCommandState(&command2, 1, 3, 3, 0, 0);
|
||||
|
||||
TeardownScheduler();
|
||||
}
|
||||
@@ -283,36 +283,36 @@ TEST_F(CommandTest,
|
||||
MockCommand command2;
|
||||
command2.Requires(&subsystem);
|
||||
|
||||
AssertCommandState(command1, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(command2, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&command1, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&command2, 0, 0, 0, 0, 0);
|
||||
|
||||
command1.Start();
|
||||
AssertCommandState(command1, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(command2, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&command1, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&command2, 0, 0, 0, 0, 0);
|
||||
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command1, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(command2, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&command1, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&command2, 0, 0, 0, 0, 0);
|
||||
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command1, 1, 1, 1, 0, 0);
|
||||
AssertCommandState(command2, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&command1, 1, 1, 1, 0, 0);
|
||||
AssertCommandState(&command2, 0, 0, 0, 0, 0);
|
||||
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command1, 1, 2, 2, 0, 0);
|
||||
AssertCommandState(command2, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&command1, 1, 2, 2, 0, 0);
|
||||
AssertCommandState(&command2, 0, 0, 0, 0, 0);
|
||||
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command1, 1, 3, 3, 0, 0);
|
||||
AssertCommandState(command2, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&command1, 1, 3, 3, 0, 0);
|
||||
AssertCommandState(&command2, 0, 0, 0, 0, 0);
|
||||
|
||||
command2.Start();
|
||||
AssertCommandState(command1, 1, 3, 3, 0, 0);
|
||||
AssertCommandState(command2, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&command1, 1, 3, 3, 0, 0);
|
||||
AssertCommandState(&command2, 0, 0, 0, 0, 0);
|
||||
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command1, 1, 4, 4, 0, 0);
|
||||
AssertCommandState(command2, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&command1, 1, 4, 4, 0, 0);
|
||||
AssertCommandState(&command2, 0, 0, 0, 0, 0);
|
||||
|
||||
TeardownScheduler();
|
||||
}
|
||||
@@ -333,33 +333,33 @@ TEST_F(CommandTest, TwoSecondTimeout) {
|
||||
command.Requires(&subsystem);
|
||||
|
||||
command.Start();
|
||||
AssertCommandState(command, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&command, 0, 0, 0, 0, 0);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&command, 0, 0, 0, 0, 0);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command, 1, 1, 1, 0, 0);
|
||||
AssertCommandState(&command, 1, 1, 1, 0, 0);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command, 1, 2, 2, 0, 0);
|
||||
AssertCommandState(&command, 1, 2, 2, 0, 0);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command, 1, 3, 3, 0, 0);
|
||||
AssertCommandState(&command, 1, 3, 3, 0, 0);
|
||||
Wait(2);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command, 1, 4, 4, 1, 0);
|
||||
AssertCommandState(&command, 1, 4, 4, 1, 0);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(command, 1, 4, 4, 1, 0);
|
||||
AssertCommandState(&command, 1, 4, 4, 1, 0);
|
||||
|
||||
TeardownScheduler();
|
||||
}
|
||||
|
||||
TEST_F(CommandTest, DefaultCommandWhereTheInteruptingCommandEndsItself) {
|
||||
ASubsystem subsystem("Default Command Test Subsystem");
|
||||
MockCommand defaultCommand;
|
||||
defaultCommand.Requires(&subsystem);
|
||||
auto defaultCommand = new MockCommand;
|
||||
defaultCommand->Requires(&subsystem);
|
||||
MockCommand anotherCommand;
|
||||
anotherCommand.Requires(&subsystem);
|
||||
|
||||
AssertCommandState(defaultCommand, 0, 0, 0, 0, 0);
|
||||
subsystem.Init(&defaultCommand);
|
||||
subsystem.Init(defaultCommand);
|
||||
|
||||
AssertCommandState(defaultCommand, 0, 0, 0, 0, 0);
|
||||
Scheduler::GetInstance()->Run();
|
||||
@@ -371,42 +371,41 @@ TEST_F(CommandTest, DefaultCommandWhereTheInteruptingCommandEndsItself) {
|
||||
|
||||
anotherCommand.Start();
|
||||
AssertCommandState(defaultCommand, 1, 2, 2, 0, 0);
|
||||
AssertCommandState(anotherCommand, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&anotherCommand, 0, 0, 0, 0, 0);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
|
||||
AssertCommandState(anotherCommand, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&anotherCommand, 0, 0, 0, 0, 0);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
|
||||
AssertCommandState(anotherCommand, 1, 1, 1, 0, 0);
|
||||
AssertCommandState(&anotherCommand, 1, 1, 1, 0, 0);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
|
||||
AssertCommandState(anotherCommand, 1, 2, 2, 0, 0);
|
||||
AssertCommandState(&anotherCommand, 1, 2, 2, 0, 0);
|
||||
anotherCommand.SetHasFinished(true);
|
||||
AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
|
||||
AssertCommandState(anotherCommand, 1, 2, 2, 0, 0);
|
||||
AssertCommandState(&anotherCommand, 1, 2, 2, 0, 0);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
|
||||
AssertCommandState(anotherCommand, 1, 3, 3, 1, 0);
|
||||
AssertCommandState(&anotherCommand, 1, 3, 3, 1, 0);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(defaultCommand, 2, 4, 4, 0, 1);
|
||||
AssertCommandState(anotherCommand, 1, 3, 3, 1, 0);
|
||||
AssertCommandState(&anotherCommand, 1, 3, 3, 1, 0);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(defaultCommand, 2, 5, 5, 0, 1);
|
||||
AssertCommandState(anotherCommand, 1, 3, 3, 1, 0);
|
||||
AssertCommandState(&anotherCommand, 1, 3, 3, 1, 0);
|
||||
|
||||
TeardownScheduler();
|
||||
}
|
||||
|
||||
TEST_F(CommandTest, DefaultCommandsInterruptingCommandCanceled) {
|
||||
ASubsystem subsystem("Default Command Test Subsystem");
|
||||
MockCommand defaultCommand;
|
||||
defaultCommand.Requires(&subsystem);
|
||||
auto defaultCommand = new MockCommand;
|
||||
defaultCommand->Requires(&subsystem);
|
||||
MockCommand anotherCommand;
|
||||
anotherCommand.Requires(&subsystem);
|
||||
|
||||
AssertCommandState(defaultCommand, 0, 0, 0, 0, 0);
|
||||
subsystem.Init(&defaultCommand);
|
||||
subsystem.InitDefaultCommand();
|
||||
subsystem.Init(defaultCommand);
|
||||
AssertCommandState(defaultCommand, 0, 0, 0, 0, 0);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(defaultCommand, 0, 0, 0, 0, 0);
|
||||
@@ -417,28 +416,28 @@ TEST_F(CommandTest, DefaultCommandsInterruptingCommandCanceled) {
|
||||
|
||||
anotherCommand.Start();
|
||||
AssertCommandState(defaultCommand, 1, 2, 2, 0, 0);
|
||||
AssertCommandState(anotherCommand, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&anotherCommand, 0, 0, 0, 0, 0);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
|
||||
AssertCommandState(anotherCommand, 0, 0, 0, 0, 0);
|
||||
AssertCommandState(&anotherCommand, 0, 0, 0, 0, 0);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
|
||||
AssertCommandState(anotherCommand, 1, 1, 1, 0, 0);
|
||||
AssertCommandState(&anotherCommand, 1, 1, 1, 0, 0);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
|
||||
AssertCommandState(anotherCommand, 1, 2, 2, 0, 0);
|
||||
AssertCommandState(&anotherCommand, 1, 2, 2, 0, 0);
|
||||
anotherCommand.Cancel();
|
||||
AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
|
||||
AssertCommandState(anotherCommand, 1, 2, 2, 0, 0);
|
||||
AssertCommandState(&anotherCommand, 1, 2, 2, 0, 0);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
|
||||
AssertCommandState(anotherCommand, 1, 2, 2, 0, 1);
|
||||
AssertCommandState(&anotherCommand, 1, 2, 2, 0, 1);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(defaultCommand, 2, 4, 4, 0, 1);
|
||||
AssertCommandState(anotherCommand, 1, 2, 2, 0, 1);
|
||||
AssertCommandState(&anotherCommand, 1, 2, 2, 0, 1);
|
||||
Scheduler::GetInstance()->Run();
|
||||
AssertCommandState(defaultCommand, 2, 5, 5, 0, 1);
|
||||
AssertCommandState(anotherCommand, 1, 2, 2, 0, 1);
|
||||
AssertCommandState(&anotherCommand, 1, 2, 2, 0, 1);
|
||||
|
||||
TeardownScheduler();
|
||||
}
|
||||
|
||||
@@ -7,7 +7,7 @@
|
||||
|
||||
package edu.wpi.first.wpilibj.command;
|
||||
|
||||
import java.util.Enumeration;
|
||||
import java.util.Collections;
|
||||
|
||||
import edu.wpi.first.wpilibj.Sendable;
|
||||
import edu.wpi.first.wpilibj.SendableBase;
|
||||
@@ -93,17 +93,7 @@ public abstract class Subsystem extends SendableBase {
|
||||
if (command == null) {
|
||||
m_defaultCommand = null;
|
||||
} else {
|
||||
boolean found = false;
|
||||
Enumeration requirements = command.getRequirements();
|
||||
while (requirements.hasMoreElements()) {
|
||||
if (requirements.nextElement().equals(this)) {
|
||||
found = true;
|
||||
// } else {
|
||||
// throw new
|
||||
// IllegalUseOfCommandException("A default command cannot require multiple subsystems");
|
||||
}
|
||||
}
|
||||
if (!found) {
|
||||
if (!Collections.list(command.getRequirements()).contains(this)) {
|
||||
throw new IllegalUseOfCommandException("A default command must require the subsystem");
|
||||
}
|
||||
m_defaultCommand = command;
|
||||
|
||||
Reference in New Issue
Block a user