diff --git a/wpilibc/src/main/native/cpp/Commands/Command.cpp b/wpilibc/src/main/native/cpp/Commands/Command.cpp index 449d8b6dcf..25c1ec0910 100644 --- a/wpilibc/src/main/native/cpp/Commands/Command.cpp +++ b/wpilibc/src/main/native/cpp/Commands/Command.cpp @@ -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; } diff --git a/wpilibc/src/main/native/cpp/Commands/CommandGroup.cpp b/wpilibc/src/main/native/cpp/Commands/CommandGroup.cpp index 4c78766ee8..7e6e7a1727 100644 --- a/wpilibc/src/main/native/cpp/Commands/CommandGroup.cpp +++ b/wpilibc/src/main/native/cpp/Commands/CommandGroup.cpp @@ -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(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); diff --git a/wpilibc/src/main/native/cpp/Commands/Scheduler.cpp b/wpilibc/src/main/native/cpp/Commands/Scheduler.cpp index 3ac8adc3f6..4febcd1e3d 100644 --- a/wpilibc/src/main/native/cpp/Commands/Scheduler.cpp +++ b/wpilibc/src/main/native/cpp/Commands/Scheduler.cpp @@ -33,7 +33,7 @@ void Scheduler::AddCommand(Command* command) { void Scheduler::AddButton(ButtonScheduler* button) { std::lock_guard 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 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 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; diff --git a/wpilibc/src/main/native/cpp/Commands/Subsystem.cpp b/wpilibc/src/main/native/cpp/Commands/Subsystem.cpp index 9bd21cffb1..7dd0a8c813 100644 --- a/wpilibc/src/main/native/cpp/Commands/Subsystem.cpp +++ b/wpilibc/src/main/native/cpp/Commands/Subsystem.cpp @@ -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; diff --git a/wpilibc/src/main/native/include/Commands/Command.h b/wpilibc/src/main/native/include/Commands/Command.h index 0d49eb48a1..3e619af191 100644 --- a/wpilibc/src/main/native/include/Commands/Command.h +++ b/wpilibc/src/main/native/include/Commands/Command.h @@ -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. diff --git a/wpilibc/src/main/native/include/Commands/CommandGroup.h b/wpilibc/src/main/native/include/Commands/CommandGroup.h index 6a0af692e5..8aa81b228f 100644 --- a/wpilibc/src/main/native/include/Commands/CommandGroup.h +++ b/wpilibc/src/main/native/include/Commands/CommandGroup.h @@ -7,7 +7,6 @@ #pragma once -#include #include #include @@ -169,7 +168,7 @@ class CommandGroup : public Command { std::vector m_commands; // The active children in this group (stored in entries) - std::list m_children; + std::vector m_children; // The current command, -1 signifies that none have been run int m_currentCommandIndex = -1; diff --git a/wpilibc/src/main/native/include/Commands/Scheduler.h b/wpilibc/src/main/native/include/Commands/Scheduler.h index 4f0a7afc13..bf96dad28c 100644 --- a/wpilibc/src/main/native/include/Commands/Scheduler.h +++ b/wpilibc/src/main/native/include/Commands/Scheduler.h @@ -97,7 +97,7 @@ class Scheduler : public ErrorBase, public SendableBase { Command::SubsystemSet m_subsystems; wpi::mutex m_buttonsMutex; - typedef std::vector ButtonVector; + typedef std::vector> ButtonVector; ButtonVector m_buttons; typedef std::vector CommandVector; wpi::mutex m_additionsMutex; diff --git a/wpilibcIntegrationTests/src/main/native/cpp/command/CommandTest.cpp b/wpilibcIntegrationTests/src/main/native/cpp/command/CommandTest.cpp index ac08c17f98..21f64e550c 100644 --- a/wpilibcIntegrationTests/src/main/native/cpp/command/CommandTest.cpp +++ b/wpilibcIntegrationTests/src/main/native/cpp/command/CommandTest.cpp @@ -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(); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java index 5dc30e5716..0da80c253c 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java @@ -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;