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

@@ -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);