Add periodic method to subsystems (#528)

Fixes #526.

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:

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

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.

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) {

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.