Diff: PID.cpp

Revision:

1:c67f4f763c45

Parent:

0:6e12a3e5af19

--- a/PID.cpp	Thu Sep 02 16:48:10 2010 +0000
+++ b/PID.cpp	Tue Aug 11 13:05:25 2015 +0000
@@ -74,6 +74,10 @@
     bias_     = 0.0;
     
     realOutput_ = 0.0;
+    
+    for (int i = 0; i < 15; i++ )
+        accErrorArray[i] = 0;
+    accPosition = 0;
 
 }
 
@@ -125,13 +129,12 @@
 }
 
 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;
     }
 
-    //Store raw values to hand back to user on request.
+    //Store raw values to hand back to user on request.    
     pParam_ = Kc;
     iParam_ = tauI;
     dParam_ = tauD;
@@ -156,7 +159,51 @@
     Kc_   = Kc;
     tauR_ = tempTauR;
     tauD_ = tauD / tSample_;
+}
 
+
+void PID::setTunings(int id, float data) {
+    //Verify that the tunings make sense.
+    if ((id == 1 && data == 0.0) || data < 0.0) {
+        return;
+    }
+    
+    //Store raw values to hand back to user on request.
+    switch (id)
+    {
+        case 1:
+            pParam_ = data;
+            break;
+        case 2:                
+            iParam_ = data;
+            break;
+        case 3:
+            dParam_ = data;
+            break;
+    }
+
+/*
+    float tempTauR;
+
+    if (tauI == 0.0) {
+        tempTauR = 0.0;
+    } else {
+        tempTauR = (1.0 / tauI) * tSample_;
+    }
+
+    //For "bumpless transfer" we need to rescale the accumulated error.
+    if (inAuto) {
+        if (tempTauR == 0.0) {
+            accError_ = 0.0;
+        } else {
+            accError_ *= (Kc_ * tauR_) / (Kc * tempTauR);
+        }
+    }
+
+    Kc_   = Kc;
+    tauR_ = tempTauR;
+    tauD_ = tauD / tSample_;
+*/
 }
 
 void PID::reset(void) {
@@ -243,7 +290,11 @@
     //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)) {
-        accError_ += error;
+        accPosition = ( accPosition == 49 ) ? 0 : accPosition + 1;
+        accErrorArray[accPosition] = error;
+        accError_ = 0;
+        for (int i = 0; i < 50; i++)
+            accError_ += accErrorArray[i] * tSample_;   
     }
 
     //Compute the current slope of the input signal.
@@ -265,6 +316,11 @@
         controllerOutput_ = 1.0;
     }
 
+    if (controllerOutput_ - prevControllerOutput_ > 0.1)
+        controllerOutput_ = prevControllerOutput_ + 0.1;
+    else if ( prevControllerOutput_ - controllerOutput_ > 0.1)
+        controllerOutput_ = prevControllerOutput_ - 0.1;
+        
     //Remember this output for the windup check next time.
     prevControllerOutput_ = controllerOutput_;
     //Remember the input for the derivative calculation next time.