diff --git a/wpilibc/Athena/src/PIDController.cpp b/wpilibc/Athena/src/PIDController.cpp
index eaeec8f010..81e97fc83a 100644
--- a/wpilibc/Athena/src/PIDController.cpp
+++ b/wpilibc/Athena/src/PIDController.cpp
@@ -67,6 +67,7 @@ void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf,
   m_period = period;
 
   m_controlLoop->StartPeriodic(m_period);
+  m_setpointTimer.Start();
 
   static int32_t instances = 0;
   instances++;
@@ -87,7 +88,7 @@ void PIDController::Calculate() {
   PIDOutput *pidOutput;
 
   {
-    std::lock_guard<priority_mutex> sync(m_mutex);
+    std::lock_guard<priority_recursive_mutex> sync(m_mutex);
     pidInput = m_pidInput;
     pidOutput = m_pidOutput;
     enabled = m_enabled;
@@ -97,7 +98,7 @@ void PIDController::Calculate() {
   if (pidOutput == nullptr) return;
 
   if (enabled) {
-    std::lock_guard<priority_mutex> sync(m_mutex);
+    std::lock_guard<priority_recursive_mutex> sync(m_mutex);
     float input = pidInput->PIDGet();
     float result;
     PIDOutput *pidOutput;
@@ -126,7 +127,7 @@ void PIDController::Calculate() {
         }
       }
 
-      m_result = m_D * m_error + m_P * m_totalError + m_setpoint * m_F;
+      m_result = m_D * m_error + m_P * m_totalError + CalculateFeedForward();
     }
     else {
       if (m_I != 0) {
@@ -142,9 +143,9 @@ void PIDController::Calculate() {
       }
 
       m_result = m_P * m_error + m_I * m_totalError +
-                 m_D * (m_prevInput - input) + m_setpoint * m_F;
+                 m_D * (m_error - m_prevError) + CalculateFeedForward();
     }
-    m_prevInput = input;
+    m_prevError = m_error;
 
     if (m_result > m_maximumOutput)
       m_result = m_maximumOutput;
@@ -167,6 +168,33 @@ void PIDController::Calculate() {
   }
 }
 
+/**
+ * Calculate the feed forward term
+ *
+ * Both of the provided feed forward calculations are velocity feed forwards.
+ * If a different feed forward calculation is desired, the user can override
+ * this function and provide his or her own. This function  does no
+ * synchronization because the PIDController class only calls it in synchronized
+ * code, so be careful if calling it oneself.
+ *
+ * If a velocity PID controller is being used, the F term should be set to 1
+ * over the maximum setpoint for the output. If a position PID controller is
+ * being used, the F term should be set to 1 over the maximum speed for the
+ * output measured in setpoint units per this controller's update period (see
+ * the default period in this class's constructor).
+ */
+double PIDController::CalculateFeedForward() {
+  if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) {
+    return m_F * GetSetpoint();
+  }
+  else {
+    double temp = m_F * GetDeltaSetpoint();
+    m_prevSetpoint = m_setpoint;
+    m_setpointTimer.Reset();
+    return temp;
+  }
+}
+
 /**
  * Set the PID Controller gain parameters.
  * Set the proportional, integral, and differential coefficients.
@@ -176,7 +204,7 @@ void PIDController::Calculate() {
  */
 void PIDController::SetPID(double p, double i, double d) {
   {
-    std::lock_guard<priority_mutex> sync(m_mutex);
+    std::lock_guard<priority_recursive_mutex> sync(m_mutex);
     m_P = p;
     m_I = i;
     m_D = d;
@@ -199,7 +227,7 @@ void PIDController::SetPID(double p, double i, double d) {
  */
 void PIDController::SetPID(double p, double i, double d, double f) {
   {
-    std::lock_guard<priority_mutex> sync(m_mutex);
+    std::lock_guard<priority_recursive_mutex> sync(m_mutex);
     m_P = p;
     m_I = i;
     m_D = d;
@@ -219,7 +247,7 @@ void PIDController::SetPID(double p, double i, double d, double f) {
  * @return proportional coefficient
  */
 double PIDController::GetP() const {
-  std::lock_guard<priority_mutex> sync(m_mutex);
+  std::lock_guard<priority_recursive_mutex> sync(m_mutex);
   return m_P;
 }
 
@@ -228,7 +256,7 @@ double PIDController::GetP() const {
  * @return integral coefficient
  */
 double PIDController::GetI() const {
-  std::lock_guard<priority_mutex> sync(m_mutex);
+  std::lock_guard<priority_recursive_mutex> sync(m_mutex);
   return m_I;
 }
 
@@ -237,7 +265,7 @@ double PIDController::GetI() const {
  * @return differential coefficient
  */
 double PIDController::GetD() const {
-  std::lock_guard<priority_mutex> sync(m_mutex);
+  std::lock_guard<priority_recursive_mutex> sync(m_mutex);
   return m_D;
 }
 
@@ -246,7 +274,7 @@ double PIDController::GetD() const {
  * @return Feed forward coefficient
  */
 double PIDController::GetF() const {
-  std::lock_guard<priority_mutex> sync(m_mutex);
+  std::lock_guard<priority_recursive_mutex> sync(m_mutex);
   return m_F;
 }
 
@@ -256,7 +284,7 @@ double PIDController::GetF() const {
  * @return the latest calculated output
  */
 float PIDController::Get() const {
-  std::lock_guard<priority_mutex> sync(m_mutex);
+  std::lock_guard<priority_recursive_mutex> sync(m_mutex);
   return m_result;
 }
 
@@ -268,7 +296,7 @@ float PIDController::Get() const {
  * @param continuous Set to true turns on continuous, false turns off continuous
  */
 void PIDController::SetContinuous(bool continuous) {
-  std::lock_guard<priority_mutex> sync(m_mutex);
+  std::lock_guard<priority_recursive_mutex> sync(m_mutex);
   m_continuous = continuous;
 }
 
@@ -280,7 +308,7 @@ void PIDController::SetContinuous(bool continuous) {
  */
 void PIDController::SetInputRange(float minimumInput, float maximumInput) {
   {
-    std::lock_guard<priority_mutex> sync(m_mutex);
+    std::lock_guard<priority_recursive_mutex> sync(m_mutex);
     m_minimumInput = minimumInput;
     m_maximumInput = maximumInput;
   }
@@ -296,7 +324,7 @@ void PIDController::SetInputRange(float minimumInput, float maximumInput) {
  */
 void PIDController::SetOutputRange(float minimumOutput, float maximumOutput) {
   {
-    std::lock_guard<priority_mutex> sync(m_mutex);
+    std::lock_guard<priority_recursive_mutex> sync(m_mutex);
     m_minimumOutput = minimumOutput;
     m_maximumOutput = maximumOutput;
   }
@@ -309,7 +337,8 @@ void PIDController::SetOutputRange(float minimumOutput, float maximumOutput) {
  */
 void PIDController::SetSetpoint(float setpoint) {
   {
-    std::lock_guard<priority_mutex> sync(m_mutex);
+    std::lock_guard<priority_recursive_mutex> sync(m_mutex);
+
     if (m_maximumInput > m_minimumInput) {
       if (setpoint > m_maximumInput)
         m_setpoint = m_maximumInput;
@@ -335,10 +364,19 @@ void PIDController::SetSetpoint(float setpoint) {
  * @return the current setpoint
  */
 double PIDController::GetSetpoint() const {
-  std::lock_guard<priority_mutex> sync(m_mutex);
+  std::lock_guard<priority_recursive_mutex> sync(m_mutex);
   return m_setpoint;
 }
 
+/**
+ * Returns the change in setpoint over time of the PIDController
+ * @return the change in setpoint over time
+ */
+double PIDController::GetDeltaSetpoint() const {
+  std::lock_guard<priority_recursive_mutex> sync(m_mutex);
+  return (m_setpoint - m_prevSetpoint) / m_setpointTimer.Get();
+}
+
 /**
  * Returns the current difference of the input from the setpoint
  * @return the current error
@@ -346,7 +384,7 @@ double PIDController::GetSetpoint() const {
 float PIDController::GetError() const {
   double pidInput;
   {
-    std::lock_guard<priority_mutex> sync(m_mutex);
+    std::lock_guard<priority_recursive_mutex> sync(m_mutex);
     pidInput = m_pidInput->PIDGet();
   }
   return GetSetpoint() - pidInput;
@@ -375,7 +413,7 @@ PIDSourceType PIDController::GetPIDSourceType() const {
 float PIDController::GetAvgError() const {
   float avgError = 0;
   {
-    std::lock_guard<priority_mutex> sync(m_mutex);
+    std::lock_guard<priority_recursive_mutex> sync(m_mutex);
     // Don't divide by zero.
     if (m_buf.size()) avgError = m_bufTotal / m_buf.size();
   }
@@ -389,7 +427,7 @@ float PIDController::GetAvgError() const {
  */
 void PIDController::SetTolerance(float percent) {
   {
-    std::lock_guard<priority_mutex> sync(m_mutex);
+    std::lock_guard<priority_recursive_mutex> sync(m_mutex);
     m_toleranceType = kPercentTolerance;
     m_tolerance = percent;
   }
@@ -402,7 +440,7 @@ void PIDController::SetTolerance(float percent) {
  */
 void PIDController::SetPercentTolerance(float percent) {
   {
-    std::lock_guard<priority_mutex> sync(m_mutex);
+    std::lock_guard<priority_recursive_mutex> sync(m_mutex);
     m_toleranceType = kPercentTolerance;
     m_tolerance = percent;
   }
@@ -415,7 +453,7 @@ void PIDController::SetPercentTolerance(float percent) {
  */
 void PIDController::SetAbsoluteTolerance(float absTolerance) {
   {
-    std::lock_guard<priority_mutex> sync(m_mutex);
+    std::lock_guard<priority_recursive_mutex> sync(m_mutex);
     m_toleranceType = kAbsoluteTolerance;
     m_tolerance = absTolerance;
   }
@@ -452,7 +490,7 @@ void PIDController::SetToleranceBuffer(unsigned bufLength) {
 bool PIDController::OnTarget() const {
   double error = GetAvgError();
 
-  std::lock_guard<priority_mutex> sync(m_mutex);
+  std::lock_guard<priority_recursive_mutex> sync(m_mutex);
   switch (m_toleranceType) {
     case kPercentTolerance:
       return fabs(error) < m_tolerance / 100 * (m_maximumInput - m_minimumInput);
@@ -472,7 +510,7 @@ bool PIDController::OnTarget() const {
  */
 void PIDController::Enable() {
   {
-    std::lock_guard<priority_mutex> sync(m_mutex);
+    std::lock_guard<priority_recursive_mutex> sync(m_mutex);
     m_enabled = true;
   }
 
@@ -486,7 +524,7 @@ void PIDController::Enable() {
  */
 void PIDController::Disable() {
   {
-    std::lock_guard<priority_mutex> sync(m_mutex);
+    std::lock_guard<priority_recursive_mutex> sync(m_mutex);
     m_pidOutput->PIDWrite(0);
     m_enabled = false;
   }
@@ -500,7 +538,7 @@ void PIDController::Disable() {
  * Return true if PIDController is enabled.
  */
 bool PIDController::IsEnabled() const {
-  std::lock_guard<priority_mutex> sync(m_mutex);
+  std::lock_guard<priority_recursive_mutex> sync(m_mutex);
   return m_enabled;
 }
 
@@ -510,8 +548,8 @@ bool PIDController::IsEnabled() const {
 void PIDController::Reset() {
   Disable();
 
-  std::lock_guard<priority_mutex> sync(m_mutex);
-  m_prevInput = 0;
+  std::lock_guard<priority_recursive_mutex> sync(m_mutex);
+  m_prevError = 0;
   m_totalError = 0;
   m_result = 0;
 }
diff --git a/wpilibc/shared/include/PIDController.h b/wpilibc/shared/include/PIDController.h
index e0a9521fd6..5e743894e2 100644
--- a/wpilibc/shared/include/PIDController.h
+++ b/wpilibc/shared/include/PIDController.h
@@ -14,6 +14,7 @@
 #include "PIDSource.h"
 #include "Notifier.h"
 #include "HAL/cpp/priority_mutex.h"
+#include "Timer.h"
 
 #include <memory>
 
@@ -55,6 +56,7 @@ class PIDController : public LiveWindowSendable,
 
   virtual void SetSetpoint(float setpoint) override;
   virtual double GetSetpoint() const override;
+  double GetDeltaSetpoint() const;
 
   virtual float GetError() const;
   virtual float GetAvgError() const;
@@ -82,6 +84,7 @@ class PIDController : public LiveWindowSendable,
 
   std::shared_ptr<ITable> m_table;
   virtual void Calculate();
+  virtual double CalculateFeedForward();
 
  private:
   float m_P;              // factor for "proportional" control
@@ -94,7 +97,7 @@ class PIDController : public LiveWindowSendable,
   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,
@@ -105,6 +108,7 @@ class PIDController : public LiveWindowSendable,
   // 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;
@@ -114,9 +118,10 @@ class PIDController : public LiveWindowSendable,
   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);
diff --git a/wpilibc/simulation/src/PIDController.cpp b/wpilibc/simulation/src/PIDController.cpp
index fa42439191..f337d45b30 100644
--- a/wpilibc/simulation/src/PIDController.cpp
+++ b/wpilibc/simulation/src/PIDController.cpp
@@ -75,7 +75,7 @@ void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf,
 	m_enabled = false;
 	m_setpoint = 0;
 
-	m_prevInput = 0;
+	m_prevError = 0;
 	m_totalError = 0;
 	m_tolerance = .05;
 
@@ -108,7 +108,7 @@ void PIDController::Calculate()
 	PIDSource *pidInput;
 
 	{
-		std::lock_guard<priority_mutex> lock(m_mutex);
+		std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 		if (m_pidInput == 0) return;
 		if (m_pidOutput == 0) return;
 		enabled = m_enabled;
@@ -122,7 +122,7 @@ void PIDController::Calculate()
 		PIDOutput *pidOutput;
 
 		{
-			std::lock_guard<priority_mutex> sync(m_mutex);
+			std::lock_guard<priority_recursive_mutex> sync(m_mutex);
 			m_error = m_setpoint - input;
 			if (m_continuous)
 			{
@@ -155,7 +155,8 @@ void PIDController::Calculate()
                 }
               }
 
-              m_result = m_D * m_error + m_P * m_totalError + m_setpoint * m_F;
+              m_result = m_D * m_error + m_P * m_totalError +
+                         CalculateFeedForward();
             }
             else {
               if (m_I != 0) {
@@ -173,9 +174,10 @@ void PIDController::Calculate()
                 }
               }
 
-              m_result = m_P * m_error + m_I * m_totalError + m_D * (m_prevInput - input) + m_setpoint * m_F;
+              m_result = m_P * m_error + m_I * m_totalError +
+                         m_D * (m_error - m_prevError) + CalculateFeedForward();
             }
-			m_prevInput = input;
+			m_prevError = m_error;
 
 			if (m_result > m_maximumOutput) m_result = m_maximumOutput;
 			else if (m_result < m_minimumOutput) m_result = m_minimumOutput;
@@ -188,6 +190,33 @@ void PIDController::Calculate()
 	}
 }
 
+/**
+ * Calculate the feed forward term
+ *
+ * Both of the provided feed forward calculations are velocity feed forwards.
+ * If a different feed forward calculation is desired, the user can override
+ * this function and provide his or her own. This function  does no
+ * synchronization because the PIDController class only calls it in synchronized
+ * code, so be careful if calling it oneself.
+ *
+ * If a velocity PID controller is being used, the F term should be set to 1
+ * over the maximum setpoint for the output. If a position PID controller is
+ * being used, the F term should be set to 1 over the maximum speed for the
+ * output measured in setpoint units per this controller's update period (see
+ * the default period in this class's constructor).
+ */
+double PIDController::CalculateFeedForward() {
+  if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) {
+    return m_F * GetSetpoint();
+  }
+  else {
+    double temp = m_F * GetDeltaSetpoint();
+    m_prevSetpoint = m_setpoint;
+    m_setpointTimer.Reset();
+    return temp;
+  }
+}
+
 /**
  * Set the PID Controller gain parameters.
  * Set the proportional, integral, and differential coefficients.
@@ -198,7 +227,7 @@ void PIDController::Calculate()
 void PIDController::SetPID(double p, double i, double d)
 {
 	{
-		std::lock_guard<priority_mutex> lock(m_mutex);
+		std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 		m_P = p;
 		m_I = i;
 		m_D = d;
@@ -222,7 +251,7 @@ void PIDController::SetPID(double p, double i, double d)
 void PIDController::SetPID(double p, double i, double d, double f)
 {
 	{
-		std::lock_guard<priority_mutex> lock(m_mutex);
+		std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 		m_P = p;
 		m_I = i;
 		m_D = d;
@@ -243,7 +272,7 @@ void PIDController::SetPID(double p, double i, double d, double f)
  */
 double PIDController::GetP() const
 {
-	std::lock_guard<priority_mutex> lock(m_mutex);
+	std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 	return m_P;
 }
 
@@ -253,7 +282,7 @@ double PIDController::GetP() const
  */
 double PIDController::GetI() const
 {
-	std::lock_guard<priority_mutex> lock(m_mutex);
+	std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 	return m_I;
 }
 
@@ -263,7 +292,7 @@ double PIDController::GetI() const
  */
 double PIDController::GetD() const
 {
-	std::lock_guard<priority_mutex> lock(m_mutex);
+	std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 	return m_D;
 }
 
@@ -273,7 +302,7 @@ double PIDController::GetD() const
  */
 double PIDController::GetF() const
 {
-	std::lock_guard<priority_mutex> lock(m_mutex);
+	std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 	return m_F;
 }
 
@@ -284,7 +313,7 @@ double PIDController::GetF() const
  */
 float PIDController::Get() const
 {
-	std::lock_guard<priority_mutex> lock(m_mutex);
+	std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 	return m_result;
 }
 
@@ -297,7 +326,7 @@ float PIDController::Get() const
  */
 void PIDController::SetContinuous(bool continuous)
 {
-	std::lock_guard<priority_mutex> lock(m_mutex);
+	std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 	m_continuous = continuous;
 }
 
@@ -310,7 +339,7 @@ void PIDController::SetContinuous(bool continuous)
 void PIDController::SetInputRange(float minimumInput, float maximumInput)
 {
 	{
-		std::lock_guard<priority_mutex> lock(m_mutex);
+		std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 		m_minimumInput = minimumInput;
 		m_maximumInput = maximumInput;
 	}
@@ -326,7 +355,7 @@ void PIDController::SetInputRange(float minimumInput, float maximumInput)
  */
 void PIDController::SetOutputRange(float minimumOutput, float maximumOutput)
 {
-	std::lock_guard<priority_mutex> lock(m_mutex);
+	std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 	m_minimumOutput = minimumOutput;
 	m_maximumOutput = maximumOutput;
 }
@@ -338,7 +367,8 @@ void PIDController::SetOutputRange(float minimumOutput, float maximumOutput)
 void PIDController::SetSetpoint(float setpoint)
 {
 	{
-		std::lock_guard<priority_mutex> lock(m_mutex);
+		std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+
 		if (m_maximumInput > m_minimumInput)
 		{
 			if (setpoint > m_maximumInput)
@@ -365,10 +395,20 @@ void PIDController::SetSetpoint(float setpoint)
  */
 double PIDController::GetSetpoint() const
 {
-	std::lock_guard<priority_mutex> lock(m_mutex);
+	std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 	return m_setpoint;
 }
 
+/**
+ * Returns the change in setpoint over time of the PIDController
+ * @return the change in setpoint over time
+ */
+double PIDController::GetDeltaSetpoint() const
+{
+	std::lock_guard<priority_recursive_mutex> sync(m_mutex);
+	return (m_setpoint - m_prevSetpoint) / m_setpointTimer.Get();
+}
+
 /**
  * Retruns the current difference of the input from the setpoint
  * @return the current error
@@ -377,7 +417,7 @@ float PIDController::GetError() const
 {
 	double pidInput;
 	{
-		std::lock_guard<priority_mutex> lock(m_mutex);
+		std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 		pidInput = m_pidInput->PIDGet();
 	}
 	return GetSetpoint() - pidInput;
@@ -407,7 +447,7 @@ PIDSourceType PIDController::GetPIDSourceType() const {
 float PIDController::GetAvgError() const {
   float avgError = 0;
   {
-    std::lock_guard<priority_mutex> sync(m_mutex);
+    std::lock_guard<priority_recursive_mutex> sync(m_mutex);
     // Don't divide by zero.
     if (m_buf.size()) avgError = m_bufTotal / m_buf.size();
   }
@@ -421,7 +461,7 @@ float PIDController::GetAvgError() const {
  */
 void PIDController::SetTolerance(float percent)
 {
-	std::lock_guard<priority_mutex> lock(m_mutex);
+	std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 	m_toleranceType = kPercentTolerance;
 	m_tolerance = percent;
 }
@@ -433,7 +473,7 @@ void PIDController::SetTolerance(float percent)
  */
 void PIDController::SetPercentTolerance(float percent)
 {
-	std::lock_guard<priority_mutex> lock(m_mutex);
+	std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 	m_toleranceType = kPercentTolerance;
 	m_tolerance = percent;
 }
@@ -445,7 +485,7 @@ void PIDController::SetPercentTolerance(float percent)
  */
 void PIDController::SetAbsoluteTolerance(float absTolerance)
 {
-	std::lock_guard<priority_mutex> lock(m_mutex);
+	std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 	m_toleranceType = kAbsoluteTolerance;
 	m_tolerance = absTolerance;
 }
@@ -480,7 +520,7 @@ bool PIDController::OnTarget() const
 {
 	double error = GetError();
 
-	std::lock_guard<priority_mutex> sync(m_mutex);
+	std::lock_guard<priority_recursive_mutex> sync(m_mutex);
 	switch (m_toleranceType) {
 	case kPercentTolerance:
 		return fabs(error) < m_tolerance / 100 * (m_maximumInput - m_minimumInput);
@@ -500,7 +540,7 @@ bool PIDController::OnTarget() const
 void PIDController::Enable()
 {
 	{
-		std::lock_guard<priority_mutex> lock(m_mutex);
+		std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 		m_enabled = true;
 	}
 
@@ -515,7 +555,7 @@ void PIDController::Enable()
 void PIDController::Disable()
 {
 	{
-		std::lock_guard<priority_mutex> lock(m_mutex);
+		std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 		m_pidOutput->PIDWrite(0);
 		m_enabled = false;
 	}
@@ -530,7 +570,7 @@ void PIDController::Disable()
  */
 bool PIDController::IsEnabled() const
 {
-	std::lock_guard<priority_mutex> lock(m_mutex);
+	std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 	return m_enabled;
 }
 
@@ -541,8 +581,8 @@ void PIDController::Reset()
 {
 	Disable();
 
-	std::lock_guard<priority_mutex> lock(m_mutex);
-	m_prevInput = 0;
+	std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+	m_prevError = 0;
 	m_totalError = 0;
 	m_result = 0;
 }
diff --git a/wpilibj/src/shared/java/edu/wpi/first/wpilibj/PIDController.java b/wpilibj/src/shared/java/edu/wpi/first/wpilibj/PIDController.java
index d6f74a34dc..da0989bcca 100644
--- a/wpilibj/src/shared/java/edu/wpi/first/wpilibj/PIDController.java
+++ b/wpilibj/src/shared/java/edu/wpi/first/wpilibj/PIDController.java
@@ -8,6 +8,7 @@ package edu.wpi.first.wpilibj;
 import java.util.TimerTask;
 import java.util.LinkedList;
 
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
 import edu.wpi.first.wpilibj.tables.ITable;
 import edu.wpi.first.wpilibj.tables.ITableListener;
@@ -34,7 +35,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
   private boolean m_continuous = false; // do the endpoints wrap around? eg.
                                         // Absolute encoder
   private boolean m_enabled = false; // is the pid controller enabled
-  private double m_prevInput = 0.0; // the prior sensor input (used to compute
+  private double m_prevError = 0.0; // the prior error (used to compute
                                     // velocity)
   private double m_totalError = 0.0; // the sum of the errors for use in the
                                      // integral calc
@@ -44,12 +45,14 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
   private LinkedList<Double> m_buf;
   private double m_bufTotal = 0.0;
   private double m_setpoint = 0.0;
+  private double m_prevSetpoint = 0.0;
   private double m_error = 0.0;
   private double m_result = 0.0;
   private double m_period = kDefaultPeriod;
   protected PIDSource m_pidInput;
   protected PIDOutput m_pidOutput;
   java.util.Timer m_controlLoop;
+  Timer m_setpointTimer;
   private boolean m_freed = false;
   private boolean m_usingPercentTolerance;
 
@@ -141,7 +144,8 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
     }
 
     m_controlLoop = new java.util.Timer();
-
+    m_setpointTimer = new Timer();
+    m_setpointTimer.start();
 
     m_P = Kp;
     m_I = Ki;
@@ -275,7 +279,8 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
               m_totalError = m_maximumOutput / m_P;
             }
 
-            m_result = m_P * m_totalError + m_D * m_error + m_setpoint * m_F;
+            m_result = m_P * m_totalError + m_D * m_error +
+                       calculateFeedForward();
           }
         }
         else {
@@ -292,10 +297,10 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
             }
           }
 
-          m_result =
-              m_P * m_error + m_I * m_totalError + m_D * (m_prevInput - input) + m_setpoint * m_F;
+          m_result = m_P * m_error + m_I * m_totalError +
+                     m_D * (m_error - m_prevError) + calculateFeedForward();
         }
-        m_prevInput = input;
+        m_prevError = m_error;
 
         if (m_result > m_maximumOutput) {
           m_result = m_maximumOutput;
@@ -318,6 +323,33 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
     }
   }
 
+  /**
+   * Calculate the feed forward term
+   *
+   * Both of the provided feed forward calculations are velocity feed forwards.
+   * If a different feed forward calculation is desired, the user can override
+   * this function and provide his or her own. This function  does no
+   * synchronization because the PIDController class only calls it in
+   * synchronized code, so be careful if calling it oneself.
+   *
+   * If a velocity PID controller is being used, the F term should be set to 1
+   * over the maximum setpoint for the output. If a position PID controller is
+   * being used, the F term should be set to 1 over the maximum speed for the
+   * output measured in setpoint units per this controller's update period (see
+   * the default period in this class's constructor).
+   */
+  protected double calculateFeedForward() {
+    if (m_pidInput.getPIDSourceType().equals(PIDSourceType.kRate)) {
+      return m_F * getSetpoint();
+    }
+    else {
+      double temp = m_F * getDeltaSetpoint();
+      m_prevSetpoint = m_setpoint;
+      m_setpointTimer.reset();
+      return temp;
+    }
+  }
+
   /**
    * Set the PID Controller gain parameters. Set the proportional, integral, and
    * differential coefficients.
@@ -491,6 +523,15 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
     return m_setpoint;
   }
 
+  /**
+   * Returns the change in setpoint over time of the PIDController
+   *$
+   * @return the change in setpoint over time
+   */
+  public synchronized double getDeltaSetpoint() {
+    return (m_setpoint - m_prevSetpoint) / m_setpointTimer.get();
+  }
+
   /**
    * Returns the current difference of the input from the setpoint
    *$
@@ -658,7 +699,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
    */
   public synchronized void reset() {
     disable();
-    m_prevInput = 0;
+    m_prevError = 0;
     m_totalError = 0;
     m_result = 0;
   }