diff --git a/wpilibc/shared/src/Commands/Command.cpp b/wpilibc/shared/src/Commands/Command.cpp
new file mode 100644
index 0000000..37effbe
--- /dev/null
+++ b/wpilibc/shared/src/Commands/Command.cpp
@@ -0,0 +1,392 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "Commands/Command.h"
+#include "Commands/CommandGroup.h"
+#include "Commands/Scheduler.h"
+#include "RobotState.h"
+#include "Timer.h"
+#include "WPIErrors.h"
+#include <typeinfo>
+
+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();
+}
+
+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);
+    }
+  }
+}
+
+/**
+ * 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> table) {
+  if (m_table != nullptr) m_table->RemoveTableListener(this);
+  m_table = table;
+  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..edb5ec9
--- /dev/null
+++ b/wpilibc/shared/src/Commands/CommandGroup.cpp
@@ -0,0 +1,338 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "Commands/CommandGroup.h"
+#include "WPIErrors.h"
+
+/**
+ * Creates a new {@link CommandGroup CommandGroup} with the given name.
+ * @param name the name for this command group
+ */
+CommandGroup::CommandGroup(const std::string &name) : Command(name) {}
+
+/**
+ * Adds a new {@link Command Command} to the group.  The {@link Command Command}
+ * will be started after
+ * all the previously added {@link Command Commands}.
+ *
+ * <p>Note that any requirements the given {@link Command Command} has will be
+ * added to the
+ * group.  For this reason, a {@link Command Command's} requirements can not be
+ * changed after
+ * being added to a group.</p>
+ *
+ * <p>It is recommended that this method be called in the constructor.</p>
+ *
+ * @param command The {@link Command Command} to be added
+ */
+void CommandGroup::AddSequential(Command *command) {
+  if (command == nullptr) {
+    wpi_setWPIErrorWithContext(NullParameter, "command");
+    return;
+  }
+  if (!AssertUnlocked("Cannot add new command to command group")) return;
+
+  command->SetParent(this);
+
+  m_commands.push_back(
+      CommandGroupEntry(command, CommandGroupEntry::kSequence_InSequence));
+  // Iterate through command->GetRequirements() and call Requires() on each
+  // required subsystem
+  Command::SubsystemSet requirements = command->GetRequirements();
+  auto iter = requirements.begin();
+  for (; iter != requirements.end(); iter++) Requires(*iter);
+}
+
+/**
+ * Adds a new {@link Command Command} to the group with a given timeout.
+ * The {@link Command Command} will be started after all the previously added
+ * commands.
+ *
+ * <p>Once the {@link Command Command} is started, it will be run until it
+ * finishes or the time
+ * expires, whichever is sooner.  Note that the given {@link Command Command}
+ * will have no
+ * knowledge that it is on a timer.</p>
+ *
+ * <p>Note that any requirements the given {@link Command Command} has will be
+ * added to the
+ * group.  For this reason, a {@link Command Command's} requirements can not be
+ * changed after
+ * being added to a group.</p>
+ *
+ * <p>It is recommended that this method be called in the constructor.</p>
+ *
+ * @param command The {@link Command Command} to be added
+ * @param timeout The timeout (in seconds)
+ */
+void CommandGroup::AddSequential(Command *command, double timeout) {
+  if (command == nullptr) {
+    wpi_setWPIErrorWithContext(NullParameter, "command");
+    return;
+  }
+  if (!AssertUnlocked("Cannot add new command to command group")) return;
+  if (timeout < 0.0) {
+    wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
+    return;
+  }
+
+  command->SetParent(this);
+
+  m_commands.push_back(CommandGroupEntry(
+      command, CommandGroupEntry::kSequence_InSequence, timeout));
+  // Iterate through command->GetRequirements() and call Requires() on each
+  // required subsystem
+  Command::SubsystemSet requirements = command->GetRequirements();
+  auto iter = requirements.begin();
+  for (; iter != requirements.end(); iter++) Requires(*iter);
+}
+
+/**
+ * Adds a new child {@link Command} to the group.  The {@link Command} will be
+ * started after
+ * all the previously added {@link Command Commands}.
+ *
+ * <p>Instead of waiting for the child to finish, a {@link CommandGroup} will
+ * have it
+ * run at the same time as the subsequent {@link Command Commands}.  The child
+ * will run until either
+ * it finishes, a new child with conflicting requirements is started, or
+ * the main sequence runs a {@link Command} with conflicting requirements.  In
+ * the latter
+ * two cases, the child will be canceled even if it says it can't be
+ * interrupted.</p>
+ *
+ * <p>Note that any requirements the given {@link Command Command} has will be
+ * added to the
+ * group.  For this reason, a {@link Command Command's} requirements can not be
+ * changed after
+ * being added to a group.</p>
+ *
+ * <p>It is recommended that this method be called in the constructor.</p>
+ *
+ * @param command The command to be added
+ */
+void CommandGroup::AddParallel(Command *command) {
+  if (command == nullptr) {
+    wpi_setWPIErrorWithContext(NullParameter, "command");
+    return;
+  }
+  if (!AssertUnlocked("Cannot add new command to command group")) return;
+
+  command->SetParent(this);
+
+  m_commands.push_back(
+      CommandGroupEntry(command, CommandGroupEntry::kSequence_BranchChild));
+  // Iterate through command->GetRequirements() and call Requires() on each
+  // required subsystem
+  Command::SubsystemSet requirements = command->GetRequirements();
+  auto iter = requirements.begin();
+  for (; iter != requirements.end(); iter++) Requires(*iter);
+}
+
+/**
+ * Adds a new child {@link Command} to the group with the given timeout.  The
+ * {@link Command} will be started after
+ * all the previously added {@link Command Commands}.
+ *
+ * <p>Once the {@link Command Command} is started, it will run until it
+ * finishes, is interrupted,
+ * or the time expires, whichever is sooner.  Note that the given {@link Command
+ * Command} will have no
+ * knowledge that it is on a timer.</p>
+ *
+ * <p>Instead of waiting for the child to finish, a {@link CommandGroup} will
+ * have it
+ * run at the same time as the subsequent {@link Command Commands}.  The child
+ * will run until either
+ * it finishes, the timeout expires, a new child with conflicting requirements
+ * is started, or
+ * the main sequence runs a {@link Command} with conflicting requirements.  In
+ * the latter
+ * two cases, the child will be canceled even if it says it can't be
+ * interrupted.</p>
+ *
+ * <p>Note that any requirements the given {@link Command Command} has will be
+ * added to the
+ * group.  For this reason, a {@link Command Command's} requirements can not be
+ * changed after
+ * being added to a group.</p>
+ *
+ * <p>It is recommended that this method be called in the constructor.</p>
+ *
+ * @param command The command to be added
+ * @param timeout The timeout (in seconds)
+ */
+void CommandGroup::AddParallel(Command *command, double timeout) {
+  if (command == nullptr) {
+    wpi_setWPIErrorWithContext(NullParameter, "command");
+    return;
+  }
+  if (!AssertUnlocked("Cannot add new command to command group")) return;
+  if (timeout < 0.0) {
+    wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
+    return;
+  }
+
+  command->SetParent(this);
+
+  m_commands.push_back(CommandGroupEntry(
+      command, CommandGroupEntry::kSequence_BranchChild, timeout));
+  // Iterate through command->GetRequirements() and call Requires() on each
+  // required subsystem
+  Command::SubsystemSet requirements = command->GetRequirements();
+  auto iter = requirements.begin();
+  for (; iter != requirements.end(); iter++) Requires(*iter);
+}
+
+void CommandGroup::_Initialize() { m_currentCommandIndex = -1; }
+
+void CommandGroup::_Execute() {
+  CommandGroupEntry entry;
+  Command *cmd = nullptr;
+  bool firstRun = false;
+
+  if (m_currentCommandIndex == -1) {
+    firstRun = true;
+    m_currentCommandIndex = 0;
+  }
+
+  while ((unsigned)m_currentCommandIndex < m_commands.size()) {
+    if (cmd != nullptr) {
+      if (entry.IsTimedOut()) cmd->_Cancel();
+
+      if (cmd->Run()) {
+        break;
+      } else {
+        cmd->Removed();
+        m_currentCommandIndex++;
+        firstRun = true;
+        cmd = nullptr;
+        continue;
+      }
+    }
+
+    entry = m_commands[m_currentCommandIndex];
+    cmd = nullptr;
+
+    switch (entry.m_state) {
+      case CommandGroupEntry::kSequence_InSequence:
+        cmd = entry.m_command;
+        if (firstRun) {
+          cmd->StartRunning();
+          CancelConflicts(cmd);
+          firstRun = false;
+        }
+        break;
+
+      case CommandGroupEntry::kSequence_BranchPeer:
+        m_currentCommandIndex++;
+        entry.m_command->Start();
+        break;
+
+      case CommandGroupEntry::kSequence_BranchChild:
+        m_currentCommandIndex++;
+        CancelConflicts(entry.m_command);
+        entry.m_command->StartRunning();
+        m_children.push_back(entry);
+        break;
+    }
+  }
+
+  // Run Children
+  auto iter = m_children.begin();
+  for (; iter != m_children.end();) {
+    entry = *iter;
+    Command *child = entry.m_command;
+    if (entry.IsTimedOut()) child->_Cancel();
+
+    if (!child->Run()) {
+      child->Removed();
+      iter = m_children.erase(iter);
+    } else {
+      iter++;
+    }
+  }
+}
+
+void CommandGroup::_End() {
+  // Theoretically, we don't have to check this, but we do if teams override the
+  // IsFinished method
+  if (m_currentCommandIndex != -1 &&
+      (unsigned)m_currentCommandIndex < m_commands.size()) {
+    Command *cmd = m_commands[m_currentCommandIndex].m_command;
+    cmd->_Cancel();
+    cmd->Removed();
+  }
+
+  auto iter = m_children.begin();
+  for (; iter != m_children.end(); iter++) {
+    Command *cmd = iter->m_command;
+    cmd->_Cancel();
+    cmd->Removed();
+  }
+  m_children.clear();
+}
+
+void CommandGroup::_Interrupted() { _End(); }
+
+// Can be overwritten by teams
+void CommandGroup::Initialize() {}
+
+// Can be overwritten by teams
+void CommandGroup::Execute() {}
+
+// Can be overwritten by teams
+void CommandGroup::End() {}
+
+// Can be overwritten by teams
+void CommandGroup::Interrupted() {}
+
+bool CommandGroup::IsFinished() {
+  return (unsigned)m_currentCommandIndex >= m_commands.size() &&
+         m_children.empty();
+}
+
+bool CommandGroup::IsInterruptible() const {
+  if (!Command::IsInterruptible()) return false;
+
+  if (m_currentCommandIndex != -1 &&
+      (unsigned)m_currentCommandIndex < m_commands.size()) {
+    Command *cmd = m_commands[m_currentCommandIndex].m_command;
+    if (!cmd->IsInterruptible()) return false;
+  }
+
+  auto iter = m_children.cbegin();
+  for (; iter != m_children.cend(); iter++) {
+    if (!iter->m_command->IsInterruptible()) return false;
+  }
+
+  return true;
+}
+
+void CommandGroup::CancelConflicts(Command *command) {
+  auto childIter = m_children.begin();
+  for (; childIter != m_children.end();) {
+    Command *child = childIter->m_command;
+    bool erased = false;
+
+    Command::SubsystemSet requirements = command->GetRequirements();
+    auto requirementIter = requirements.begin();
+    for (; requirementIter != requirements.end(); requirementIter++) {
+      if (child->DoesRequire(*requirementIter)) {
+        child->_Cancel();
+        child->Removed();
+        childIter = m_children.erase(childIter);
+        erased = true;
+        break;
+      }
+    }
+    if (!erased) childIter++;
+  }
+}
+
+int CommandGroup::GetSize() const { return m_children.size(); }
diff --git a/wpilibc/shared/src/Commands/CommandGroupEntry.cpp b/wpilibc/shared/src/Commands/CommandGroupEntry.cpp
new file mode 100644
index 0000000..c9f3fa4
--- /dev/null
+++ b/wpilibc/shared/src/Commands/CommandGroupEntry.cpp
@@ -0,0 +1,21 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "Commands/CommandGroupEntry.h"
+
+#include "Commands/Command.h"
+
+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/PIDCommand.cpp b/wpilibc/shared/src/Commands/PIDCommand.cpp
new file mode 100644
index 0000000..c6ac919
--- /dev/null
+++ b/wpilibc/shared/src/Commands/PIDCommand.cpp
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "Commands/PIDCommand.h"
+
+#include "float.h"
+
+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(float 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> table) {
+  m_controller->InitTable(table);
+  Command::InitTable(table);
+}
diff --git a/wpilibc/shared/src/Commands/PIDSubsystem.cpp b/wpilibc/shared/src/Commands/PIDSubsystem.cpp
new file mode 100644
index 0000000..b3d327f
--- /dev/null
+++ b/wpilibc/shared/src/Commands/PIDSubsystem.cpp
@@ -0,0 +1,225 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "Commands/PIDSubsystem.h"
+#include "PIDController.h"
+#include "float.h"
+
+/**
+ * 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(float minimumInput, float 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(float minimumOutput, float maximumOutput) {
+  m_controller->SetOutputRange(minimumOutput, maximumOutput);
+}
+
+/*
+ * Set the absolute error which is considered tolerable for use with
+ * OnTarget.
+ * @param percentage error which is tolerable
+ */
+void PIDSubsystem::SetAbsoluteTolerance(float absValue) {
+  m_controller->SetAbsoluteTolerance(absValue);
+}
+
+/*
+ * Set the percentage error which is considered tolerable for use with
+ * OnTarget.
+ * @param percentage error which is tolerable
+ */
+void PIDSubsystem::SetPercentTolerance(float 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(float output) { UsePIDOutput(output); }
+
+double PIDSubsystem::PIDGet() { return ReturnPIDInput(); }
+
+std::string PIDSubsystem::GetSmartDashboardType() const { return "PIDCommand"; }
+void PIDSubsystem::InitTable(std::shared_ptr<ITable> table) {
+  m_controller->InitTable(table);
+  Subsystem::InitTable(table);
+}
diff --git a/wpilibc/shared/src/Commands/PrintCommand.cpp b/wpilibc/shared/src/Commands/PrintCommand.cpp
new file mode 100644
index 0000000..b5e8dc7
--- /dev/null
+++ b/wpilibc/shared/src/Commands/PrintCommand.cpp
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "Commands/PrintCommand.h"
+#include "stdio.h"
+#include <sstream>
+
+PrintCommand::PrintCommand(const std::string &message)
+    : Command(((std::stringstream &)(std::stringstream("Print \"") << message
+                                                                   << "\""))
+                  .str()
+                  .c_str()) {
+  m_message = message;
+}
+
+void PrintCommand::Initialize() { printf("%s", m_message.c_str()); }
+
+void PrintCommand::Execute() {}
+
+bool PrintCommand::IsFinished() { return true; }
+
+void PrintCommand::End() {}
+
+void PrintCommand::Interrupted() {}
diff --git a/wpilibc/shared/src/Commands/Scheduler.cpp b/wpilibc/shared/src/Commands/Scheduler.cpp
new file mode 100644
index 0000000..0b796e3
--- /dev/null
+++ b/wpilibc/shared/src/Commands/Scheduler.cpp
@@ -0,0 +1,273 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "Commands/Scheduler.h"
+
+#include "Buttons/ButtonScheduler.h"
+#include "Commands/Subsystem.h"
+#include "HLUsageReporting.h"
+#include "WPIErrors.h"
+#include <iostream>
+#include <set>
+#include <algorithm>
+
+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 (unsigned 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..60db054
--- /dev/null
+++ b/wpilibc/shared/src/Commands/StartCommand.cpp
@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "Commands/StartCommand.h"
+
+StartCommand::StartCommand(Command *commandToStart) : Command("StartCommand") {
+  m_commandToFork = commandToStart;
+}
+
+void StartCommand::Initialize() { m_commandToFork->Start(); }
+
+void StartCommand::Execute() {}
+
+void StartCommand::End() {}
+
+void StartCommand::Interrupted() {}
+
+bool StartCommand::IsFinished() { return true; }
diff --git a/wpilibc/shared/src/Commands/Subsystem.cpp b/wpilibc/shared/src/Commands/Subsystem.cpp
new file mode 100644
index 0000000..0b4b92f
--- /dev/null
+++ b/wpilibc/shared/src/Commands/Subsystem.cpp
@@ -0,0 +1,147 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "Commands/Subsystem.h"
+
+#include "Commands/Command.h"
+#include "Commands/Scheduler.h"
+#include "WPIErrors.h"
+
+/**
+ * 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> table) {
+  m_table = table;
+  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/WaitCommand.cpp b/wpilibc/shared/src/Commands/WaitCommand.cpp
new file mode 100644
index 0000000..f4e4c00
--- /dev/null
+++ b/wpilibc/shared/src/Commands/WaitCommand.cpp
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "Commands/WaitCommand.h"
+#include <sstream>
+
+WaitCommand::WaitCommand(double timeout)
+    : Command(
+          ((std::stringstream &)(std::stringstream("Wait(") << timeout << ")"))
+              .str()
+              .c_str(),
+          timeout) {}
+
+WaitCommand::WaitCommand(const std::string &name, double timeout)
+    : Command(name, timeout) {}
+
+void WaitCommand::Initialize() {}
+
+void WaitCommand::Execute() {}
+
+bool WaitCommand::IsFinished() { return IsTimedOut(); }
+
+void WaitCommand::End() {}
+
+void WaitCommand::Interrupted() {}
diff --git a/wpilibc/shared/src/Commands/WaitForChildren.cpp b/wpilibc/shared/src/Commands/WaitForChildren.cpp
new file mode 100644
index 0000000..a3a129d
--- /dev/null
+++ b/wpilibc/shared/src/Commands/WaitForChildren.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "Commands/WaitForChildren.h"
+#include "Commands/CommandGroup.h"
+
+WaitForChildren::WaitForChildren(double timeout)
+    : Command("WaitForChildren", timeout) {}
+
+WaitForChildren::WaitForChildren(const std::string &name, double timeout)
+    : Command(name, timeout) {}
+
+void WaitForChildren::Initialize() {}
+
+void WaitForChildren::Execute() {}
+
+void WaitForChildren::End() {}
+
+void WaitForChildren::Interrupted() {}
+
+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..ea0e664
--- /dev/null
+++ b/wpilibc/shared/src/Commands/WaitUntilCommand.cpp
@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "Commands/WaitUntilCommand.h"
+#include "Timer.h"
+
+/**
+ * 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;
+}
+
+void WaitUntilCommand::Initialize() {}
+
+void WaitUntilCommand::Execute() {}
+
+/**
+ * Check if we've reached the actual finish time.
+ */
+bool WaitUntilCommand::IsFinished() { return Timer::GetMatchTime() >= m_time; }
+
+void WaitUntilCommand::End() {}
+
+void WaitUntilCommand::Interrupted() {}