From aa0a874ad06fb2a57c7be68ffd79ff955475a863 Mon Sep 17 00:00:00 2001
From: Austin Shalit <austinshalit@gmail.com>
Date: Sun, 2 Jul 2017 23:02:41 -0700
Subject: [PATCH] Add periodic method to subsystems (#528)

Fixes #526.
---
 wpilibc/shared/include/Commands/Subsystem.h                | 1 +
 wpilibc/shared/src/Commands/Scheduler.cpp                  | 7 +++++++
 wpilibc/shared/src/Commands/Subsystem.cpp                  | 5 +++++
 .../java/edu/wpi/first/wpilibj/command/Scheduler.java      | 7 +++++++
 .../java/edu/wpi/first/wpilibj/command/Subsystem.java      | 7 +++++++
 5 files changed, 27 insertions(+)

diff --git a/wpilibc/shared/include/Commands/Subsystem.h b/wpilibc/shared/include/Commands/Subsystem.h
index 312109857d..23f10b6351 100644
--- a/wpilibc/shared/include/Commands/Subsystem.h
+++ b/wpilibc/shared/include/Commands/Subsystem.h
@@ -28,6 +28,7 @@ class Subsystem : public ErrorBase, public NamedSendable {
   Command* GetDefaultCommand();
   void SetCurrentCommand(Command* command);
   Command* GetCurrentCommand() const;
+  virtual void Periodic();
   virtual void InitDefaultCommand();
 
  private:
diff --git a/wpilibc/shared/src/Commands/Scheduler.cpp b/wpilibc/shared/src/Commands/Scheduler.cpp
index 19da1dd1c5..04d192ffe5 100644
--- a/wpilibc/shared/src/Commands/Scheduler.cpp
+++ b/wpilibc/shared/src/Commands/Scheduler.cpp
@@ -121,6 +121,13 @@ void Scheduler::Run() {
     }
   }
 
+  // Call every subsystem's periodic method
+  for (auto subsystemIter = m_subsystems.begin();
+       subsystemIter != m_subsystems.end(); subsystemIter++) {
+    Subsystem* subsystem = *subsystemIter;
+    subsystem->Periodic();
+  }
+
   m_runningCommandsChanged = false;
 
   // Loop through the commands
diff --git a/wpilibc/shared/src/Commands/Subsystem.cpp b/wpilibc/shared/src/Commands/Subsystem.cpp
index e12febfd44..7e73fa9b38 100644
--- a/wpilibc/shared/src/Commands/Subsystem.cpp
+++ b/wpilibc/shared/src/Commands/Subsystem.cpp
@@ -104,6 +104,11 @@ void Subsystem::SetCurrentCommand(Command* command) {
  */
 Command* Subsystem::GetCurrentCommand() const { return m_currentCommand; }
 
+/**
+ * When the run method of the scheduler is called this method will be called.
+ */
+void Subsystem::Periodic() {}
+
 /**
  * Call this to alert Subsystem that the current command is actually the
  * command.
diff --git a/wpilibj/src/shared/java/edu/wpi/first/wpilibj/command/Scheduler.java b/wpilibj/src/shared/java/edu/wpi/first/wpilibj/command/Scheduler.java
index 0f39d91a9e..d078c303e4 100644
--- a/wpilibj/src/shared/java/edu/wpi/first/wpilibj/command/Scheduler.java
+++ b/wpilibj/src/shared/java/edu/wpi/first/wpilibj/command/Scheduler.java
@@ -198,6 +198,13 @@ public class Scheduler implements NamedSendable {
         ((ButtonScheduler) m_buttons.elementAt(i)).execute();
       }
     }
+
+    // Call every subsystem's periodic method
+    Enumeration subsystems = m_subsystems.getElements();
+    while (subsystems.hasMoreElements()) {
+      ((Subsystem) subsystems.nextElement()).periodic();
+    }
+
     // Loop through the commands
     LinkedListElement element = m_firstCommand;
     while (element != null) {
diff --git a/wpilibj/src/shared/java/edu/wpi/first/wpilibj/command/Subsystem.java b/wpilibj/src/shared/java/edu/wpi/first/wpilibj/command/Subsystem.java
index ab178261bc..c88c907464 100644
--- a/wpilibj/src/shared/java/edu/wpi/first/wpilibj/command/Subsystem.java
+++ b/wpilibj/src/shared/java/edu/wpi/first/wpilibj/command/Subsystem.java
@@ -73,6 +73,13 @@ public abstract class Subsystem implements NamedSendable {
    */
   protected abstract void initDefaultCommand();
 
+  /**
+   * When the run method of the scheduler is called this method will be called.
+   */
+  public void periodic() {
+    // Override me!
+  }
+
   /**
    * Sets the default command. If this is not called or is called with null, then there will be no
    * default command for the subsystem.