diff --git a/wpilibc/src/main/native/cpp/PIDController.cpp b/wpilibc/src/main/native/cpp/PIDController.cpp
index db30d5a6ec..9f06d2f413 100644
--- a/wpilibc/src/main/native/cpp/PIDController.cpp
+++ b/wpilibc/src/main/native/cpp/PIDController.cpp
@@ -7,6 +7,7 @@
 
 #include "PIDController.h"
 
+#include <algorithm>
 #include <cmath>
 #include <vector>
 
@@ -25,6 +26,11 @@ static const std::string kF = "f";
 static const std::string kSetpoint = "setpoint";
 static const std::string kEnabled = "enabled";
 
+template <class T>
+constexpr const T& clamp(const T& value, const T& low, const T& high) {
+  return std::max(low, std::min(value, high));
+}
+
 /**
  * Allocate a PID object with the given constants for P, I, D.
  *
@@ -112,29 +118,15 @@ void PIDController::Calculate() {
 
     if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) {
       if (m_P != 0) {
-        double potentialPGain = (m_totalError + m_error) * m_P;
-        if (potentialPGain < m_maximumOutput) {
-          if (potentialPGain > m_minimumOutput)
-            m_totalError += m_error;
-          else
-            m_totalError = m_minimumOutput / m_P;
-        } else {
-          m_totalError = m_maximumOutput / m_P;
-        }
+        m_totalError = clamp(m_totalError + m_error, m_minimumOutput / m_P,
+                             m_maximumOutput / m_P);
       }
 
       m_result = m_D * m_error + m_P * m_totalError + feedForward;
     } else {
       if (m_I != 0) {
-        double potentialIGain = (m_totalError + m_error) * m_I;
-        if (potentialIGain < m_maximumOutput) {
-          if (potentialIGain > m_minimumOutput)
-            m_totalError += m_error;
-          else
-            m_totalError = m_minimumOutput / m_I;
-        } else {
-          m_totalError = m_maximumOutput / m_I;
-        }
+        m_totalError = clamp(m_totalError + m_error, m_minimumOutput / m_I,
+                             m_maximumOutput / m_I);
       }
 
       m_result = m_P * m_error + m_I * m_totalError +
@@ -142,10 +134,7 @@ void PIDController::Calculate() {
     }
     m_prevError = m_error;
 
-    if (m_result > m_maximumOutput)
-      m_result = m_maximumOutput;
-    else if (m_result < m_minimumOutput)
-      m_result = m_minimumOutput;
+    m_result = clamp(m_result, m_minimumOutput, m_maximumOutput);
 
     pidOutput = m_pidOutput;
     result = m_result;
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 4806bf01e3..7496f2be31 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java
@@ -261,32 +261,16 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
 
         if (m_pidInput.getPIDSourceType().equals(PIDSourceType.kRate)) {
           if (m_P != 0) {
-            double potentialPGain = (m_totalError + m_error) * m_P;
-            if (potentialPGain < m_maximumOutput) {
-              if (potentialPGain > m_minimumOutput) {
-                m_totalError += m_error;
-              } else {
-                m_totalError = m_minimumOutput / m_P;
-              }
-            } else {
-              m_totalError = m_maximumOutput / m_P;
-            }
+            m_totalError = clamp(m_totalError + m_error, m_minimumOutput / m_P,
+                m_maximumOutput / m_P);
           }
 
           m_result = m_P * m_totalError + m_D * m_error
               + calculateFeedForward();
         } else {
           if (m_I != 0) {
-            double potentialIGain = (m_totalError + m_error) * m_I;
-            if (potentialIGain < m_maximumOutput) {
-              if (potentialIGain > m_minimumOutput) {
-                m_totalError += m_error;
-              } else {
-                m_totalError = m_minimumOutput / m_I;
-              }
-            } else {
-              m_totalError = m_maximumOutput / m_I;
-            }
+            m_totalError = clamp(m_totalError + m_error, m_minimumOutput / m_I,
+                m_maximumOutput / m_I);
           }
 
           m_result = m_P * m_error + m_I * m_totalError
@@ -294,11 +278,8 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
         }
         m_prevError = m_error;
 
-        if (m_result > m_maximumOutput) {
-          m_result = m_maximumOutput;
-        } else if (m_result < m_minimumOutput) {
-          m_result = m_minimumOutput;
-        }
+        m_result = clamp(m_result, m_minimumOutput, m_maximumOutput);
+
         pidOutput = m_pidOutput;
         result = m_result;
 
@@ -838,4 +819,8 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
   @Override
   public void stopLiveWindowMode() {
   }
+
+  private static double clamp(double value, double low, double high) {
+    return Math.max(low, Math.min(value, high));
+  }
 }