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; }