From 0431cf97ff77afaa3c011b4a54589c0df7d9d9a5 Mon Sep 17 00:00:00 2001
From: Caleb Smith <karagenit@outlook.com>
Date: Wed, 22 Nov 2017 21:56:35 -0500
Subject: [PATCH] Cleanup PIDController (#597)

---
 wpilibc/src/main/native/cpp/PIDController.cpp         | 11 +++++------
 wpilibc/src/main/native/include/PIDController.h       |  3 +++
 .../java/edu/wpi/first/wpilibj/PIDController.java     | 10 ++++++----
 3 files changed, 14 insertions(+), 10 deletions(-)

diff --git a/wpilibc/src/main/native/cpp/PIDController.cpp b/wpilibc/src/main/native/cpp/PIDController.cpp
index 0ce0058dd6..9642a3cbe7 100644
--- a/wpilibc/src/main/native/cpp/PIDController.cpp
+++ b/wpilibc/src/main/native/cpp/PIDController.cpp
@@ -332,6 +332,7 @@ void PIDController::SetInputRange(double minimumInput, double maximumInput) {
     std::lock_guard<wpi::mutex> lock(m_mutex);
     m_minimumInput = minimumInput;
     m_maximumInput = maximumInput;
+    m_inputRange = maximumInput - minimumInput;
   }
 
   SetSetpoint(m_setpoint);
@@ -502,8 +503,7 @@ bool PIDController::OnTarget() const {
   std::lock_guard<wpi::mutex> lock(m_mutex);
   switch (m_toleranceType) {
     case kPercentTolerance:
-      return std::fabs(error) <
-             m_tolerance / 100 * (m_maximumInput - m_minimumInput);
+      return std::fabs(error) < m_tolerance / 100 * m_inputRange;
       break;
     case kAbsoluteTolerance:
       return std::fabs(error) < m_tolerance;
@@ -640,12 +640,11 @@ void PIDController::InitTable(std::shared_ptr<nt::NetworkTable> subtable) {
  * @return Error for continuous inputs.
  */
 double PIDController::GetContinuousError(double error) const {
-  if (m_continuous &&
-      std::fabs(error) > (m_maximumInput - m_minimumInput) / 2) {
+  if (m_continuous && std::fabs(error) > m_inputRange / 2) {
     if (error > 0) {
-      return error - (m_maximumInput - m_minimumInput);
+      return error - m_inputRange;
     } else {
-      return error + (m_maximumInput - m_minimumInput);
+      return error + m_inputRange;
     }
   }
 
diff --git a/wpilibc/src/main/native/include/PIDController.h b/wpilibc/src/main/native/include/PIDController.h
index 7fda3f5e54..60d8ae10a4 100644
--- a/wpilibc/src/main/native/include/PIDController.h
+++ b/wpilibc/src/main/native/include/PIDController.h
@@ -135,6 +135,9 @@ class PIDController : public LiveWindowSendable, public PIDInterface {
   // Minimum input - limit setpoint to this
   double m_minimumInput = 0;
 
+  // input range - difference between maximum and minimum
+  double m_inputRange = 0;
+
   // Do the endpoints wrap around? eg. Absolute encoder
   bool m_continuous = false;
 
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 a2665ef3cd..96a7b52c85 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java
@@ -44,6 +44,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
   private double m_minimumOutput = -1.0; // |minimum output|
   private double m_maximumInput = 0.0; // maximum input - limit setpoint to this
   private double m_minimumInput = 0.0; // minimum input - limit setpoint to this
+  private double m_inputRange = 0.0; // input range - difference between maximum and minimum
   // do the endpoints wrap around? eg. Absolute encoder
   private boolean m_continuous = false;
   private boolean m_enabled = false; // is the pid controller enabled
@@ -96,7 +97,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
 
     @Override
     public boolean onTarget() {
-      return Math.abs(getError()) < m_percentage / 100 * (m_maximumInput - m_minimumInput);
+      return Math.abs(getError()) < m_percentage / 100 * m_inputRange;
     }
   }
 
@@ -448,6 +449,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
     }
     m_minimumInput = minimumInput;
     m_maximumInput = maximumInput;
+    m_inputRange = maximumInput - minimumInput;
     setSetpoint(m_setpoint);
   }
 
@@ -769,11 +771,11 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
    * @return Error for continuous inputs.
    */
   protected double getContinuousError(double error) {
-    if (m_continuous && Math.abs(error) > (m_maximumInput - m_minimumInput) / 2) {
+    if (m_continuous && Math.abs(error) > m_inputRange / 2) {
       if (error > 0) {
-        return error - (m_maximumInput - m_minimumInput);
+        return error - m_inputRange;
       } else {
-        return error + (m_maximumInput - m_minimumInput);
+        return error + m_inputRange;
       }
     }