diff --git a/wpilibc/simulation/src/PIDController.cpp b/wpilibc/simulation/src/PIDController.cpp
index 03beb5c..d3c18b5 100644
--- a/wpilibc/simulation/src/PIDController.cpp
+++ b/wpilibc/simulation/src/PIDController.cpp
@@ -1,7 +1,8 @@
 /*----------------------------------------------------------------------------*/
-/* 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.                                                               */
 /*----------------------------------------------------------------------------*/
 
 #include "PIDController.h"
@@ -74,7 +75,7 @@
 	m_enabled = false;
 	m_setpoint = 0;
 
-	m_prevInput = 0;
+	m_prevError = 0;
 	m_totalError = 0;
 	m_tolerance = .05;
 
@@ -84,7 +85,7 @@
 	m_pidOutput = output;
 	m_period = period;
 
-	m_controlLoop = std::make_unique<Notifier>(PIDController::CallCalculate, this);
+	m_controlLoop = std::make_unique<Notifier>(&PIDController::Calculate, this);
 	m_controlLoop->StartPeriodic(m_period);
 
 	static int32_t instances = 0;
@@ -98,30 +99,16 @@
 }
 
 /**
- * Call the Calculate method as a non-static method. This avoids having to prepend
- * all local variables in that method with the class pointer. This way the "this"
- * pointer will be set up and class variables can be called more easily.
- * This method is static and called by the Notifier class.
- * @param controller the address of the PID controller object to use in the background loop
+ * Read the input, calculate the output accordingly, and write to the output.
+ * This should only be called by the Notifier.
  */
-void PIDController::CallCalculate(void *controller)
-{
-	PIDController *control = (PIDController*) controller;
-	control->Calculate();
-}
-
- /**
-  * Read the input, calculate the output accordingly, and write to the output.
-  * This should only be called by the Notifier indirectly through CallCalculate
-  * and is created during initialization.
-  */
 void PIDController::Calculate()
 {
 	bool enabled;
 	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;
@@ -135,7 +122,7 @@
 		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)
 			{
@@ -168,7 +155,8 @@
                 }
               }
 
-              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) {
@@ -186,9 +174,10 @@
                 }
               }
 
-              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;
@@ -202,6 +191,33 @@
 }
 
 /**
+ * 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.
  * @param p Proportional coefficient
@@ -211,7 +227,7 @@
 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;
@@ -235,7 +251,7 @@
 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;
@@ -256,7 +272,7 @@
  */
 double PIDController::GetP() const
 {
-	std::lock_guard<priority_mutex> lock(m_mutex);
+	std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 	return m_P;
 }
 
@@ -266,7 +282,7 @@
  */
 double PIDController::GetI() const
 {
-	std::lock_guard<priority_mutex> lock(m_mutex);
+	std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 	return m_I;
 }
 
@@ -276,7 +292,7 @@
  */
 double PIDController::GetD() const
 {
-	std::lock_guard<priority_mutex> lock(m_mutex);
+	std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 	return m_D;
 }
 
@@ -286,7 +302,7 @@
  */
 double PIDController::GetF() const
 {
-	std::lock_guard<priority_mutex> lock(m_mutex);
+	std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 	return m_F;
 }
 
@@ -297,7 +313,7 @@
  */
 float PIDController::Get() const
 {
-	std::lock_guard<priority_mutex> lock(m_mutex);
+	std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 	return m_result;
 }
 
@@ -310,7 +326,7 @@
  */
 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;
 }
 
@@ -323,7 +339,7 @@
 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;
 	}
@@ -339,7 +355,7 @@
  */
 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;
 }
@@ -351,7 +367,8 @@
 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)
@@ -378,11 +395,21 @@
  */
 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
  */
@@ -390,7 +417,7 @@
 {
 	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;
@@ -420,7 +447,7 @@
 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();
   }
@@ -434,7 +461,7 @@
  */
 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;
 }
@@ -446,7 +473,7 @@
  */
 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;
 }
@@ -458,7 +485,7 @@
  */
 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;
 }
@@ -473,6 +500,7 @@
  * @param bufLength Number of previous cycles to average. Defaults to 1.
  */
 void PIDController::SetToleranceBuffer(unsigned bufLength) {
+  std::lock_guard<priority_recursive_mutex> lock(m_mutex);
   m_bufLength = bufLength;
 
   // Cut the buffer down to size if needed.
@@ -491,9 +519,9 @@
  */
 bool PIDController::OnTarget() const
 {
+	std::lock_guard<priority_recursive_mutex> sync(m_mutex);
+	if (m_buf.size() == 0) return false;
 	double error = GetError();
-
-	std::lock_guard<priority_mutex> sync(m_mutex);
 	switch (m_toleranceType) {
 	case kPercentTolerance:
 		return fabs(error) < m_tolerance / 100 * (m_maximumInput - m_minimumInput);
@@ -513,7 +541,7 @@
 void PIDController::Enable()
 {
 	{
-		std::lock_guard<priority_mutex> lock(m_mutex);
+		std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 		m_enabled = true;
 	}
 
@@ -528,7 +556,7 @@
 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;
 	}
@@ -543,7 +571,7 @@
  */
 bool PIDController::IsEnabled() const
 {
-	std::lock_guard<priority_mutex> lock(m_mutex);
+	std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 	return m_enabled;
 }
 
@@ -554,8 +582,8 @@
 {
 	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;
 }