Move CameraServer and WPILib headers into their own folder

The old headers were moved into folders because doing so avoids polluting
the system include directories.

Folder names were also normalized to lowercase.

wpilibc/src/main/native/cpp/commands/Command.cpp

@@ -0,0 +1,226 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. 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 "frc/commands/Command.h"
+
+#include <typeinfo>
+
+#include "frc/RobotState.h"
+#include "frc/Timer.h"
+#include "frc/WPIErrors.h"
+#include "frc/commands/CommandGroup.h"
+#include "frc/commands/Scheduler.h"
+#include "frc/livewindow/LiveWindow.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+int Command::m_commandCounter = 0;
+
+Command::Command() : Command("", -1.0) {}
+
+Command::Command(const wpi::Twine& name) : Command(name, -1.0) {}
+
+Command::Command(double timeout) : Command("", timeout) {}
+
+Command::Command(const wpi::Twine& name, double timeout) : SendableBase(false) {
+  // 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.isTriviallyEmpty() ||
+      (name.isSingleStringRef() && name.getSingleStringRef().empty())) {
+    SetName("Command_" + wpi::Twine(typeid(*this).name()));
+  } else {
+    SetName(name);
+  }
+}
+
+double Command::TimeSinceInitialized() const {
+  if (m_startTime < 0.0)
+    return 0.0;
+  else
+    return Timer::GetFPGATimestamp() - m_startTime;
+}
+
+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");
+}
+
+void Command::Start() {
+  LockChanges();
+  if (m_parent != nullptr)
+    wpi_setWPIErrorWithContext(
+        CommandIllegalUse,
+        "Can not start a command that is part of a command group");
+
+  m_completed = false;
+  Scheduler::GetInstance()->AddCommand(this);
+}
+
+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::Cancel() {
+  if (m_parent != nullptr)
+    wpi_setWPIErrorWithContext(
+        CommandIllegalUse,
+        "Can not cancel a command that is part of a command group");
+
+  _Cancel();
+}
+
+bool Command::IsRunning() const { return m_running; }
+
+bool Command::IsInitialized() const { return m_initialized; }
+
+bool Command::IsCompleted() const { return m_completed; }
+
+bool Command::IsCanceled() const { return m_canceled; }
+
+bool Command::IsInterruptible() const { return m_interruptible; }
+
+void Command::SetInterruptible(bool interruptible) {
+  m_interruptible = interruptible;
+}
+
+bool Command::DoesRequire(Subsystem* system) const {
+  return m_requirements.count(system) > 0;
+}
+
+const Command::SubsystemSet& Command::GetRequirements() const {
+  return m_requirements;
+}
+
+CommandGroup* Command::GetGroup() const { return m_parent; }
+
+void Command::SetRunWhenDisabled(bool run) { m_runWhenDisabled = run; }
+
+bool Command::WillRunWhenDisabled() const { return m_runWhenDisabled; }
+
+int Command::GetID() const { return m_commandID; }
+
+void Command::SetTimeout(double timeout) {
+  if (timeout < 0.0)
+    wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
+  else
+    m_timeout = timeout;
+}
+
+bool Command::IsTimedOut() const {
+  return m_timeout != -1 && TimeSinceInitialized() >= m_timeout;
+}
+
+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;
+}
+
+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;
+  }
+}
+
+bool Command::IsParented() const { return m_parent != nullptr; }
+
+void Command::ClearRequirements() { m_requirements.clear(); }
+
+void Command::Initialize() {}
+
+void Command::Execute() {}
+
+void Command::End() {}
+
+void Command::Interrupted() { End(); }
+
+void Command::_Initialize() { m_completed = false; }
+
+void Command::_Interrupted() { m_completed = true; }
+
+void Command::_Execute() {}
+
+void Command::_End() { m_completed = true; }
+
+void Command::_Cancel() {
+  if (IsRunning()) m_canceled = true;
+}
+
+void Command::LockChanges() { m_locked = true; }
+
+void Command::Removed() {
+  if (m_initialized) {
+    if (IsCanceled()) {
+      Interrupted();
+      _Interrupted();
+    } else {
+      End();
+      _End();
+    }
+  }
+  m_initialized = false;
+  m_canceled = false;
+  m_running = false;
+  m_completed = true;
+}
+
+void Command::StartRunning() {
+  m_running = true;
+  m_startTime = -1;
+  m_completed = false;
+}
+
+void Command::StartTiming() { m_startTime = Timer::GetFPGATimestamp(); }
+
+void Command::InitSendable(SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Command");
+  builder.AddStringProperty(".name", [=]() { return GetName(); }, nullptr);
+  builder.AddBooleanProperty("running", [=]() { return IsRunning(); },
+                             [=](bool value) {
+                               if (value) {
+                                 if (!IsRunning()) Start();
+                               } else {
+                                 if (IsRunning()) Cancel();
+                               }
+                             });
+  builder.AddBooleanProperty(".isParented", [=]() { return IsParented(); },
+                             nullptr);
+}

wpilibc/src/main/native/cpp/commands/CommandGroup.cpp

@@ -0,0 +1,243 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. 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 "frc/commands/CommandGroup.h"
+
+#include "frc/WPIErrors.h"
+
+using namespace frc;
+
+CommandGroup::CommandGroup(const wpi::Twine& name) : Command(name) {}
+
+void CommandGroup::AddSequential(Command* command) {
+  if (command == nullptr) {
+    wpi_setWPIErrorWithContext(NullParameter, "command");
+    return;
+  }
+  if (!AssertUnlocked("Cannot add new command to command group")) return;
+
+  m_commands.emplace_back(command, CommandGroupEntry::kSequence_InSequence);
+
+  command->SetParent(this);
+
+  // Iterate through command->GetRequirements() and call Requires() on each
+  // required subsystem
+  for (auto& requirement : command->GetRequirements()) Requires(requirement);
+}
+
+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;
+  }
+
+  m_commands.emplace_back(command, CommandGroupEntry::kSequence_InSequence,
+                          timeout);
+
+  command->SetParent(this);
+
+  // Iterate through command->GetRequirements() and call Requires() on each
+  // required subsystem
+  for (auto& requirement : command->GetRequirements()) Requires(requirement);
+}
+
+void CommandGroup::AddParallel(Command* command) {
+  if (command == nullptr) {
+    wpi_setWPIErrorWithContext(NullParameter, "command");
+    return;
+  }
+  if (!AssertUnlocked("Cannot add new command to command group")) return;
+
+  m_commands.emplace_back(command, CommandGroupEntry::kSequence_BranchChild);
+
+  command->SetParent(this);
+
+  // Iterate through command->GetRequirements() and call Requires() on each
+  // required subsystem
+  for (auto& requirement : command->GetRequirements()) Requires(requirement);
+}
+
+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;
+  }
+
+  m_commands.emplace_back(command, CommandGroupEntry::kSequence_BranchChild,
+                          timeout);
+
+  command->SetParent(this);
+
+  // Iterate through command->GetRequirements() and call Requires() on each
+  // required subsystem
+  for (auto& requirement : command->GetRequirements()) Requires(requirement);
+}
+
+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;
+  }
+
+  for (const auto& child : m_children) {
+    if (!child->m_command->IsInterruptible()) return false;
+  }
+
+  return true;
+}
+
+int CommandGroup::GetSize() const { return m_children.size(); }
+
+void CommandGroup::Initialize() {}
+
+void CommandGroup::Execute() {}
+
+bool CommandGroup::IsFinished() {
+  return static_cast<size_t>(m_currentCommandIndex) >= m_commands.size() &&
+         m_children.empty();
+}
+
+void CommandGroup::End() {}
+
+void CommandGroup::Interrupted() {}
+
+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 there are still commands in this group to run
+  while (static_cast<size_t>(m_currentCommandIndex) < m_commands.size()) {
+    // If a command is prepared to run
+    if (cmd != nullptr) {
+      // If command timed out, cancel it so it's removed from the Scheduler
+      if (entry->IsTimedOut()) cmd->_Cancel();
+
+      // If command finished or was cancelled, remove it from Scheduler
+      if (cmd->Run()) {
+        break;
+      } else {
+        cmd->Removed();
+
+        // Advance to next command in group
+        m_currentCommandIndex++;
+        firstRun = true;
+        cmd = nullptr;
+        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:
+        // Start executing a parallel command and advance to next entry in group
+        m_currentCommandIndex++;
+        entry->m_command->Start();
+        break;
+
+      case CommandGroupEntry::kSequence_BranchChild:
+        m_currentCommandIndex++;
+
+        /* Causes scheduler to skip children of current command which require
+         * the same subsystems as it
+         */
+        CancelConflicts(entry->m_command);
+        entry->m_command->StartRunning();
+
+        // Add current command entry to list of children of this group
+        m_children.push_back(entry);
+        break;
+    }
+  }
+
+  // Run Children
+  for (auto& entry : m_children) {
+    auto child = entry->m_command;
+    if (entry->IsTimedOut()) {
+      child->_Cancel();
+    }
+
+    // If child finished or was cancelled, set it to nullptr. nullptr entries
+    // are removed later.
+    if (!child->Run()) {
+      child->Removed();
+      entry = nullptr;
+    }
+  }
+
+  m_children.erase(std::remove(m_children.begin(), m_children.end(), nullptr),
+                   m_children.end());
+}
+
+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();
+  }
+
+  for (auto& child : m_children) {
+    Command* cmd = child->m_command;
+    cmd->_Cancel();
+    cmd->Removed();
+  }
+  m_children.clear();
+}
+
+void CommandGroup::_Interrupted() { _End(); }
+
+void CommandGroup::CancelConflicts(Command* command) {
+  for (auto childIter = m_children.begin(); childIter != m_children.end();) {
+    Command* child = (*childIter)->m_command;
+    bool erased = false;
+
+    for (auto& requirement : command->GetRequirements()) {
+      if (child->DoesRequire(requirement)) {
+        child->_Cancel();
+        child->Removed();
+        childIter = m_children.erase(childIter);
+        erased = true;
+        break;
+      }
+    }
+    if (!erased) childIter++;
+  }
+}

wpilibc/src/main/native/cpp/commands/CommandGroupEntry.cpp

@@ -0,0 +1,23 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. 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 "frc/commands/CommandGroupEntry.h"
+
+#include "frc/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;
+}

wpilibc/src/main/native/cpp/commands/ConditionalCommand.cpp

@@ -0,0 +1,82 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. 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 "frc/commands/ConditionalCommand.h"
+
+#include <iostream>
+
+#include "frc/commands/Scheduler.h"
+
+using namespace frc;
+
+static void RequireAll(Command& command, Command* onTrue, Command* onFalse) {
+  if (onTrue != nullptr) {
+    for (auto requirement : onTrue->GetRequirements())
+      command.Requires(requirement);
+  }
+  if (onFalse != nullptr) {
+    for (auto requirement : onFalse->GetRequirements())
+      command.Requires(requirement);
+  }
+}
+
+ConditionalCommand::ConditionalCommand(Command* onTrue, Command* onFalse) {
+  m_onTrue = onTrue;
+  m_onFalse = onFalse;
+
+  RequireAll(*this, onTrue, onFalse);
+}
+
+ConditionalCommand::ConditionalCommand(const wpi::Twine& name, Command* onTrue,
+                                       Command* onFalse)
+    : Command(name) {
+  m_onTrue = onTrue;
+  m_onFalse = onFalse;
+
+  RequireAll(*this, onTrue, onFalse);
+}
+
+void ConditionalCommand::_Initialize() {
+  if (Condition()) {
+    m_chosenCommand = m_onTrue;
+  } else {
+    m_chosenCommand = m_onFalse;
+  }
+
+  if (m_chosenCommand != nullptr) {
+    // This is a hack to make cancelling the chosen command inside a
+    // CommandGroup work properly
+    m_chosenCommand->ClearRequirements();
+
+    m_chosenCommand->Start();
+  }
+  Command::_Initialize();
+}
+
+void ConditionalCommand::_Cancel() {
+  if (m_chosenCommand != nullptr && m_chosenCommand->IsRunning()) {
+    m_chosenCommand->Cancel();
+  }
+
+  Command::_Cancel();
+}
+
+bool ConditionalCommand::IsFinished() {
+  if (m_chosenCommand != nullptr) {
+    return m_chosenCommand->IsCompleted();
+  } else {
+    return true;
+  }
+}
+
+void ConditionalCommand::_Interrupted() {
+  if (m_chosenCommand != nullptr && m_chosenCommand->IsRunning()) {
+    m_chosenCommand->Cancel();
+  }
+
+  Command::_Interrupted();
+}

wpilibc/src/main/native/cpp/commands/InstantCommand.cpp

@@ -0,0 +1,14 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. 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 "frc/commands/InstantCommand.h"
+
+using namespace frc;
+
+InstantCommand::InstantCommand(const wpi::Twine& name) : Command(name) {}
+
+bool InstantCommand::IsFinished() { return true; }

wpilibc/src/main/native/cpp/commands/PIDCommand.cpp

@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. 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 "frc/commands/PIDCommand.h"
+
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+PIDCommand::PIDCommand(const wpi::Twine& 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 wpi::Twine& name, double p, double i, double d)
+    : Command(name) {
+  m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+}
+
+PIDCommand::PIDCommand(const wpi::Twine& 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(); }
+
+void PIDCommand::InitSendable(SendableBuilder& builder) {
+  m_controller->InitSendable(builder);
+  Command::InitSendable(builder);
+  builder.SetSmartDashboardType("PIDCommand");
+}

wpilibc/src/main/native/cpp/commands/PIDSubsystem.cpp

@@ -0,0 +1,97 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. 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 "frc/commands/PIDSubsystem.h"
+
+#include "frc/PIDController.h"
+
+using namespace frc;
+
+PIDSubsystem::PIDSubsystem(const wpi::Twine& name, double p, double i, double d)
+    : Subsystem(name) {
+  m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+  AddChild("PIDController", m_controller);
+}
+
+PIDSubsystem::PIDSubsystem(const wpi::Twine& name, double p, double i, double d,
+                           double f)
+    : Subsystem(name) {
+  m_controller = std::make_shared<PIDController>(p, i, d, f, this, this);
+  AddChild("PIDController", m_controller);
+}
+
+PIDSubsystem::PIDSubsystem(const wpi::Twine& 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);
+  AddChild("PIDController", m_controller);
+}
+
+PIDSubsystem::PIDSubsystem(double p, double i, double d)
+    : Subsystem("PIDSubsystem") {
+  m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+  AddChild("PIDController", m_controller);
+}
+
+PIDSubsystem::PIDSubsystem(double p, double i, double d, double f)
+    : Subsystem("PIDSubsystem") {
+  m_controller = std::make_shared<PIDController>(p, i, d, f, this, this);
+  AddChild("PIDController", m_controller);
+}
+
+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);
+  AddChild("PIDController", m_controller);
+}
+
+void PIDSubsystem::Enable() { m_controller->Enable(); }
+
+void PIDSubsystem::Disable() { m_controller->Disable(); }
+
+void PIDSubsystem::PIDWrite(double output) { UsePIDOutput(output); }
+
+double PIDSubsystem::PIDGet() { return ReturnPIDInput(); }
+
+void PIDSubsystem::SetSetpoint(double setpoint) {
+  m_controller->SetSetpoint(setpoint);
+}
+
+void PIDSubsystem::SetSetpointRelative(double deltaSetpoint) {
+  SetSetpoint(GetSetpoint() + deltaSetpoint);
+}
+
+void PIDSubsystem::SetInputRange(double minimumInput, double maximumInput) {
+  m_controller->SetInputRange(minimumInput, maximumInput);
+}
+
+void PIDSubsystem::SetOutputRange(double minimumOutput, double maximumOutput) {
+  m_controller->SetOutputRange(minimumOutput, maximumOutput);
+}
+
+double PIDSubsystem::GetSetpoint() { return m_controller->GetSetpoint(); }
+
+double PIDSubsystem::GetPosition() { return ReturnPIDInput(); }
+
+double PIDSubsystem::GetRate() { return ReturnPIDInput(); }
+
+void PIDSubsystem::SetAbsoluteTolerance(double absValue) {
+  m_controller->SetAbsoluteTolerance(absValue);
+}
+
+void PIDSubsystem::SetPercentTolerance(double percent) {
+  m_controller->SetPercentTolerance(percent);
+}
+
+bool PIDSubsystem::OnTarget() const { return m_controller->OnTarget(); }
+
+std::shared_ptr<PIDController> PIDSubsystem::GetPIDController() {
+  return m_controller;
+}

wpilibc/src/main/native/cpp/commands/PrintCommand.cpp

@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. 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 "frc/commands/PrintCommand.h"
+
+#include <wpi/raw_ostream.h>
+
+using namespace frc;
+
+PrintCommand::PrintCommand(const wpi::Twine& message)
+    : InstantCommand("Print \"" + message + wpi::Twine('"')) {
+  m_message = message.str();
+}
+
+void PrintCommand::Initialize() { wpi::outs() << m_message << '\n'; }

wpilibc/src/main/native/cpp/commands/Scheduler.cpp

@@ -0,0 +1,212 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. 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 "frc/commands/Scheduler.h"
+
+#include <algorithm>
+#include <set>
+
+#include <hal/HAL.h>
+
+#include "frc/WPIErrors.h"
+#include "frc/buttons/ButtonScheduler.h"
+#include "frc/commands/Subsystem.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+Scheduler* Scheduler::GetInstance() {
+  static Scheduler instance;
+  return &instance;
+}
+
+void Scheduler::AddCommand(Command* command) {
+  std::lock_guard<wpi::mutex> lock(m_additionsMutex);
+  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<wpi::mutex> lock(m_buttonsMutex);
+  m_buttons.emplace_back(button);
+}
+
+void Scheduler::RegisterSubsystem(Subsystem* subsystem) {
+  if (subsystem == nullptr) {
+    wpi_setWPIErrorWithContext(NullParameter, "subsystem");
+    return;
+  }
+  m_subsystems.insert(subsystem);
+}
+
+void Scheduler::Run() {
+  // Get button input (going backwards preserves button priority)
+  {
+    if (!m_enabled) return;
+
+    std::lock_guard<wpi::mutex> lock(m_buttonsMutex);
+    for (auto& button : m_buttons) {
+      button->Execute();
+    }
+  }
+
+  // Call every subsystem's periodic method
+  for (auto& subsystem : m_subsystems) {
+    subsystem->Periodic();
+  }
+
+  m_runningCommandsChanged = false;
+
+  // Loop through the commands
+  for (auto cmdIter = m_commands.begin(); cmdIter != m_commands.end();) {
+    Command* command = *cmdIter;
+    // Increment before potentially removing to keep the iterator valid
+    ++cmdIter;
+    if (!command->Run()) {
+      Remove(command);
+      m_runningCommandsChanged = true;
+    }
+  }
+
+  // Add the new things
+  {
+    std::lock_guard<wpi::mutex> lock(m_additionsMutex);
+    for (auto& addition : m_additions) {
+      ProcessCommandAddition(addition);
+    }
+    m_additions.clear();
+  }
+
+  // Add in the defaults
+  for (auto& subsystem : m_subsystems) {
+    if (subsystem->GetCurrentCommand() == nullptr) {
+      ProcessCommandAddition(subsystem->GetDefaultCommand());
+    }
+    subsystem->ConfirmCommand();
+  }
+}
+
+void Scheduler::Remove(Command* command) {
+  if (command == nullptr) {
+    wpi_setWPIErrorWithContext(NullParameter, "command");
+    return;
+  }
+
+  if (!m_commands.erase(command)) return;
+
+  for (auto& requirement : command->GetRequirements()) {
+    requirement->SetCurrentCommand(nullptr);
+  }
+
+  command->Removed();
+}
+
+void Scheduler::RemoveAll() {
+  while (m_commands.size() > 0) {
+    Remove(*m_commands.begin());
+  }
+}
+
+void Scheduler::ResetAll() {
+  RemoveAll();
+  m_subsystems.clear();
+  m_buttons.clear();
+  m_additions.clear();
+  m_commands.clear();
+  m_namesEntry = nt::NetworkTableEntry();
+  m_idsEntry = nt::NetworkTableEntry();
+  m_cancelEntry = nt::NetworkTableEntry();
+}
+
+void Scheduler::SetEnabled(bool enabled) { m_enabled = enabled; }
+
+void Scheduler::InitSendable(SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Scheduler");
+  m_namesEntry = builder.GetEntry("Names");
+  m_idsEntry = builder.GetEntry("Ids");
+  m_cancelEntry = builder.GetEntry("Cancel");
+  builder.SetUpdateTable([=]() {
+    // Get the list of possible commands to cancel
+    auto new_toCancel = m_cancelEntry.GetValue();
+    if (new_toCancel)
+      toCancel = new_toCancel->GetDoubleArray();
+    else
+      toCancel.resize(0);
+
+    // Cancel commands whose cancel buttons were pressed on the SmartDashboard
+    if (!toCancel.empty()) {
+      for (auto& command : m_commands) {
+        for (const auto& cancelled : toCancel) {
+          if (command->GetID() == cancelled) {
+            command->Cancel();
+          }
+        }
+      }
+      toCancel.resize(0);
+      m_cancelEntry.SetDoubleArray(toCancel);
+    }
+
+    // Set the running commands
+    if (m_runningCommandsChanged) {
+      commands.resize(0);
+      ids.resize(0);
+      for (const auto& command : m_commands) {
+        commands.emplace_back(command->GetName());
+        ids.emplace_back(command->GetID());
+      }
+      m_namesEntry.SetStringArray(commands);
+      m_idsEntry.SetDoubleArray(ids);
+    }
+  });
+}
+
+Scheduler::Scheduler() {
+  HAL_Report(HALUsageReporting::kResourceType_Command,
+             HALUsageReporting::kCommand_Scheduler);
+  SetName("Scheduler");
+}
+
+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();
+    for (const auto& requirement : requirements) {
+      if (requirement->GetCurrentCommand() != nullptr &&
+          !requirement->GetCurrentCommand()->IsInterruptible())
+        return;
+    }
+
+    // Give it the requirements
+    m_adding = true;
+    for (auto& requirement : requirements) {
+      if (requirement->GetCurrentCommand() != nullptr) {
+        requirement->GetCurrentCommand()->Cancel();
+        Remove(requirement->GetCurrentCommand());
+      }
+      requirement->SetCurrentCommand(command);
+    }
+    m_adding = false;
+
+    m_commands.insert(command);
+
+    command->StartRunning();
+    m_runningCommandsChanged = true;
+  }
+}

wpilibc/src/main/native/cpp/commands/StartCommand.cpp

@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. 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 "frc/commands/StartCommand.h"
+
+using namespace frc;
+
+StartCommand::StartCommand(Command* commandToStart)
+    : InstantCommand("StartCommand") {
+  m_commandToFork = commandToStart;
+}
+
+void StartCommand::Initialize() { m_commandToFork->Start(); }

wpilibc/src/main/native/cpp/commands/Subsystem.cpp

@@ -0,0 +1,114 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. 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 "frc/commands/Subsystem.h"
+
+#include "frc/WPIErrors.h"
+#include "frc/commands/Command.h"
+#include "frc/commands/Scheduler.h"
+#include "frc/livewindow/LiveWindow.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+Subsystem::Subsystem(const wpi::Twine& name) {
+  SetName(name, name);
+  Scheduler::GetInstance()->RegisterSubsystem(this);
+}
+
+void Subsystem::SetDefaultCommand(Command* command) {
+  if (command == nullptr) {
+    m_defaultCommand = nullptr;
+  } else {
+    const auto& reqs = command->GetRequirements();
+    if (std::find(reqs.begin(), reqs.end(), this) == reqs.end()) {
+      wpi_setWPIErrorWithContext(
+          CommandIllegalUse, "A default command must require the subsystem");
+      return;
+    }
+
+    m_defaultCommand = command;
+  }
+}
+
+Command* Subsystem::GetDefaultCommand() {
+  if (!m_initializedDefaultCommand) {
+    m_initializedDefaultCommand = true;
+    InitDefaultCommand();
+  }
+  return m_defaultCommand;
+}
+
+wpi::StringRef Subsystem::GetDefaultCommandName() {
+  Command* defaultCommand = GetDefaultCommand();
+  if (defaultCommand) {
+    return defaultCommand->GetName();
+  } else {
+    return wpi::StringRef();
+  }
+}
+
+void Subsystem::SetCurrentCommand(Command* command) {
+  m_currentCommand = command;
+  m_currentCommandChanged = true;
+}
+
+Command* Subsystem::GetCurrentCommand() const { return m_currentCommand; }
+
+wpi::StringRef Subsystem::GetCurrentCommandName() const {
+  Command* currentCommand = GetCurrentCommand();
+  if (currentCommand) {
+    return currentCommand->GetName();
+  } else {
+    return wpi::StringRef();
+  }
+}
+
+void Subsystem::Periodic() {}
+
+void Subsystem::InitDefaultCommand() {}
+
+void Subsystem::AddChild(const wpi::Twine& name,
+                         std::shared_ptr<Sendable> child) {
+  AddChild(name, *child);
+}
+
+void Subsystem::AddChild(const wpi::Twine& name, Sendable* child) {
+  AddChild(name, *child);
+}
+
+void Subsystem::AddChild(const wpi::Twine& name, Sendable& child) {
+  child.SetName(GetSubsystem(), name);
+  LiveWindow::GetInstance()->Add(&child);
+}
+
+void Subsystem::AddChild(std::shared_ptr<Sendable> child) { AddChild(*child); }
+
+void Subsystem::AddChild(Sendable* child) { AddChild(*child); }
+
+void Subsystem::AddChild(Sendable& child) {
+  child.SetSubsystem(GetSubsystem());
+  LiveWindow::GetInstance()->Add(&child);
+}
+
+void Subsystem::ConfirmCommand() {
+  if (m_currentCommandChanged) m_currentCommandChanged = false;
+}
+
+void Subsystem::InitSendable(SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Subsystem");
+
+  builder.AddBooleanProperty(
+      ".hasDefault", [=]() { return m_defaultCommand != nullptr; }, nullptr);
+  builder.AddStringProperty(".default",
+                            [=]() { return GetDefaultCommandName(); }, nullptr);
+
+  builder.AddBooleanProperty(
+      ".hasCommand", [=]() { return m_currentCommand != nullptr; }, nullptr);
+  builder.AddStringProperty(".command",
+                            [=]() { return GetCurrentCommandName(); }, nullptr);
+}

wpilibc/src/main/native/cpp/commands/TimedCommand.cpp

@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. 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 "frc/commands/TimedCommand.h"
+
+using namespace frc;
+
+TimedCommand::TimedCommand(const wpi::Twine& name, double timeout)
+    : Command(name, timeout) {}
+
+TimedCommand::TimedCommand(double timeout) : Command(timeout) {}
+
+bool TimedCommand::IsFinished() { return IsTimedOut(); }

wpilibc/src/main/native/cpp/commands/WaitCommand.cpp

@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. 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 "frc/commands/WaitCommand.h"
+
+using namespace frc;
+
+WaitCommand::WaitCommand(double timeout)
+    : TimedCommand("Wait(" + std::to_string(timeout) + ")", timeout) {}
+
+WaitCommand::WaitCommand(const wpi::Twine& name, double timeout)
+    : TimedCommand(name, timeout) {}

wpilibc/src/main/native/cpp/commands/WaitForChildren.cpp

@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. 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 "frc/commands/WaitForChildren.h"
+
+#include "frc/commands/CommandGroup.h"
+
+using namespace frc;
+
+WaitForChildren::WaitForChildren(double timeout)
+    : Command("WaitForChildren", timeout) {}
+
+WaitForChildren::WaitForChildren(const wpi::Twine& name, double timeout)
+    : Command(name, timeout) {}
+
+bool WaitForChildren::IsFinished() {
+  return GetGroup() == nullptr || GetGroup()->GetSize() == 0;
+}

wpilibc/src/main/native/cpp/commands/WaitUntilCommand.cpp

@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. 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 "frc/commands/WaitUntilCommand.h"
+
+#include "frc/Timer.h"
+
+using namespace frc;
+
+WaitUntilCommand::WaitUntilCommand(double time)
+    : Command("WaitUntilCommand", time) {
+  m_time = time;
+}
+
+WaitUntilCommand::WaitUntilCommand(const wpi::Twine& name, double time)
+    : Command(name, time) {
+  m_time = time;
+}
+
+bool WaitUntilCommand::IsFinished() { return Timer::GetMatchTime() >= m_time; }