From de63e1c8a1131b0d23bdc78c3a09896e4037bbbe Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Fri, 24 Nov 2017 00:55:35 -0800
Subject: [PATCH] Fixed race condition between PIDController enable/disable and
 PIDWrite() call

To make this work in PIDController.java, the use of synchronized had to be
replaced with ReentrantLock and try-catch blocks. The locking in
PIDController.java was made equivalent to PIDController.cpp and some existing
race conditions in PIDController.java were fixed in the process.

Fixes #30.
---
 wpilibc/src/main/native/cpp/PIDController.cpp |  79 +++--
 .../src/main/native/include/PIDController.h   |   6 +-
 .../edu/wpi/first/wpilibj/PIDController.java  | 325 ++++++++++++++----
 3 files changed, 301 insertions(+), 109 deletions(-)

diff --git a/wpilibc/src/main/native/cpp/PIDController.cpp b/wpilibc/src/main/native/cpp/PIDController.cpp
index fb77f8472c..0bbce79c63 100644
--- a/wpilibc/src/main/native/cpp/PIDController.cpp
+++ b/wpilibc/src/main/native/cpp/PIDController.cpp
@@ -135,7 +135,7 @@ void PIDController::Calculate() {
 
   bool enabled;
   {
-    std::lock_guard<wpi::mutex> lock(m_mutex);
+    std::lock_guard<wpi::mutex> lock(m_thisMutex);
     enabled = m_enabled;
   }
 
@@ -157,7 +157,7 @@ void PIDController::Calculate() {
     double totalError;
 
     {
-      std::lock_guard<wpi::mutex> lock(m_mutex);
+      std::lock_guard<wpi::mutex> lock(m_thisMutex);
 
       input = m_pidInput->PIDGet();
 
@@ -195,9 +195,19 @@ void PIDController::Calculate() {
 
     result = clamp(result, minimumOutput, maximumOutput);
 
-    m_pidOutput->PIDWrite(result);
+    {
+      // Ensures m_enabled check and PIDWrite() call occur atomically
+      std::lock_guard<wpi::mutex> pidWriteLock(m_pidWriteMutex);
+      std::unique_lock<wpi::mutex> mainLock(m_thisMutex);
+      if (m_enabled) {
+        // Don't block other PIDController operations on PIDWrite()
+        mainLock.unlock();
 
-    std::lock_guard<wpi::mutex> lock(m_mutex);
+        m_pidOutput->PIDWrite(result);
+      }
+    }
+
+    std::lock_guard<wpi::mutex> lock(m_thisMutex);
     m_prevError = m_error;
     m_error = error;
     m_totalError = totalError;
@@ -242,7 +252,7 @@ double PIDController::CalculateFeedForward() {
  */
 void PIDController::SetPID(double p, double i, double d) {
   {
-    std::lock_guard<wpi::mutex> lock(m_mutex);
+    std::lock_guard<wpi::mutex> lock(m_thisMutex);
     m_P = p;
     m_I = i;
     m_D = d;
@@ -265,7 +275,7 @@ void PIDController::SetPID(double p, double i, double d) {
  */
 void PIDController::SetPID(double p, double i, double d, double f) {
   {
-    std::lock_guard<wpi::mutex> lock(m_mutex);
+    std::lock_guard<wpi::mutex> lock(m_thisMutex);
     m_P = p;
     m_I = i;
     m_D = d;
@@ -284,7 +294,7 @@ void PIDController::SetPID(double p, double i, double d, double f) {
  * @return proportional coefficient
  */
 double PIDController::GetP() const {
-  std::lock_guard<wpi::mutex> lock(m_mutex);
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
   return m_P;
 }
 
@@ -294,7 +304,7 @@ double PIDController::GetP() const {
  * @return integral coefficient
  */
 double PIDController::GetI() const {
-  std::lock_guard<wpi::mutex> lock(m_mutex);
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
   return m_I;
 }
 
@@ -304,7 +314,7 @@ double PIDController::GetI() const {
  * @return differential coefficient
  */
 double PIDController::GetD() const {
-  std::lock_guard<wpi::mutex> lock(m_mutex);
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
   return m_D;
 }
 
@@ -314,7 +324,7 @@ double PIDController::GetD() const {
  * @return Feed forward coefficient
  */
 double PIDController::GetF() const {
-  std::lock_guard<wpi::mutex> lock(m_mutex);
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
   return m_F;
 }
 
@@ -326,7 +336,7 @@ double PIDController::GetF() const {
  * @return the latest calculated output
  */
 double PIDController::Get() const {
-  std::lock_guard<wpi::mutex> lock(m_mutex);
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
   return m_result;
 }
 
@@ -340,7 +350,7 @@ double PIDController::Get() const {
  * @param continuous true turns on continuous, false turns off continuous
  */
 void PIDController::SetContinuous(bool continuous) {
-  std::lock_guard<wpi::mutex> lock(m_mutex);
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
   m_continuous = continuous;
 }
 
@@ -352,7 +362,7 @@ void PIDController::SetContinuous(bool continuous) {
  */
 void PIDController::SetInputRange(double minimumInput, double maximumInput) {
   {
-    std::lock_guard<wpi::mutex> lock(m_mutex);
+    std::lock_guard<wpi::mutex> lock(m_thisMutex);
     m_minimumInput = minimumInput;
     m_maximumInput = maximumInput;
     m_inputRange = maximumInput - minimumInput;
@@ -368,7 +378,7 @@ void PIDController::SetInputRange(double minimumInput, double maximumInput) {
  * @param maximumOutput the maximum value to write to the output
  */
 void PIDController::SetOutputRange(double minimumOutput, double maximumOutput) {
-  std::lock_guard<wpi::mutex> lock(m_mutex);
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
   m_minimumOutput = minimumOutput;
   m_maximumOutput = maximumOutput;
 }
@@ -380,7 +390,7 @@ void PIDController::SetOutputRange(double minimumOutput, double maximumOutput) {
  */
 void PIDController::SetSetpoint(double setpoint) {
   {
-    std::lock_guard<wpi::mutex> lock(m_mutex);
+    std::lock_guard<wpi::mutex> lock(m_thisMutex);
 
     if (m_maximumInput > m_minimumInput) {
       if (setpoint > m_maximumInput)
@@ -403,7 +413,7 @@ void PIDController::SetSetpoint(double setpoint) {
  * @return the current setpoint
  */
 double PIDController::GetSetpoint() const {
-  std::lock_guard<wpi::mutex> lock(m_mutex);
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
   return m_setpoint;
 }
 
@@ -413,7 +423,7 @@ double PIDController::GetSetpoint() const {
  * @return the change in setpoint over time
  */
 double PIDController::GetDeltaSetpoint() const {
-  std::lock_guard<wpi::mutex> lock(m_mutex);
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
   return (m_setpoint - m_prevSetpoint) / m_setpointTimer.Get();
 }
 
@@ -425,7 +435,7 @@ double PIDController::GetDeltaSetpoint() const {
 double PIDController::GetError() const {
   double setpoint = GetSetpoint();
   {
-    std::lock_guard<wpi::mutex> lock(m_mutex);
+    std::lock_guard<wpi::mutex> lock(m_thisMutex);
     return GetContinuousError(setpoint - m_pidInput->PIDGet());
   }
 }
@@ -462,7 +472,7 @@ PIDSourceType PIDController::GetPIDSourceType() const {
  * @param percentage error which is tolerable
  */
 void PIDController::SetTolerance(double percent) {
-  std::lock_guard<wpi::mutex> lock(m_mutex);
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
   m_toleranceType = kPercentTolerance;
   m_tolerance = percent;
 }
@@ -474,7 +484,7 @@ void PIDController::SetTolerance(double percent) {
  * @param percentage error which is tolerable
  */
 void PIDController::SetAbsoluteTolerance(double absTolerance) {
-  std::lock_guard<wpi::mutex> lock(m_mutex);
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
   m_toleranceType = kAbsoluteTolerance;
   m_tolerance = absTolerance;
 }
@@ -486,7 +496,7 @@ void PIDController::SetAbsoluteTolerance(double absTolerance) {
  * @param percentage error which is tolerable
  */
 void PIDController::SetPercentTolerance(double percent) {
-  std::lock_guard<wpi::mutex> lock(m_mutex);
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
   m_toleranceType = kPercentTolerance;
   m_tolerance = percent;
 }
@@ -502,7 +512,7 @@ void PIDController::SetPercentTolerance(double percent) {
  * @param bufLength Number of previous cycles to average. Defaults to 1.
  */
 void PIDController::SetToleranceBuffer(int bufLength) {
-  std::lock_guard<wpi::mutex> lock(m_mutex);
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
 
   // Create LinearDigitalFilter with original source as its source argument
   m_filter = LinearDigitalFilter::MovingAverage(m_origSource, bufLength);
@@ -523,7 +533,7 @@ void PIDController::SetToleranceBuffer(int bufLength) {
 bool PIDController::OnTarget() const {
   double error = GetError();
 
-  std::lock_guard<wpi::mutex> lock(m_mutex);
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
   switch (m_toleranceType) {
     case kPercentTolerance:
       return std::fabs(error) < m_tolerance / 100 * m_inputRange;
@@ -543,7 +553,7 @@ bool PIDController::OnTarget() const {
  */
 void PIDController::Enable() {
   {
-    std::lock_guard<wpi::mutex> lock(m_mutex);
+    std::lock_guard<wpi::mutex> lock(m_thisMutex);
     m_enabled = true;
   }
 
@@ -555,9 +565,14 @@ void PIDController::Enable() {
  */
 void PIDController::Disable() {
   {
-    std::lock_guard<wpi::mutex> lock(m_mutex);
+    // Ensures m_enabled modification and PIDWrite() call occur atomically
+    std::lock_guard<wpi::mutex> pidWriteLock(m_pidWriteMutex);
+    {
+      std::lock_guard<wpi::mutex> mainLock(m_thisMutex);
+      m_enabled = false;
+    }
+
     m_pidOutput->PIDWrite(0);
-    m_enabled = false;
   }
 
   if (m_enabledEntry) m_enabledEntry.SetBoolean(false);
@@ -567,7 +582,7 @@ void PIDController::Disable() {
  * Return true if PIDController is enabled.
  */
 bool PIDController::IsEnabled() const {
-  std::lock_guard<wpi::mutex> lock(m_mutex);
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
   return m_enabled;
 }
 
@@ -577,7 +592,7 @@ bool PIDController::IsEnabled() const {
 void PIDController::Reset() {
   Disable();
 
-  std::lock_guard<wpi::mutex> lock(m_mutex);
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
   m_prevError = 0;
   m_totalError = 0;
   m_result = 0;
@@ -606,7 +621,7 @@ void PIDController::InitTable(std::shared_ptr<nt::NetworkTable> subtable) {
     m_pListener = m_pEntry.AddListener(
         [=](const nt::EntryNotification& event) {
           if (!event.value->IsDouble()) return;
-          std::lock_guard<wpi::mutex> lock(m_mutex);
+          std::lock_guard<wpi::mutex> lock(m_thisMutex);
           m_P = event.value->GetDouble();
         },
         NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
@@ -614,7 +629,7 @@ void PIDController::InitTable(std::shared_ptr<nt::NetworkTable> subtable) {
     m_iListener = m_iEntry.AddListener(
         [=](const nt::EntryNotification& event) {
           if (!event.value->IsDouble()) return;
-          std::lock_guard<wpi::mutex> lock(m_mutex);
+          std::lock_guard<wpi::mutex> lock(m_thisMutex);
           m_I = event.value->GetDouble();
         },
         NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
@@ -622,7 +637,7 @@ void PIDController::InitTable(std::shared_ptr<nt::NetworkTable> subtable) {
     m_dListener = m_dEntry.AddListener(
         [=](const nt::EntryNotification& event) {
           if (!event.value->IsDouble()) return;
-          std::lock_guard<wpi::mutex> lock(m_mutex);
+          std::lock_guard<wpi::mutex> lock(m_thisMutex);
           m_D = event.value->GetDouble();
         },
         NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
@@ -630,7 +645,7 @@ void PIDController::InitTable(std::shared_ptr<nt::NetworkTable> subtable) {
     m_fListener = m_fEntry.AddListener(
         [=](const nt::EntryNotification& event) {
           if (!event.value->IsDouble()) return;
-          std::lock_guard<wpi::mutex> lock(m_mutex);
+          std::lock_guard<wpi::mutex> lock(m_thisMutex);
           m_F = event.value->GetDouble();
         },
         NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
diff --git a/wpilibc/src/main/native/include/PIDController.h b/wpilibc/src/main/native/include/PIDController.h
index aac2b8d354..6ed4a878fa 100644
--- a/wpilibc/src/main/native/include/PIDController.h
+++ b/wpilibc/src/main/native/include/PIDController.h
@@ -172,7 +172,11 @@ class PIDController : public LiveWindowSendable, public PIDInterface {
   std::shared_ptr<PIDSource> m_origSource;
   LinearDigitalFilter m_filter{nullptr, {}, {}};
 
-  mutable wpi::mutex m_mutex;
+  mutable wpi::mutex m_thisMutex;
+
+  // Ensures when Disable() is called, PIDWrite() won't run if Calculate()
+  // is already running at that time.
+  mutable wpi::mutex m_pidWriteMutex;
 
   std::unique_ptr<Notifier> m_controlLoop;
   Timer m_setpointTimer;
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java
index caf42a7833..f300106cc4 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java
@@ -8,6 +8,7 @@
 package edu.wpi.first.wpilibj;
 
 import java.util.TimerTask;
+import java.util.concurrent.locks.ReentrantLock;
 
 import edu.wpi.first.networktables.EntryListenerFlags;
 import edu.wpi.first.networktables.NetworkTable;
@@ -64,6 +65,12 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
   PIDSource m_origSource;
   LinearDigitalFilter m_filter;
 
+  ReentrantLock m_thisMutex = new ReentrantLock();
+
+  // Ensures when disable() is called, pidWrite() won't run if calculate()
+  // is already running at that time.
+  ReentrantLock m_pidWriteMutex = new ReentrantLock();
+
   protected PIDSource m_pidInput;
   protected PIDOutput m_pidOutput;
   java.util.Timer m_controlLoop;
@@ -227,10 +234,13 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
    */
   public void free() {
     m_controlLoop.cancel();
-    synchronized (this) {
+    m_thisMutex.lock();
+    try {
       m_pidOutput = null;
       m_pidInput = null;
       m_controlLoop = null;
+    } finally {
+      m_thisMutex.unlock();
     }
     removeListeners();
   }
@@ -247,8 +257,11 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
 
     boolean enabled;
 
-    synchronized (this) {
+    m_thisMutex.lock();
+    try {
       enabled = m_enabled;
+    } finally {
+      m_thisMutex.unlock();
     }
 
     if (enabled) {
@@ -268,7 +281,8 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
       double error;
       double totalError;
 
-      synchronized (this) {
+      m_thisMutex.lock();
+      try {
         input = m_pidInput.pidGet();
 
         pidSourceType = m_pidInput.getPIDSourceType();
@@ -281,6 +295,8 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
         prevError = m_prevError;
         error = getContinuousError(m_setpoint - input);
         totalError = m_totalError;
+      } finally {
+        m_thisMutex.unlock();
       }
 
       // Storage for function outputs
@@ -305,13 +321,34 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
 
       result = clamp(result, minimumOutput, maximumOutput);
 
-      m_pidOutput.pidWrite(result);
+      // Ensures m_enabled check and pidWrite() call occur atomically
+      m_pidWriteMutex.lock();
+      try {
+        m_thisMutex.lock();
+        try {
+          if (m_enabled) {
+            // Don't block other PIDController operations on pidWrite()
+            m_thisMutex.unlock();
 
-      synchronized (this) {
+            m_pidOutput.pidWrite(result);
+          }
+        } finally {
+          if (m_thisMutex.isHeldByCurrentThread()) {
+            m_thisMutex.unlock();
+          }
+        }
+      } finally {
+        m_pidWriteMutex.unlock();
+      }
+
+      m_thisMutex.lock();
+      try {
         m_prevError = error;
         m_error = error;
         m_totalError = totalError;
         m_result = result;
+      } finally {
+        m_thisMutex.unlock();
       }
     }
   }
@@ -349,10 +386,15 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
    * @param d Differential coefficient
    */
   @SuppressWarnings("ParameterName")
-  public synchronized void setPID(double p, double i, double d) {
-    m_P = p;
-    m_I = i;
-    m_D = d;
+  public void setPID(double p, double i, double d) {
+    m_thisMutex.lock();
+    try {
+      m_P = p;
+      m_I = i;
+      m_D = d;
+    } finally {
+      m_thisMutex.unlock();
+    }
 
     if (m_pEntry != null) {
       m_pEntry.setDouble(p);
@@ -375,11 +417,16 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
    * @param f Feed forward coefficient
    */
   @SuppressWarnings("ParameterName")
-  public synchronized void setPID(double p, double i, double d, double f) {
-    m_P = p;
-    m_I = i;
-    m_D = d;
-    m_F = f;
+  public void setPID(double p, double i, double d, double f) {
+    m_thisMutex.lock();
+    try {
+      m_P = p;
+      m_I = i;
+      m_D = d;
+      m_F = f;
+    } finally {
+      m_thisMutex.unlock();
+    }
 
     if (m_pEntry != null) {
       m_pEntry.setDouble(p);
@@ -400,8 +447,13 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
    *
    * @return proportional coefficient
    */
-  public synchronized double getP() {
-    return m_P;
+  public double getP() {
+    m_thisMutex.lock();
+    try {
+      return m_P;
+    } finally {
+      m_thisMutex.unlock();
+    }
   }
 
   /**
@@ -409,8 +461,13 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
    *
    * @return integral coefficient
    */
-  public synchronized double getI() {
-    return m_I;
+  public double getI() {
+    m_thisMutex.lock();
+    try {
+      return m_I;
+    } finally {
+      m_thisMutex.unlock();
+    }
   }
 
   /**
@@ -418,8 +475,13 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
    *
    * @return differential coefficient
    */
-  public synchronized double getD() {
-    return m_D;
+  public double getD() {
+    m_thisMutex.lock();
+    try {
+      return m_D;
+    } finally {
+      m_thisMutex.unlock();
+    }
   }
 
   /**
@@ -427,8 +489,13 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
    *
    * @return feed forward coefficient
    */
-  public synchronized double getF() {
-    return m_F;
+  public double getF() {
+    m_thisMutex.lock();
+    try {
+      return m_F;
+    } finally {
+      m_thisMutex.unlock();
+    }
   }
 
   /**
@@ -437,8 +504,13 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
    *
    * @return the latest calculated output
    */
-  public synchronized double get() {
-    return m_result;
+  public double get() {
+    m_thisMutex.lock();
+    try {
+      return m_result;
+    } finally {
+      m_thisMutex.unlock();
+    }
   }
 
   /**
@@ -448,8 +520,13 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
    *
    * @param continuous Set to true turns on continuous, false turns off continuous
    */
-  public synchronized void setContinuous(boolean continuous) {
-    m_continuous = continuous;
+  public void setContinuous(boolean continuous) {
+    m_thisMutex.lock();
+    try {
+      m_continuous = continuous;
+    } finally {
+      m_thisMutex.unlock();
+    }
   }
 
   /**
@@ -457,7 +534,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
    * min in as constraints, it considers them to be the same point and automatically calculates the
    * shortest route to the setpoint.
    */
-  public synchronized void setContinuous() {
+  public void setContinuous() {
     setContinuous(true);
   }
 
@@ -467,13 +544,19 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
    * @param minimumInput the minimum value expected from the input
    * @param maximumInput the maximum value expected from the input
    */
-  public synchronized void setInputRange(double minimumInput, double maximumInput) {
-    if (minimumInput > maximumInput) {
-      throw new BoundaryException("Lower bound is greater than upper bound");
+  public void setInputRange(double minimumInput, double maximumInput) {
+    m_thisMutex.lock();
+    try {
+      if (minimumInput > maximumInput) {
+        throw new BoundaryException("Lower bound is greater than upper bound");
+      }
+      m_minimumInput = minimumInput;
+      m_maximumInput = maximumInput;
+      m_inputRange = maximumInput - minimumInput;
+    } finally {
+      m_thisMutex.unlock();
     }
-    m_minimumInput = minimumInput;
-    m_maximumInput = maximumInput;
-    m_inputRange = maximumInput - minimumInput;
+
     setSetpoint(m_setpoint);
   }
 
@@ -483,12 +566,17 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
    * @param minimumOutput the minimum percentage to write to the output
    * @param maximumOutput the maximum percentage to write to the output
    */
-  public synchronized void setOutputRange(double minimumOutput, double maximumOutput) {
-    if (minimumOutput > maximumOutput) {
-      throw new BoundaryException("Lower bound is greater than upper bound");
+  public void setOutputRange(double minimumOutput, double maximumOutput) {
+    m_thisMutex.lock();
+    try {
+      if (minimumOutput > maximumOutput) {
+        throw new BoundaryException("Lower bound is greater than upper bound");
+      }
+      m_minimumOutput = minimumOutput;
+      m_maximumOutput = maximumOutput;
+    } finally {
+      m_thisMutex.unlock();
     }
-    m_minimumOutput = minimumOutput;
-    m_maximumOutput = maximumOutput;
   }
 
   /**
@@ -496,17 +584,22 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
    *
    * @param setpoint the desired setpoint
    */
-  public synchronized void setSetpoint(double setpoint) {
-    if (m_maximumInput > m_minimumInput) {
-      if (setpoint > m_maximumInput) {
-        m_setpoint = m_maximumInput;
-      } else if (setpoint < m_minimumInput) {
-        m_setpoint = m_minimumInput;
+  public void setSetpoint(double setpoint) {
+    m_thisMutex.lock();
+    try {
+      if (m_maximumInput > m_minimumInput) {
+        if (setpoint > m_maximumInput) {
+          m_setpoint = m_maximumInput;
+        } else if (setpoint < m_minimumInput) {
+          m_setpoint = m_minimumInput;
+        } else {
+          m_setpoint = setpoint;
+        }
       } else {
         m_setpoint = setpoint;
       }
-    } else {
-      m_setpoint = setpoint;
+    } finally {
+      m_thisMutex.unlock();
     }
 
     if (m_setpointEntry != null) {
@@ -519,8 +612,13 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
    *
    * @return the current setpoint
    */
-  public synchronized double getSetpoint() {
-    return m_setpoint;
+  public double getSetpoint() {
+    m_thisMutex.lock();
+    try {
+      return m_setpoint;
+    } finally {
+      m_thisMutex.unlock();
+    }
   }
 
   /**
@@ -528,8 +626,13 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
    *
    * @return the change in setpoint over time
    */
-  public synchronized double getDeltaSetpoint() {
-    return (m_setpoint - m_prevSetpoint) / m_setpointTimer.get();
+  public double getDeltaSetpoint() {
+    m_thisMutex.lock();
+    try {
+      return (m_setpoint - m_prevSetpoint) / m_setpointTimer.get();
+    } finally {
+      m_thisMutex.unlock();
+    }
   }
 
   /**
@@ -537,8 +640,13 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
    *
    * @return the current error
    */
-  public synchronized double getError() {
-    return getContinuousError(getSetpoint() - m_pidInput.pidGet());
+  public double getError() {
+    m_thisMutex.lock();
+    try {
+      return getContinuousError(getSetpoint() - m_pidInput.pidGet());
+    } finally {
+      m_thisMutex.unlock();
+    }
   }
 
   /**
@@ -550,8 +658,13 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
    * @return     the current average of the error
    */
   @Deprecated
-  public synchronized double getAvgError() {
-    return getError();
+  public double getAvgError() {
+    m_thisMutex.lock();
+    try {
+      return getError();
+    } finally {
+      m_thisMutex.unlock();
+    }
   }
 
   /**
@@ -592,8 +705,13 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
    *
    * @param absvalue absolute error which is tolerable in the units of the input object
    */
-  public synchronized void setAbsoluteTolerance(double absvalue) {
-    m_tolerance = new AbsoluteTolerance(absvalue);
+  public void setAbsoluteTolerance(double absvalue) {
+    m_thisMutex.lock();
+    try {
+      m_tolerance = new AbsoluteTolerance(absvalue);
+    } finally {
+      m_thisMutex.unlock();
+    }
   }
 
   /**
@@ -602,8 +720,13 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
    *
    * @param percentage percent error which is tolerable
    */
-  public synchronized void setPercentTolerance(double percentage) {
-    m_tolerance = new PercentageTolerance(percentage);
+  public void setPercentTolerance(double percentage) {
+    m_thisMutex.lock();
+    try {
+      m_tolerance = new PercentageTolerance(percentage);
+    } finally {
+      m_thisMutex.unlock();
+    }
   }
 
   /**
@@ -617,9 +740,14 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
    * @param bufLength Number of previous cycles to average.
    */
   @Deprecated
-  public synchronized void setToleranceBuffer(int bufLength) {
-    m_filter = LinearDigitalFilter.movingAverage(m_origSource, bufLength);
-    m_pidInput = m_filter;
+  public void setToleranceBuffer(int bufLength) {
+    m_thisMutex.lock();
+    try {
+      m_filter = LinearDigitalFilter.movingAverage(m_origSource, bufLength);
+      m_pidInput = m_filter;
+    } finally {
+      m_thisMutex.unlock();
+    }
   }
 
   /**
@@ -628,16 +756,26 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
    *
    * @return true if the error is less than the tolerance
    */
-  public synchronized boolean onTarget() {
-    return m_tolerance.onTarget();
+  public boolean onTarget() {
+    m_thisMutex.lock();
+    try {
+      return m_tolerance.onTarget();
+    } finally {
+      m_thisMutex.unlock();
+    }
   }
 
   /**
    * Begin running the PIDController.
    */
   @Override
-  public synchronized void enable() {
-    m_enabled = true;
+  public void enable() {
+    m_thisMutex.lock();
+    try {
+      m_enabled = true;
+    } finally {
+      m_thisMutex.unlock();
+    }
 
     if (m_enabledEntry != null) {
       m_enabledEntry.setBoolean(true);
@@ -648,9 +786,21 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
    * Stop running the PIDController, this sets the output to zero before stopping.
    */
   @Override
-  public synchronized void disable() {
-    m_pidOutput.pidWrite(0);
-    m_enabled = false;
+  public void disable() {
+    // Ensures m_enabled check and pidWrite() call occur atomically
+    m_pidWriteMutex.lock();
+    try {
+      m_thisMutex.lock();
+      try {
+        m_enabled = false;
+      } finally {
+        m_thisMutex.unlock();
+      }
+
+      m_pidOutput.pidWrite(0);
+    } finally {
+      m_pidWriteMutex.unlock();
+    }
 
     if (m_enabledEntry != null) {
       m_enabledEntry.setBoolean(false);
@@ -662,7 +812,12 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
    */
   @Override
   public boolean isEnabled() {
-    return m_enabled;
+    m_thisMutex.lock();
+    try {
+      return m_enabled;
+    } finally {
+      m_thisMutex.unlock();
+    }
   }
 
   /**
@@ -671,9 +826,15 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
   @Override
   public synchronized void reset() {
     disable();
-    m_prevError = 0;
-    m_totalError = 0;
-    m_result = 0;
+
+    m_thisMutex.lock();
+    try {
+      m_prevError = 0;
+      m_totalError = 0;
+      m_result = 0;
+    } finally {
+      m_thisMutex.unlock();
+    }
   }
 
   @Override
@@ -741,26 +902,38 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
       m_enabledEntry.setBoolean(isEnabled());
 
       m_pListener = m_pEntry.addListener((entry) -> {
-        synchronized (this) {
+        m_thisMutex.lock();
+        try {
           m_P = entry.value.getDouble();
+        } finally {
+          m_thisMutex.unlock();
         }
       }, EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
 
       m_iListener = m_iEntry.addListener((entry) -> {
-        synchronized (this) {
+        m_thisMutex.lock();
+        try {
           m_I = entry.value.getDouble();
+        } finally {
+          m_thisMutex.unlock();
         }
       }, EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
 
       m_dListener = m_dEntry.addListener((entry) -> {
-        synchronized (this) {
+        m_thisMutex.lock();
+        try {
           m_D = entry.value.getDouble();
+        } finally {
+          m_thisMutex.unlock();
         }
       }, EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
 
       m_fListener = m_fEntry.addListener((entry) -> {
-        synchronized (this) {
+        m_thisMutex.lock();
+        try {
           m_F = entry.value.getDouble();
+        } finally {
+          m_thisMutex.unlock();
         }
       }, EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);