Squashed 'third_party/allwpilib_2016/' content from commit 7f61816
Change-Id: If9d9245880859cdf580f5d7f77045135d0521ce7
git-subtree-dir: third_party/allwpilib_2016
git-subtree-split: 7f618166ed253a24629934fcf89c3decb0528a3b
diff --git a/wpilibc/shared/src/Commands/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; }