Revision 1:0861bf4f8f14, committed 2015-04-05

Comitter:

Spilly

Date:

Sun Apr 05 01:42:09 2015 +0000

Parent:

0:6e12a3e5af19

Child:

2:8a8bb3164e1c

Commit message:

Fixed some mathematical issues

Changed in this revision

--- a/PID.cpp	Thu Sep 02 16:48:10 2010 +0000
+++ b/PID.cpp	Sun Apr 05 01:42:09 2015 +0000
@@ -49,7 +49,8 @@
  */
 #include "PID.h"
 
-PID::PID(float Kc, float tauI, float tauD, float interval) {
+PID::PID(float Kc, float tauI, float tauD, float interval) 
+{
 
     usingFeedForward = false;
     inAuto           = false;
@@ -77,46 +78,16 @@
 
 }
 
-void PID::setInputLimits(float inMin, float inMax) {
-
-    //Make sure we haven't been given impossible values.
-    if (inMin >= inMax) {
-        return;
-    }
-
-    //Rescale the working variables to reflect the changes.
-    prevProcessVariable_ *= (inMax - inMin) / inSpan_;
-    accError_            *= (inMax - inMin) / inSpan_;
-
-    //Make sure the working variables are within the new limits.
-    if (prevProcessVariable_ > 1) {
-        prevProcessVariable_ = 1;
-    } else if (prevProcessVariable_ < 0) {
-        prevProcessVariable_ = 0;
-    }
-
+void PID::setInputLimits(float inMin, float inMax) 
+{
     inMin_  = inMin;
     inMax_  = inMax;
     inSpan_ = inMax - inMin;
 
 }
 
-void PID::setOutputLimits(float outMin, float outMax) {
-
-    //Make sure we haven't been given impossible values.
-    if (outMin >= outMax) {
-        return;
-    }
-
-    //Rescale the working variables to reflect the changes.
-    prevControllerOutput_ *= (outMax - outMin) / outSpan_;
-
-    //Make sure the working variables are within the new limits.
-    if (prevControllerOutput_ > 1) {
-        prevControllerOutput_ = 1;
-    } else if (prevControllerOutput_ < 0) {
-        prevControllerOutput_ = 0;
-    }
+void PID::setOutputLimits(float outMin, float outMax) 
+{
 
     outMin_  = outMin;
     outMax_  = outMax;
@@ -124,13 +95,8 @@
 
 }
 
-void PID::setTunings(float Kc, float tauI, float tauD) {
-
-    //Verify that the tunings make sense.
-    if (Kc == 0.0 || tauI < 0.0 || tauD < 0.0) {
-        return;
-    }
-
+void PID::setTunings(float Kc, float tauI, float tauD) 
+{
     //Store raw values to hand back to user on request.
     pParam_ = Kc;
     iParam_ = tauI;
@@ -138,16 +104,16 @@
 
     float tempTauR;
 
-    if (tauI == 0.0) {
-        tempTauR = 0.0;
+    if (tauI == 0.0f) {
+        tempTauR = 0.0f;
     } else {
-        tempTauR = (1.0 / tauI) * tSample_;
+        tempTauR = (1.0f / tauI) * tSample_;
     }
 
     //For "bumpless transfer" we need to rescale the accumulated error.
     if (inAuto) {
-        if (tempTauR == 0.0) {
-            accError_ = 0.0;
+        if (tempTauR == 0.0f) {
+            accError_ = 0.0f;
         } else {
             accError_ *= (Kc_ * tauR_) / (Kc * tempTauR);
         }
@@ -159,21 +125,12 @@
 
 }
 
-void PID::reset(void) {
-
-    float scaledBias = 0.0;
-
-    if (usingFeedForward) {
-        scaledBias = (bias_ - outMin_) / outSpan_;
-    } else {
-        scaledBias = (realOutput_ - outMin_) / outSpan_;
-    }
-
-    prevControllerOutput_ = scaledBias;
-    prevProcessVariable_  = (processVariable_ - inMin_) / inSpan_;
+void PID::reset(void) 
+{
+    prevControllerOutput_ = 0.0f;
 
     //Clear any error in the integral.
-    accError_ = 0;
+    accError_ = 0.0f;
 
 }
 
@@ -220,59 +177,40 @@
 
 }
 
-float PID::compute() {
-
-    //Pull in the input and setpoint, and scale them into percent span.
-    float scaledPV = (processVariable_ - inMin_) / inSpan_;
-
-    if (scaledPV > 1.0) {
-        scaledPV = 1.0;
-    } else if (scaledPV < 0.0) {
-        scaledPV = 0.0;
-    }
-
-    float scaledSP = (setPoint_ - inMin_) / inSpan_;
-    if (scaledSP > 1.0) {
-        scaledSP = 1;
-    } else if (scaledSP < 0.0) {
-        scaledSP = 0;
-    }
-
-    float error = scaledSP - scaledPV;
+float PID::compute() 
+{
+    float error = setPoint_ - processVariable_;
 
     //Check and see if the output is pegged at a limit and only
     //integrate if it is not. This is to prevent reset-windup.
-    if (!(prevControllerOutput_ >= 1 && error > 0) && !(prevControllerOutput_ <= 0 && error < 0)) {
+    if (!(prevControllerOutput_ >= outMax_ && error > 0) && !(prevControllerOutput_ <= outMin_ && error < 0)) {
         accError_ += error;
     }
 
     //Compute the current slope of the input signal.
-    float dMeas = (scaledPV - prevProcessVariable_) / tSample_;
-
-    float scaledBias = 0.0;
+    float dMeas = (processVariable_ - prevProcessVariable_) / tSample_;
 
-    if (usingFeedForward) {
-        scaledBias = (bias_ - outMin_) / outSpan_;
-    }
+    float scaledBias = 0.0f;
 
     //Perform the PID calculation.
     controllerOutput_ = scaledBias + Kc_ * (error + (tauR_ * accError_) - (tauD_ * dMeas));
-
+    
+    controllerOutput_ = ((outMax_ - outMin_) * ((Kc_ * controllerOutput_) - inMin_)) / (inMax_ - inMin_) + outMin_;
+    
     //Make sure the computed output is within output constraints.
-    if (controllerOutput_ < 0.0) {
-        controllerOutput_ = 0.0;
-    } else if (controllerOutput_ > 1.0) {
-        controllerOutput_ = 1.0;
+    if (controllerOutput_ < outMin_) {
+        controllerOutput_ = outMin_;
+    } else if (controllerOutput_ > outMax_) {
+        controllerOutput_ = outMax_;
     }
-
+    
     //Remember this output for the windup check next time.
     prevControllerOutput_ = controllerOutput_;
     //Remember the input for the derivative calculation next time.
-    prevProcessVariable_  = scaledPV;
-
+    prevProcessVariable_  = processVariable_;
+    
     //Scale the output from percent span back out to a real world number.
-    return ((controllerOutput_ * outSpan_) + outMin_);
-
+    return controllerOutput_;
 }
 
 float PID::getInMin() {