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.