Backcalculate per Astrom 1989 and http://brettbeauregard.com/blog/2011/04/improving-the-beginner%e2%80%99s-pid-reset-windup/#comment-18721

Author: David Forrest

PID_v1.cpp

@@ -82,9 +82,15 @@ bool PID::Compute()
       /*Compute Rest of PID Output*/
       output += outputSum - kd * dInput;
 
-	    if(output > outMax) output = outMax;
-      else if(output < outMin) output = outMin;
-	    *myOutput = output;
+      if(output > outMax){
+          outputSum -= output - outMax; // backcalculate integral to feasability
+          output = outMax;
+      }
+      else if(output < outMin) {
+          outputSum += outMin - output; // backcalculate integral to feasability
+          output = outMin;
+      }
+      *myOutput = output;
 
       /*Remember some variables for next time*/
       lastInput = input;