diff --git a/wpilibc/shared/include/PIDController.h b/wpilibc/shared/include/PIDController.h
index 301356c..5e74389 100644
--- a/wpilibc/shared/include/PIDController.h
+++ b/wpilibc/shared/include/PIDController.h
@@ -1,9 +1,10 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2008. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2008-2016. 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.  */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
 /*----------------------------------------------------------------------------*/
+
 #pragma once
 
 #include "Base.h"
@@ -13,6 +14,7 @@
 #include "PIDSource.h"
 #include "Notifier.h"
 #include "HAL/cpp/priority_mutex.h"
+#include "Timer.h"
 
 #include <memory>
 
@@ -54,6 +56,7 @@
 
   virtual void SetSetpoint(float setpoint) override;
   virtual double GetSetpoint() const override;
+  double GetDeltaSetpoint() const;
 
   virtual float GetError() const;
   virtual float GetAvgError() const;
@@ -81,6 +84,7 @@
 
   std::shared_ptr<ITable> m_table;
   virtual void Calculate();
+  virtual double CalculateFeedForward();
 
  private:
   float m_P;              // factor for "proportional" control
@@ -93,7 +97,7 @@
   float m_minimumInput = 0;   // minimum input - limit setpoint to this
   bool m_continuous = false;      // do the endpoints wrap around? eg. Absolute encoder
   bool m_enabled = false;  // is the pid controller enabled
-  float m_prevInput = 0;  // the prior sensor input (used to compute velocity)
+  float m_prevError = 0;  // the prior error (used to compute velocity)
   double m_totalError = 0;  // the sum of the errors for use in the integral calc
   enum {
     kAbsoluteTolerance,
@@ -104,6 +108,7 @@
   // the percetage or absolute error that is considered on target.
   float m_tolerance = 0.05;
   float m_setpoint = 0;
+  float m_prevSetpoint = 0;
   float m_error = 0;
   float m_result = 0;
   float m_period;
@@ -113,13 +118,13 @@
   std::queue<double> m_buf;
   double m_bufTotal = 0;
 
-  mutable priority_mutex m_mutex;
+  mutable priority_recursive_mutex m_mutex;
 
   std::unique_ptr<Notifier> m_controlLoop;
+  Timer m_setpointTimer;
 
   void Initialize(float p, float i, float d, float f, PIDSource *source,
                   PIDOutput *output, float period = 0.05);
-  static void CallCalculate(void *controller);
 
   virtual std::shared_ptr<ITable> GetTable() const override;
   virtual std::string GetSmartDashboardType() const override;