From a9541e9b3777cfc6dc49fa556b7da3770c44d06c Mon Sep 17 00:00:00 2001
From: bundgus <bundgus@gmail.com>
Date: Thu, 21 Feb 2013 21:09:28 -0600
Subject: [PATCH] modified for oescc

---
 .DS_Store         | Bin 0 -> 6148 bytes
 PID_v1/PID_v1.cpp |  41 +++++++++++++++++++++++++++++++++++++----
 PID_v1/PID_v1.h   |   7 +++++--
 3 files changed, 42 insertions(+), 6 deletions(-)
 create mode 100644 .DS_Store

diff --git a/.DS_Store b/.DS_Store
new file mode 100644
index 0000000000000000000000000000000000000000..2cbf1a3db3a9bcbfd6ad5d7307c56a7bef264074
GIT binary patch
literal 6148
zcmeHK%}T>S5Z-NTO;8~hp~uB@k)&Wz@DM@;y%kdE!Aebt*g{E4leUK%4Ze`C;S=~g
z&g^au!JI|x49tGB^Rt`%Ap64@<K8-Q7_%5-1}Gv&qejqO8*1oeL@vkJFelum%W#o|
zH3R)c7k+!24cQ$Vuqms4|3|o7%+t)azj&kBI%ppnrfD2c#`8EzwqCN1qx9aNC0l<U
zJ%z#QLE2f6ty3>p$;dFsW)P9~C`lGzPjvQp9%Zws)!FTwtFCZtJMW5})9YP!MgO9o
z=cZwp)~9iLc7A<33SRQpw?gs9fodry8cTQwqq3BZzls-0{0#oGrpzTI28aP-fEd_g
z2K1p|9PKd&v~*&C7^r6e_Xh!r=xQt!%B=$$ygp;Rg@^(+z9kTaMpt8@5F#L4l>(|#
zuAdlOm4jdCJXd3(P?a+-XNGa~%;n>S%h|y%WIE%nLTZTtVxY=EQ*|vo|8L-zseR;E
zOQ=T-5Ci{=0p1$;0}mFZ&(<Hy!?RX^K7yiPUV#P(*p*8FbZ{T($e@Y~v?0&cSSZ9%
T&@amY=^`Krp@tav1qQwWCniWN

literal 0
HcmV?d00001

diff --git a/PID_v1/PID_v1.cpp b/PID_v1/PID_v1.cpp
index 6c95895..f1659a9 100644
--- a/PID_v1/PID_v1.cpp
+++ b/PID_v1/PID_v1.cpp
@@ -21,6 +21,12 @@ PID::PID(double* Input, double* Output, double* Setpoint,
         double Kp, double Ki, double Kd, int ControllerDirection)
 {
 	
+    priorInputsSum = 0.0;
+    priorInputCounter = 0;
+    for (int i = 0; i<5; i++){
+        priorInputs[i] =  0.0;
+        
+    }
     myOutput = Output;
     myInput = Input;
     mySetpoint = Setpoint;
@@ -54,13 +60,33 @@ bool PID::Compute()
       /*Compute all the working error variables*/
 	  double input = *myInput;
       double error = *mySetpoint - input;
-      ITerm+= (ki * error);
+       if (((kp * error)+ ITerm) < outMax){ // Modified Tracking Anti-Windup.. if proportional output is maxed, don't wind up the accumulator
+           ITerm+= (ki * error);}
       if(ITerm > outMax) ITerm= outMax;
       else if(ITerm < outMin) ITerm= outMin;
-      double dInput = (input - lastInput);
+      
+       priorInputs[priorInputCounter] = input; // keep the last 5 input samples to be used for averaging for the differential multiplier Pd
+    
+       if (priorInputCounter < 4){
+           priorInputCounter++;}
+       else{
+           priorInputCounter = 0;
+       }
+       
+       double currentInputSum = 0.0;
+       
+       for (int i = 0; i < 5; i++){
+           currentInputSum += priorInputs[i];
+       }
+      currentInputSum = currentInputSum / 5;
+      
+       
+      double dInput = (currentInputSum - priorInputsSum); // average of last 5 input samples minus average of last 2-6 input samples
+      //double dInput = (input - lastInput);
+      priorInputsSum = currentInputSum;
  
       /*Compute PID Output*/
-      double output = kp * error + ITerm- kd * dInput;
+      double output = kp * error + ITerm - kd * dInput;
       
 	  if(output > outMax) output = outMax;
       else if(output < outMin) output = outMin;
@@ -74,6 +100,13 @@ bool PID::Compute()
    else return false;
 }
 
+/* ClearITerm() **********************************************************************
+ *     Called to clear out the accumulator value, in the event of a significant tuning change
+ **********************************************************************************/
+void PID::ClearITerm()
+{
+    ITerm = 0;
+}
 
 /* SetTunings(...)*************************************************************
  * This function allows the controller's dynamic performance to be adjusted. 
@@ -88,7 +121,7 @@ void PID::SetTunings(double Kp, double Ki, double Kd)
    
    double SampleTimeInSec = ((double)SampleTime)/1000;  
    kp = Kp;
-   ki = Ki * SampleTimeInSec;
+   ki = Ki * SampleTimeInSec/100;  // set the scale to allow reasonible Ki values > 1 for slow responding system
    kd = Kd / SampleTimeInSec;
  
   if(controllerDirection ==REVERSE)
diff --git a/PID_v1/PID_v1.h b/PID_v1/PID_v1.h
index 6f86697..1f352b7 100644
--- a/PID_v1/PID_v1.h
+++ b/PID_v1/PID_v1.h
@@ -41,7 +41,7 @@ class PID
 										  //   once it is set in the constructor.
     void SetSampleTime(int);              // * sets the frequency, in Milliseconds, with which 
                                           //   the PID calculation is performed.  default is 100
-										  
+	void ClearITerm();                    //   Called to clear out the accumulator value, in the event of a significant tuning change
 										  
 										  
   //Display functions ****************************************************************
@@ -71,7 +71,10 @@ class PID
 			  
 	unsigned long lastTime;
 	double ITerm, lastInput;
-
+    double priorInputs[5];
+    int priorInputCounter;
+    double priorInputsSum;
+    
 	unsigned long SampleTime;
 	double outMin, outMax;
 	bool inAuto;