Squashed 'third_party/allwpilib_2016/' content from commit 7f61816
Change-Id: If9d9245880859cdf580f5d7f77045135d0521ce7
git-subtree-dir: third_party/allwpilib_2016
git-subtree-split: 7f618166ed253a24629934fcf89c3decb0528a3b
diff --git a/wpilibc/shared/src/Commands/CommandGroup.cpp b/wpilibc/shared/src/Commands/CommandGroup.cpp
new file mode 100644
index 0000000..edb5ec9
--- /dev/null
+++ b/wpilibc/shared/src/Commands/CommandGroup.cpp
@@ -0,0 +1,338 @@
+/*----------------------------------------------------------------------------*/
+/* 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} with the given name.
+ * @param name the name for this command group
+ */
+CommandGroup::CommandGroup(const std::string &name) : Command(name) {}
+
+/**
+ * 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);
+}
+
+/**
+ * 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 == 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_InSequence, 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);
+}
+
+/**
+ * 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 == 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_BranchChild));
+ // 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);
+}
+
+/**
+ * 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);
+}
+
+void CommandGroup::_Initialize() { m_currentCommandIndex = -1; }
+
+void CommandGroup::_Execute() {
+ CommandGroupEntry entry;
+ Command *cmd = nullptr;
+ bool firstRun = false;
+
+ if (m_currentCommandIndex == -1) {
+ firstRun = true;
+ m_currentCommandIndex = 0;
+ }
+
+ while ((unsigned)m_currentCommandIndex < m_commands.size()) {
+ if (cmd != nullptr) {
+ if (entry.IsTimedOut()) cmd->_Cancel();
+
+ if (cmd->Run()) {
+ break;
+ } else {
+ cmd->Removed();
+ m_currentCommandIndex++;
+ firstRun = true;
+ cmd = nullptr;
+ continue;
+ }
+ }
+
+ entry = m_commands[m_currentCommandIndex];
+ cmd = nullptr;
+
+ 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
+ auto 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();
+ }
+
+ auto 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() const {
+ 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;
+ }
+
+ auto iter = m_children.cbegin();
+ for (; iter != m_children.cend(); iter++) {
+ if (!iter->m_command->IsInterruptible()) return false;
+ }
+
+ return true;
+}
+
+void CommandGroup::CancelConflicts(Command *command) {
+ auto childIter = m_children.begin();
+ for (; childIter != m_children.end();) {
+ Command *child = childIter->m_command;
+ bool erased = false;
+
+ Command::SubsystemSet requirements = command->GetRequirements();
+ auto 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() const { return m_children.size(); }