Squashed 'third_party/allwpilib_2017/' content from commit 35ac87d
Change-Id: I7bb6f5556c30d3f5a092e68de0be9c710c60c9f4
git-subtree-dir: third_party/allwpilib_2017
git-subtree-split: 35ac87d6ff8b7f061c4f18c9ea316e5dccd4888a
diff --git a/wpilibc/shared/src/Commands/Command.cpp b/wpilibc/shared/src/Commands/Command.cpp
new file mode 100644
index 0000000..3d7d2ac
--- /dev/null
+++ b/wpilibc/shared/src/Commands/Command.cpp
@@ -0,0 +1,459 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011-2017. 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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Commands/Command.h"
+
+#include <typeinfo>
+
+#include "Commands/CommandGroup.h"
+#include "Commands/Scheduler.h"
+#include "RobotState.h"
+#include "Timer.h"
+#include "WPIErrors.h"
+
+using namespace frc;
+
+static const std::string kName = "name";
+static const std::string kRunning = "running";
+static const std::string kIsParented = "isParented";
+
+int Command::m_commandCounter = 0;
+
+/**
+ * Creates a new command.
+ * The name of this command will be default.
+ */
+Command::Command() : Command("", -1.0) {}
+
+/**
+ * Creates a new command with the given name and no timeout.
+ *
+ * @param name the name for this command
+ */
+Command::Command(const std::string& name) : Command(name, -1.0) {}
+
+/**
+ * Creates a new command with the given timeout and a default name.
+ *
+ * @param timeout the time (in seconds) before this command "times out"
+ * @see Command#isTimedOut() isTimedOut()
+ */
+Command::Command(double timeout) : Command("", timeout) {}
+
+/**
+ * Creates a new command with the given name and timeout.
+ *
+ * @param name the name of the command
+ * @param timeout the time (in seconds) before this command "times out"
+ * @see Command#isTimedOut() isTimedOut()
+ */
+Command::Command(const std::string& name, double timeout) {
+ // We use -1.0 to indicate no timeout.
+ if (timeout < 0.0 && timeout != -1.0)
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
+
+ m_timeout = timeout;
+
+ // If name contains an empty string
+ if (name.length() == 0) {
+ m_name = std::string("Command_") + std::string(typeid(*this).name());
+ } else {
+ m_name = name;
+ }
+}
+
+Command::~Command() {
+ if (m_table != nullptr) m_table->RemoveTableListener(this);
+}
+
+/**
+ * Get the ID (sequence number) for this command.
+ *
+ * The ID is a unique sequence number that is incremented for each command.
+ *
+ * @return the ID of this command
+ */
+int Command::GetID() const { return m_commandID; }
+
+/**
+ * Sets the timeout of this command.
+ *
+ * @param timeout the timeout (in seconds)
+ * @see Command#isTimedOut() isTimedOut()
+ */
+void Command::SetTimeout(double timeout) {
+ if (timeout < 0.0)
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
+ else
+ m_timeout = timeout;
+}
+
+/**
+ * Returns the time since this command was initialized (in seconds).
+ *
+ * This function will work even if there is no specified timeout.
+ *
+ * @return the time since this command was initialized (in seconds).
+ */
+double Command::TimeSinceInitialized() const {
+ if (m_startTime < 0.0)
+ return 0.0;
+ else
+ return Timer::GetFPGATimestamp() - m_startTime;
+}
+
+/**
+ * This method specifies that the given {@link Subsystem} is used by this
+ * command.
+ *
+ * This method is crucial to the functioning of the Command System in general.
+ *
+ * <p>Note that the recommended way to call this method is in the
+ * constructor.</p>
+ *
+ * @param subsystem the {@link Subsystem} required
+ * @see Subsystem
+ */
+void Command::Requires(Subsystem* subsystem) {
+ if (!AssertUnlocked("Can not add new requirement to command")) return;
+
+ if (subsystem != nullptr)
+ m_requirements.insert(subsystem);
+ else
+ wpi_setWPIErrorWithContext(NullParameter, "subsystem");
+}
+
+/**
+ * Called when the command has been removed.
+ *
+ * This will call {@link Command#interrupted() interrupted()} or
+ * {@link Command#end() end()}.
+ */
+void Command::Removed() {
+ if (m_initialized) {
+ if (IsCanceled()) {
+ Interrupted();
+ _Interrupted();
+ } else {
+ End();
+ _End();
+ }
+ }
+ m_initialized = false;
+ m_canceled = false;
+ m_running = false;
+ if (m_table != nullptr) m_table->PutBoolean(kRunning, false);
+}
+
+/**
+ * Starts up the command. Gets the command ready to start.
+ *
+ * <p>Note that the command will eventually start, however it will not
+ * necessarily do so immediately, and may in fact be canceled before initialize
+ * is even called.</p>
+ */
+void Command::Start() {
+ LockChanges();
+ if (m_parent != nullptr)
+ wpi_setWPIErrorWithContext(
+ CommandIllegalUse,
+ "Can not start a command that is part of a command group");
+
+ Scheduler::GetInstance()->AddCommand(this);
+}
+
+/**
+ * The run method is used internally to actually run the commands.
+ *
+ * @return whether or not the command should stay within the {@link Scheduler}.
+ */
+bool Command::Run() {
+ if (!m_runWhenDisabled && m_parent == nullptr && RobotState::IsDisabled())
+ Cancel();
+
+ if (IsCanceled()) return false;
+
+ if (!m_initialized) {
+ m_initialized = true;
+ StartTiming();
+ _Initialize();
+ Initialize();
+ }
+ _Execute();
+ Execute();
+ return !IsFinished();
+}
+
+/**
+ * The initialize method is called the first time this Command is run after
+ * being started.
+ */
+void Command::Initialize() {}
+
+/**
+ * The execute method is called repeatedly until this Command either finishes
+ * or is canceled.
+ */
+void Command::Execute() {}
+
+/**
+ * Called when the command ended peacefully. This is where you may want
+ * to wrap up loose ends, like shutting off a motor that was being used
+ * in the command.
+ */
+void Command::End() {}
+
+/**
+ * Called when the command ends because somebody called
+ * {@link Command#cancel() cancel()} or another command shared the same
+ * requirements as this one, and booted it out.
+ *
+ * <p>This is where you may want to wrap up loose ends, like shutting off a
+ * motor that was being used in the command.</p>
+ *
+ * <p>Generally, it is useful to simply call the {@link Command#end() end()}
+ * method within this method, as done here.</p>
+ */
+void Command::Interrupted() { End(); }
+
+void Command::_Initialize() {}
+
+void Command::_Interrupted() {}
+
+void Command::_Execute() {}
+
+void Command::_End() {}
+
+/**
+ * Called to indicate that the timer should start.
+ *
+ * This is called right before {@link Command#initialize() initialize()} is,
+ * inside the {@link Command#run() run()} method.
+ */
+void Command::StartTiming() { m_startTime = Timer::GetFPGATimestamp(); }
+
+/**
+ * Returns whether or not the
+ * {@link Command#timeSinceInitialized() timeSinceInitialized()} method returns
+ * a number which is greater than or equal to the timeout for the command.
+ *
+ * If there is no timeout, this will always return false.
+ *
+ * @return whether the time has expired
+ */
+bool Command::IsTimedOut() const {
+ return m_timeout != -1 && TimeSinceInitialized() >= m_timeout;
+}
+
+/**
+ * Returns the requirements (as an std::set of {@link Subsystem Subsystems}
+ * pointers) of this command.
+ *
+ * @return the requirements (as an std::set of {@link Subsystem Subsystems}
+ * pointers) of this command
+ */
+Command::SubsystemSet Command::GetRequirements() const {
+ return m_requirements;
+}
+
+/**
+ * Prevents further changes from being made.
+ */
+void Command::LockChanges() { m_locked = true; }
+
+/**
+ * If changes are locked, then this will generate a CommandIllegalUse error.
+ *
+ * @param message the message to report on error (it is appended by a default
+ * message)
+ * @return true if assert passed, false if assert failed
+ */
+bool Command::AssertUnlocked(const std::string& message) {
+ if (m_locked) {
+ std::string buf =
+ message + " after being started or being added to a command group";
+ wpi_setWPIErrorWithContext(CommandIllegalUse, buf);
+ return false;
+ }
+ return true;
+}
+
+/**
+ * Sets the parent of this command. No actual change is made to the group.
+ *
+ * @param parent the parent
+ */
+void Command::SetParent(CommandGroup* parent) {
+ if (parent == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "parent");
+ } else if (m_parent != nullptr) {
+ wpi_setWPIErrorWithContext(CommandIllegalUse,
+ "Can not give command to a command group after "
+ "already being put in a command group");
+ } else {
+ LockChanges();
+ m_parent = parent;
+ if (m_table != nullptr) {
+ m_table->PutBoolean(kIsParented, true);
+ }
+ }
+}
+
+/**
+ * Clears list of subsystem requirements. This is only used by
+ * {@link ConditionalCommand} so cancelling the chosen command works properly in
+ * {@link CommandGroup}.
+ */
+void Command::ClearRequirements() { m_requirements.clear(); }
+
+/**
+ * This is used internally to mark that the command has been started.
+ *
+ * The lifecycle of a command is:
+ *
+ * startRunning() is called.
+ * run() is called (multiple times potentially)
+ * removed() is called
+ *
+ * It is very important that startRunning and removed be called in order or some
+ * assumptions of the code will be broken.
+ */
+void Command::StartRunning() {
+ m_running = true;
+ m_startTime = -1;
+ if (m_table != nullptr) m_table->PutBoolean(kRunning, true);
+}
+
+/**
+ * Returns whether or not the command is running.
+ *
+ * This may return true even if the command has just been canceled, as it may
+ * not have yet called {@link Command#interrupted()}.
+ *
+ * @return whether or not the command is running
+ */
+bool Command::IsRunning() const { return m_running; }
+
+/**
+ * This will cancel the current command.
+ *
+ * <p>This will cancel the current command eventually. It can be called
+ * multiple times. And it can be called when the command is not running. If
+ * the command is running though, then the command will be marked as canceled
+ * and eventually removed.</p>
+ *
+ * <p>A command can not be canceled if it is a part of a command group, you
+ * must cancel the command group instead.</p>
+ */
+void Command::Cancel() {
+ if (m_parent != nullptr)
+ wpi_setWPIErrorWithContext(
+ CommandIllegalUse,
+ "Can not cancel a command that is part of a command group");
+
+ _Cancel();
+}
+
+/**
+ * This works like cancel(), except that it doesn't throw an exception if it is
+ * a part of a command group.
+ *
+ * Should only be called by the parent command group.
+ */
+void Command::_Cancel() {
+ if (IsRunning()) m_canceled = true;
+}
+
+/**
+ * Returns whether or not this has been canceled.
+ *
+ * @return whether or not this has been canceled
+ */
+bool Command::IsCanceled() const { return m_canceled; }
+
+/**
+ * Returns whether or not this command can be interrupted.
+ *
+ * @return whether or not this command can be interrupted
+ */
+bool Command::IsInterruptible() const { return m_interruptible; }
+
+/**
+ * Sets whether or not this command can be interrupted.
+ *
+ * @param interruptible whether or not this command can be interrupted
+ */
+void Command::SetInterruptible(bool interruptible) {
+ m_interruptible = interruptible;
+}
+
+/**
+ * Checks if the command requires the given {@link Subsystem}.
+ *
+ * @param system the system
+ * @return whether or not the subsystem is required (false if given nullptr)
+ */
+bool Command::DoesRequire(Subsystem* system) const {
+ return m_requirements.count(system) > 0;
+}
+
+/**
+ * Returns the {@link CommandGroup} that this command is a part of.
+ *
+ * Will return null if this {@link Command} is not in a group.
+ *
+ * @return the {@link CommandGroup} that this command is a part of (or null if
+ * not in group)
+ */
+CommandGroup* Command::GetGroup() const { return m_parent; }
+
+/**
+ * Sets whether or not this {@link Command} should run when the robot is
+ * disabled.
+ *
+ * <p>By default a command will not run when the robot is disabled, and will in
+ * fact be canceled.</p>
+ *
+ * @param run whether or not this command should run when the robot is disabled
+ */
+void Command::SetRunWhenDisabled(bool run) { m_runWhenDisabled = run; }
+
+/**
+ * Returns whether or not this {@link Command} will run when the robot is
+ * disabled, or if it will cancel itself.
+ *
+ * @return whether or not this {@link Command} will run when the robot is
+ * disabled, or if it will cancel itself
+ */
+bool Command::WillRunWhenDisabled() const { return m_runWhenDisabled; }
+
+std::string Command::GetName() const { return m_name; }
+
+std::string Command::GetSmartDashboardType() const { return "Command"; }
+
+void Command::InitTable(std::shared_ptr<ITable> subtable) {
+ if (m_table != nullptr) m_table->RemoveTableListener(this);
+ m_table = subtable;
+ if (m_table != nullptr) {
+ m_table->PutString(kName, GetName());
+ m_table->PutBoolean(kRunning, IsRunning());
+ m_table->PutBoolean(kIsParented, m_parent != nullptr);
+ m_table->AddTableListener(kRunning, this, false);
+ }
+}
+
+std::shared_ptr<ITable> Command::GetTable() const { return m_table; }
+
+void Command::ValueChanged(ITable* source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value, bool isNew) {
+ if (!value->IsBoolean()) return;
+ if (value->GetBoolean()) {
+ if (!IsRunning()) Start();
+ } else {
+ if (IsRunning()) Cancel();
+ }
+}
diff --git a/wpilibc/shared/src/Commands/CommandGroup.cpp b/wpilibc/shared/src/Commands/CommandGroup.cpp
new file mode 100644
index 0000000..a19acbe
--- /dev/null
+++ b/wpilibc/shared/src/Commands/CommandGroup.cpp
@@ -0,0 +1,321 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011-2017. 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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Commands/CommandGroup.h"
+
+#include "WPIErrors.h"
+
+using namespace frc;
+
+/**
+ * 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 (static_cast<size_t>(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 &&
+ static_cast<size_t>(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 static_cast<size_t>(m_currentCommandIndex) >= m_commands.size() &&
+ m_children.empty();
+}
+
+bool CommandGroup::IsInterruptible() const {
+ if (!Command::IsInterruptible()) return false;
+
+ if (m_currentCommandIndex != -1 &&
+ static_cast<size_t>(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(); }
diff --git a/wpilibc/shared/src/Commands/CommandGroupEntry.cpp b/wpilibc/shared/src/Commands/CommandGroupEntry.cpp
new file mode 100644
index 0000000..22476bb
--- /dev/null
+++ b/wpilibc/shared/src/Commands/CommandGroupEntry.cpp
@@ -0,0 +1,23 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011-2017. 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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Commands/CommandGroupEntry.h"
+
+#include "Commands/Command.h"
+
+using namespace frc;
+
+CommandGroupEntry::CommandGroupEntry(Command* command, Sequence state,
+ double timeout)
+ : m_timeout(timeout), m_command(command), m_state(state) {}
+
+bool CommandGroupEntry::IsTimedOut() const {
+ if (m_timeout < 0.0) return false;
+ double time = m_command->TimeSinceInitialized();
+ if (time == 0.0) return false;
+ return time >= m_timeout;
+}
diff --git a/wpilibc/shared/src/Commands/ConditionalCommand.cpp b/wpilibc/shared/src/Commands/ConditionalCommand.cpp
new file mode 100644
index 0000000..6d36086
--- /dev/null
+++ b/wpilibc/shared/src/Commands/ConditionalCommand.cpp
@@ -0,0 +1,84 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2017. 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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Commands/ConditionalCommand.h"
+
+#include "Commands/Scheduler.h"
+
+using namespace frc;
+
+/**
+ * Creates a new ConditionalCommand with given onTrue and onFalse Commands.
+ *
+ * @param onTrue The Command to execute if {@link
+ * ConditionalCommand#Condition()} returns true
+ * @param onFalse The Command to execute if {@link
+ * ConditionalCommand#Condition()} returns false
+ */
+ConditionalCommand::ConditionalCommand(Command* onTrue, Command* onFalse) {
+ m_onTrue = onTrue;
+ m_onFalse = onFalse;
+
+ for (auto requirement : m_onTrue->GetRequirements()) Requires(requirement);
+ for (auto requirement : m_onFalse->GetRequirements()) Requires(requirement);
+}
+
+/**
+ * Creates a new ConditionalCommand with given onTrue and onFalse Commands.
+ *
+ * @param name the name for this command group
+ * @param onTrue The Command to execute if {@link
+ * ConditionalCommand#Condition()} returns true
+ * @param onFalse The Command to execute if {@link
+ * ConditionalCommand#Condition()} returns false
+ */
+ConditionalCommand::ConditionalCommand(const std::string& name, Command* onTrue,
+ Command* onFalse)
+ : Command(name) {
+ m_onTrue = onTrue;
+ m_onFalse = onFalse;
+
+ for (auto requirement : m_onTrue->GetRequirements()) Requires(requirement);
+ for (auto requirement : m_onFalse->GetRequirements()) Requires(requirement);
+}
+
+void ConditionalCommand::_Initialize() {
+ if (Condition()) {
+ m_chosenCommand = m_onTrue;
+ } else {
+ m_chosenCommand = m_onFalse;
+ }
+
+ /*
+ * This is a hack to make cancelling the chosen command inside a CommandGroup
+ * work properly
+ */
+ m_chosenCommand->ClearRequirements();
+
+ m_chosenCommand->Start();
+}
+
+void ConditionalCommand::_Cancel() {
+ if (m_chosenCommand != nullptr && m_chosenCommand->IsRunning()) {
+ m_chosenCommand->Cancel();
+ }
+
+ Command::_Cancel();
+}
+
+bool ConditionalCommand::IsFinished() {
+ return m_chosenCommand != nullptr && m_chosenCommand->IsRunning() &&
+ m_chosenCommand->IsFinished();
+}
+
+void ConditionalCommand::Interrupted() {
+ if (m_chosenCommand != nullptr && m_chosenCommand->IsRunning()) {
+ m_chosenCommand->Cancel();
+ }
+
+ Command::Interrupted();
+}
diff --git a/wpilibc/shared/src/Commands/InstantCommand.cpp b/wpilibc/shared/src/Commands/InstantCommand.cpp
new file mode 100644
index 0000000..ddae80c
--- /dev/null
+++ b/wpilibc/shared/src/Commands/InstantCommand.cpp
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. 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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Commands/InstantCommand.h"
+
+using namespace frc;
+
+/**
+ * Creates a new {@link InstantCommand} with the given name.
+ * @param name the name for this command
+ */
+InstantCommand::InstantCommand(const std::string& name) : Command(name) {}
+
+bool InstantCommand::IsFinished() { return true; }
diff --git a/wpilibc/shared/src/Commands/PIDCommand.cpp b/wpilibc/shared/src/Commands/PIDCommand.cpp
new file mode 100644
index 0000000..44d96a6
--- /dev/null
+++ b/wpilibc/shared/src/Commands/PIDCommand.cpp
@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011-2017. 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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Commands/PIDCommand.h"
+
+#include <cfloat>
+
+using namespace frc;
+
+PIDCommand::PIDCommand(const std::string& name, double p, double i, double d,
+ double f, double period)
+ : Command(name) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
+}
+
+PIDCommand::PIDCommand(double p, double i, double d, double f, double period) {
+ m_controller =
+ std::make_shared<PIDController>(p, i, d, f, this, this, period);
+}
+
+PIDCommand::PIDCommand(const std::string& name, double p, double i, double d)
+ : Command(name) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+}
+
+PIDCommand::PIDCommand(const std::string& name, double p, double i, double d,
+ double period)
+ : Command(name) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
+}
+
+PIDCommand::PIDCommand(double p, double i, double d) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+}
+
+PIDCommand::PIDCommand(double p, double i, double d, double period) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
+}
+
+void PIDCommand::_Initialize() { m_controller->Enable(); }
+
+void PIDCommand::_End() { m_controller->Disable(); }
+
+void PIDCommand::_Interrupted() { _End(); }
+
+void PIDCommand::SetSetpointRelative(double deltaSetpoint) {
+ SetSetpoint(GetSetpoint() + deltaSetpoint);
+}
+
+void PIDCommand::PIDWrite(double output) { UsePIDOutput(output); }
+
+double PIDCommand::PIDGet() { return ReturnPIDInput(); }
+
+std::shared_ptr<PIDController> PIDCommand::GetPIDController() const {
+ return m_controller;
+}
+
+void PIDCommand::SetSetpoint(double setpoint) {
+ m_controller->SetSetpoint(setpoint);
+}
+
+double PIDCommand::GetSetpoint() const { return m_controller->GetSetpoint(); }
+
+double PIDCommand::GetPosition() { return ReturnPIDInput(); }
+
+std::string PIDCommand::GetSmartDashboardType() const { return "PIDCommand"; }
+
+void PIDCommand::InitTable(std::shared_ptr<ITable> subtable) {
+ m_controller->InitTable(subtable);
+ Command::InitTable(subtable);
+}
diff --git a/wpilibc/shared/src/Commands/PIDSubsystem.cpp b/wpilibc/shared/src/Commands/PIDSubsystem.cpp
new file mode 100644
index 0000000..89146e6
--- /dev/null
+++ b/wpilibc/shared/src/Commands/PIDSubsystem.cpp
@@ -0,0 +1,248 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011-2017. 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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Commands/PIDSubsystem.h"
+
+#include "PIDController.h"
+
+using namespace frc;
+
+/**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d
+ * values.
+ *
+ * @param name the name
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ */
+PIDSubsystem::PIDSubsystem(const std::string& name, double p, double i,
+ double d)
+ : Subsystem(name) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+}
+
+/**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d
+ * values.
+ *
+ * @param name the name
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ * @param f the feedforward value
+ */
+PIDSubsystem::PIDSubsystem(const std::string& name, double p, double i,
+ double d, double f)
+ : Subsystem(name) {
+ m_controller = std::make_shared<PIDController>(p, i, d, f, this, this);
+}
+
+/**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d
+ * values.
+ *
+ * It will also space the time between PID loop calculations to be equal to the
+ * given period.
+ *
+ * @param name the name
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ * @param f the feedfoward value
+ * @param period the time (in seconds) between calculations
+ */
+PIDSubsystem::PIDSubsystem(const std::string& name, double p, double i,
+ double d, double f, double period)
+ : Subsystem(name) {
+ m_controller =
+ std::make_shared<PIDController>(p, i, d, f, this, this, period);
+}
+
+/**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d
+ * values.
+ *
+ * It will use the class name as its name.
+ *
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ */
+PIDSubsystem::PIDSubsystem(double p, double i, double d)
+ : Subsystem("PIDSubsystem") {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+}
+
+/**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d
+ * values.
+ *
+ * It will use the class name as its name.
+ *
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ * @param f the feedforward value
+ */
+PIDSubsystem::PIDSubsystem(double p, double i, double d, double f)
+ : Subsystem("PIDSubsystem") {
+ m_controller = std::make_shared<PIDController>(p, i, d, f, this, this);
+}
+
+/**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d
+ * values.
+ *
+ * It will use the class name as its name. It will also space the time
+ * between PID loop calculations to be equal to the given period.
+ *
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ * @param f the feedforward value
+ * @param period the time (in seconds) between calculations
+ */
+PIDSubsystem::PIDSubsystem(double p, double i, double d, double f,
+ double period)
+ : Subsystem("PIDSubsystem") {
+ m_controller =
+ std::make_shared<PIDController>(p, i, d, f, this, this, period);
+}
+
+/**
+ * Enables the internal {@link PIDController}.
+ */
+void PIDSubsystem::Enable() { m_controller->Enable(); }
+
+/**
+ * Disables the internal {@link PIDController}.
+ */
+void PIDSubsystem::Disable() { m_controller->Disable(); }
+
+/**
+ * Returns the {@link PIDController} used by this {@link PIDSubsystem}.
+ *
+ * Use this if you would like to fine tune the pid loop.
+ *
+ * @return the {@link PIDController} used by this {@link PIDSubsystem}
+ */
+std::shared_ptr<PIDController> PIDSubsystem::GetPIDController() {
+ return m_controller;
+}
+
+/**
+ * Sets the setpoint to the given value.
+ *
+ * If {@link PIDCommand#SetRange(double, double) SetRange(...)} was called,
+ * then the given setpoint will be trimmed to fit within the range.
+ *
+ * @param setpoint the new setpoint
+ */
+void PIDSubsystem::SetSetpoint(double setpoint) {
+ m_controller->SetSetpoint(setpoint);
+}
+
+/**
+ * Adds the given value to the setpoint.
+ *
+ * If {@link PIDCommand#SetRange(double, double) SetRange(...)} was used,
+ * then the bounds will still be honored by this method.
+ *
+ * @param deltaSetpoint the change in the setpoint
+ */
+void PIDSubsystem::SetSetpointRelative(double deltaSetpoint) {
+ SetSetpoint(GetSetpoint() + deltaSetpoint);
+}
+
+/**
+ * Return the current setpoint.
+ *
+ * @return The current setpoint
+ */
+double PIDSubsystem::GetSetpoint() { return m_controller->GetSetpoint(); }
+
+/**
+ * Sets the maximum and minimum values expected from the input.
+ *
+ * @param minimumInput the minimum value expected from the input
+ * @param maximumInput the maximum value expected from the output
+ */
+void PIDSubsystem::SetInputRange(double minimumInput, double maximumInput) {
+ m_controller->SetInputRange(minimumInput, maximumInput);
+}
+
+/**
+ * Sets the maximum and minimum values to write.
+ *
+ * @param minimumOutput the minimum value to write to the output
+ * @param maximumOutput the maximum value to write to the output
+ */
+void PIDSubsystem::SetOutputRange(double minimumOutput, double maximumOutput) {
+ m_controller->SetOutputRange(minimumOutput, maximumOutput);
+}
+
+/**
+ * Set the absolute error which is considered tolerable for use with
+ * OnTarget.
+ *
+ * @param absValue absolute error which is tolerable
+ */
+void PIDSubsystem::SetAbsoluteTolerance(double absValue) {
+ m_controller->SetAbsoluteTolerance(absValue);
+}
+
+/**
+ * Set the percentage error which is considered tolerable for use with OnTarget.
+ *
+ * @param percent percentage error which is tolerable
+ */
+void PIDSubsystem::SetPercentTolerance(double percent) {
+ m_controller->SetPercentTolerance(percent);
+}
+
+/**
+ * Return true if the error is within the percentage of the total input range,
+ * determined by SetTolerance.
+ *
+ * This asssumes that the maximum and minimum input were set using SetInput.
+ * Use OnTarget() in the IsFinished() method of commands that use this
+ * subsystem.
+ *
+ * Currently this just reports on target as the actual value passes through the
+ * setpoint. Ideally it should be based on being within the tolerance for some
+ * period of time.
+ *
+ * @return true if the error is within the percentage tolerance of the input
+ * range
+ */
+bool PIDSubsystem::OnTarget() const { return m_controller->OnTarget(); }
+
+/**
+ * Returns the current position.
+ *
+ * @return the current position
+ */
+double PIDSubsystem::GetPosition() { return ReturnPIDInput(); }
+
+/**
+ * Returns the current rate.
+ *
+ * @return the current rate
+ */
+double PIDSubsystem::GetRate() { return ReturnPIDInput(); }
+
+void PIDSubsystem::PIDWrite(double output) { UsePIDOutput(output); }
+
+double PIDSubsystem::PIDGet() { return ReturnPIDInput(); }
+
+std::string PIDSubsystem::GetSmartDashboardType() const { return "PIDCommand"; }
+
+void PIDSubsystem::InitTable(std::shared_ptr<ITable> subtable) {
+ m_controller->InitTable(subtable);
+ Subsystem::InitTable(subtable);
+}
diff --git a/wpilibc/shared/src/Commands/PrintCommand.cpp b/wpilibc/shared/src/Commands/PrintCommand.cpp
new file mode 100644
index 0000000..afdcad5
--- /dev/null
+++ b/wpilibc/shared/src/Commands/PrintCommand.cpp
@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011-2017. 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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Commands/PrintCommand.h"
+
+#include <iostream>
+
+using namespace frc;
+
+PrintCommand::PrintCommand(const std::string& message)
+ : InstantCommand("Print \"" + message + "\"") {
+ m_message = message;
+}
+
+void PrintCommand::Initialize() { std::cout << m_message << "\n"; }
diff --git a/wpilibc/shared/src/Commands/Scheduler.cpp b/wpilibc/shared/src/Commands/Scheduler.cpp
new file mode 100644
index 0000000..aa325b4
--- /dev/null
+++ b/wpilibc/shared/src/Commands/Scheduler.cpp
@@ -0,0 +1,279 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011-2017. 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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Commands/Scheduler.h"
+
+#include <algorithm>
+#include <iostream>
+#include <set>
+
+#include "Buttons/ButtonScheduler.h"
+#include "Commands/Subsystem.h"
+#include "HLUsageReporting.h"
+#include "WPIErrors.h"
+
+using namespace frc;
+
+Scheduler::Scheduler() { HLUsageReporting::ReportScheduler(); }
+
+/**
+ * Returns the {@link Scheduler}, creating it if one does not exist.
+ *
+ * @return the {@link Scheduler}
+ */
+Scheduler* Scheduler::GetInstance() {
+ static Scheduler instance;
+ return &instance;
+}
+
+void Scheduler::SetEnabled(bool enabled) { m_enabled = enabled; }
+
+/**
+ * Add a command to be scheduled later.
+ *
+ * In any pass through the scheduler, all commands are added to the additions
+ * list, then at the end of the pass, they are all scheduled.
+ *
+ * @param command The command to be scheduled
+ */
+void Scheduler::AddCommand(Command* command) {
+ std::lock_guard<priority_mutex> sync(m_additionsLock);
+ if (std::find(m_additions.begin(), m_additions.end(), command) !=
+ m_additions.end())
+ return;
+ m_additions.push_back(command);
+}
+
+void Scheduler::AddButton(ButtonScheduler* button) {
+ std::lock_guard<priority_mutex> sync(m_buttonsLock);
+ m_buttons.push_back(button);
+}
+
+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;
+ }
+}
+
+/**
+ * Runs a single iteration of the loop.
+ *
+ * This method should be called often in order to have a functioning
+ * {@link Command} system. The loop has five stages:
+ *
+ * <ol>
+ * <li> Poll the Buttons </li>
+ * <li> Execute/Remove the Commands </li>
+ * <li> Send values to SmartDashboard </li>
+ * <li> Add Commands </li>
+ * <li> Add Defaults </li>
+ * </ol>
+ */
+void Scheduler::Run() {
+ // Get button input (going backwards preserves button priority)
+ {
+ if (!m_enabled) return;
+
+ std::lock_guard<priority_mutex> sync(m_buttonsLock);
+ auto rButtonIter = m_buttons.rbegin();
+ for (; rButtonIter != m_buttons.rend(); rButtonIter++) {
+ (*rButtonIter)->Execute();
+ }
+ }
+
+ m_runningCommandsChanged = false;
+
+ // Loop through the commands
+ auto commandIter = m_commands.begin();
+ for (; commandIter != m_commands.end();) {
+ Command* command = *commandIter;
+ // Increment before potentially removing to keep the iterator valid
+ commandIter++;
+ if (!command->Run()) {
+ Remove(command);
+ m_runningCommandsChanged = true;
+ }
+ }
+
+ // Add the new things
+ {
+ std::lock_guard<priority_mutex> sync(m_additionsLock);
+ auto additionsIter = m_additions.begin();
+ for (; additionsIter != m_additions.end(); additionsIter++) {
+ ProcessCommandAddition(*additionsIter);
+ }
+ m_additions.clear();
+ }
+
+ // Add in the defaults
+ auto subsystemIter = m_subsystems.begin();
+ for (; subsystemIter != m_subsystems.end(); subsystemIter++) {
+ Subsystem* lock = *subsystemIter;
+ if (lock->GetCurrentCommand() == nullptr) {
+ ProcessCommandAddition(lock->GetDefaultCommand());
+ }
+ lock->ConfirmCommand();
+ }
+
+ UpdateTable();
+}
+
+/**
+ * Registers a {@link Subsystem} to this {@link Scheduler}, so that the {@link
+ * Scheduler} might know if a default {@link Command} needs to be run.
+ *
+ * All {@link Subsystem Subsystems} should call this.
+ *
+ * @param system the system
+ */
+void Scheduler::RegisterSubsystem(Subsystem* subsystem) {
+ if (subsystem == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "subsystem");
+ return;
+ }
+ m_subsystems.insert(subsystem);
+}
+
+/**
+ * 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();
+}
+
+void Scheduler::RemoveAll() {
+ while (m_commands.size() > 0) {
+ Remove(*m_commands.begin());
+ }
+}
+
+/**
+ * Completely resets the scheduler. Undefined behavior if running.
+ */
+void Scheduler::ResetAll() {
+ RemoveAll();
+ m_subsystems.clear();
+ m_buttons.clear();
+ m_additions.clear();
+ m_commands.clear();
+ m_table = nullptr;
+}
+
+/**
+ * Update the network tables associated with the Scheduler object on the
+ * SmartDashboard.
+ */
+void Scheduler::UpdateTable() {
+ CommandSet::iterator commandIter;
+ if (m_table != nullptr) {
+ // Get the list of possible commands to cancel
+ auto new_toCancel = m_table->GetValue("Cancel");
+ if (new_toCancel)
+ toCancel = new_toCancel->GetDoubleArray();
+ else
+ toCancel.resize(0);
+ // m_table->RetrieveValue("Ids", *ids);
+
+ // cancel commands that have had the cancel buttons pressed
+ // on the SmartDashboad
+ if (!toCancel.empty()) {
+ for (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();
+ }
+ }
+ }
+ toCancel.resize(0);
+ m_table->PutValue("Cancel", nt::Value::MakeDoubleArray(toCancel));
+ }
+
+ // Set the running commands
+ if (m_runningCommandsChanged) {
+ commands.resize(0);
+ ids.resize(0);
+ for (commandIter = m_commands.begin(); commandIter != m_commands.end();
+ ++commandIter) {
+ Command* c = *commandIter;
+ commands.push_back(c->GetName());
+ ids.push_back(c->GetID());
+ }
+ m_table->PutValue("Names", nt::Value::MakeStringArray(commands));
+ m_table->PutValue("Ids", nt::Value::MakeDoubleArray(ids));
+ }
+ }
+}
+
+std::string Scheduler::GetName() const { return "Scheduler"; }
+
+std::string Scheduler::GetType() const { return "Scheduler"; }
+
+std::string Scheduler::GetSmartDashboardType() const { return "Scheduler"; }
+
+void Scheduler::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+
+ m_table->PutValue("Names", nt::Value::MakeStringArray(commands));
+ m_table->PutValue("Ids", nt::Value::MakeDoubleArray(ids));
+ m_table->PutValue("Cancel", nt::Value::MakeDoubleArray(toCancel));
+}
+
+std::shared_ptr<ITable> Scheduler::GetTable() const { return m_table; }
diff --git a/wpilibc/shared/src/Commands/StartCommand.cpp b/wpilibc/shared/src/Commands/StartCommand.cpp
new file mode 100644
index 0000000..8b66c40
--- /dev/null
+++ b/wpilibc/shared/src/Commands/StartCommand.cpp
@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011-2017. 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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Commands/StartCommand.h"
+
+using namespace frc;
+
+StartCommand::StartCommand(Command* commandToStart)
+ : InstantCommand("StartCommand") {
+ m_commandToFork = commandToStart;
+}
+
+void StartCommand::Initialize() { m_commandToFork->Start(); }
diff --git a/wpilibc/shared/src/Commands/Subsystem.cpp b/wpilibc/shared/src/Commands/Subsystem.cpp
new file mode 100644
index 0000000..404d74f
--- /dev/null
+++ b/wpilibc/shared/src/Commands/Subsystem.cpp
@@ -0,0 +1,152 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011-2017. 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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Commands/Subsystem.h"
+
+#include "Commands/Command.h"
+#include "Commands/Scheduler.h"
+#include "WPIErrors.h"
+
+using namespace frc;
+
+/**
+ * Creates a subsystem with the given name.
+ *
+ * @param name the name of the subsystem
+ */
+Subsystem::Subsystem(const std::string& name) {
+ m_name = name;
+ Scheduler::GetInstance()->RegisterSubsystem(this);
+}
+/**
+ * Initialize the default command for this subsystem.
+ *
+ * This is meant to be the place to call SetDefaultCommand in a subsystem and
+ * will be called on all the subsystems by the CommandBase method before the
+ * program starts running by using the list of all registered Subsystems inside
+ * the Scheduler.
+ *
+ * This should be overridden by a Subsystem that has a default Command
+ */
+void Subsystem::InitDefaultCommand() {}
+
+/**
+ * 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 == nullptr) {
+ m_defaultCommand = nullptr;
+ } else {
+ bool found = false;
+ Command::SubsystemSet requirements = command->GetRequirements();
+ auto 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 != nullptr) {
+ if (m_defaultCommand != nullptr) {
+ m_table->PutBoolean("hasDefault", true);
+ m_table->PutString("default", m_defaultCommand->GetName());
+ } else {
+ m_table->PutBoolean("hasDefault", false);
+ }
+ }
+}
+
+/**
+ * Returns the default command (or null if there is none).
+ *
+ * @return the default command
+ */
+Command* Subsystem::GetDefaultCommand() {
+ if (!m_initializedDefaultCommand) {
+ m_initializedDefaultCommand = true;
+ InitDefaultCommand();
+ }
+ return m_defaultCommand;
+}
+
+/**
+ * Sets the current command.
+ *
+ * @param command the new current command
+ */
+void Subsystem::SetCurrentCommand(Command* command) {
+ m_currentCommand = command;
+ m_currentCommandChanged = true;
+}
+
+/**
+ * Returns the command which currently claims this subsystem.
+ *
+ * @return the command which currently claims this subsystem
+ */
+Command* Subsystem::GetCurrentCommand() const { return m_currentCommand; }
+
+/**
+ * Call this to alert Subsystem that the current command is actually the
+ * command.
+ *
+ * Sometimes, the {@link Subsystem} is told that it has no command while the
+ * {@link Scheduler} is going through the loop, only to be soon after given a
+ * new one. This will avoid that situation.
+ */
+void Subsystem::ConfirmCommand() {
+ if (m_currentCommandChanged) {
+ if (m_table != nullptr) {
+ if (m_currentCommand != nullptr) {
+ m_table->PutBoolean("hasCommand", true);
+ m_table->PutString("command", m_currentCommand->GetName());
+ } else {
+ m_table->PutBoolean("hasCommand", false);
+ }
+ }
+ m_currentCommandChanged = false;
+ }
+}
+
+std::string Subsystem::GetName() const { return m_name; }
+
+std::string Subsystem::GetSmartDashboardType() const { return "Subsystem"; }
+
+void Subsystem::InitTable(std::shared_ptr<ITable> subtable) {
+ m_table = subtable;
+ if (m_table != nullptr) {
+ if (m_defaultCommand != nullptr) {
+ m_table->PutBoolean("hasDefault", true);
+ m_table->PutString("default", m_defaultCommand->GetName());
+ } else {
+ m_table->PutBoolean("hasDefault", false);
+ }
+ if (m_currentCommand != nullptr) {
+ m_table->PutBoolean("hasCommand", true);
+ m_table->PutString("command", m_currentCommand->GetName());
+ } else {
+ m_table->PutBoolean("hasCommand", false);
+ }
+ }
+}
+
+std::shared_ptr<ITable> Subsystem::GetTable() const { return m_table; }
diff --git a/wpilibc/shared/src/Commands/TimedCommand.cpp b/wpilibc/shared/src/Commands/TimedCommand.cpp
new file mode 100644
index 0000000..d910749
--- /dev/null
+++ b/wpilibc/shared/src/Commands/TimedCommand.cpp
@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. 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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Commands/TimedCommand.h"
+
+using namespace frc;
+
+TimedCommand::TimedCommand(const std::string& name, double timeout)
+ : Command(name, timeout) {}
+
+TimedCommand::TimedCommand(double timeout) : Command(timeout) {}
+
+bool TimedCommand::IsFinished() { return IsTimedOut(); }
diff --git a/wpilibc/shared/src/Commands/WaitCommand.cpp b/wpilibc/shared/src/Commands/WaitCommand.cpp
new file mode 100644
index 0000000..8bda85b
--- /dev/null
+++ b/wpilibc/shared/src/Commands/WaitCommand.cpp
@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011-2017. 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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Commands/WaitCommand.h"
+
+using namespace frc;
+
+WaitCommand::WaitCommand(double timeout)
+ : TimedCommand("Wait(" + std::to_string(timeout) + ")", timeout) {}
+
+WaitCommand::WaitCommand(const std::string& name, double timeout)
+ : TimedCommand(name, timeout) {}
diff --git a/wpilibc/shared/src/Commands/WaitForChildren.cpp b/wpilibc/shared/src/Commands/WaitForChildren.cpp
new file mode 100644
index 0000000..e796299
--- /dev/null
+++ b/wpilibc/shared/src/Commands/WaitForChildren.cpp
@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011-2017. 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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Commands/WaitForChildren.h"
+
+#include "Commands/CommandGroup.h"
+
+using namespace frc;
+
+WaitForChildren::WaitForChildren(double timeout)
+ : Command("WaitForChildren", timeout) {}
+
+WaitForChildren::WaitForChildren(const std::string& name, double timeout)
+ : Command(name, timeout) {}
+
+bool WaitForChildren::IsFinished() {
+ return GetGroup() == nullptr || GetGroup()->GetSize() == 0;
+}
diff --git a/wpilibc/shared/src/Commands/WaitUntilCommand.cpp b/wpilibc/shared/src/Commands/WaitUntilCommand.cpp
new file mode 100644
index 0000000..b9bffb4
--- /dev/null
+++ b/wpilibc/shared/src/Commands/WaitUntilCommand.cpp
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011-2017. 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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Commands/WaitUntilCommand.h"
+
+#include "Timer.h"
+
+using namespace frc;
+
+/**
+ * A {@link WaitCommand} will wait until a certain match time before finishing.
+ *
+ * This will wait until the game clock reaches some value, then continue to the
+ * next command.
+ * @see CommandGroup
+ */
+WaitUntilCommand::WaitUntilCommand(double time)
+ : Command("WaitUntilCommand", time) {
+ m_time = time;
+}
+
+WaitUntilCommand::WaitUntilCommand(const std::string& name, double time)
+ : Command(name, time) {
+ m_time = time;
+}
+
+/**
+ * Check if we've reached the actual finish time.
+ */
+bool WaitUntilCommand::IsFinished() { return Timer::GetMatchTime() >= m_time; }