Diff: PidController.cpp

Revision:

2:dd64c2cf9066

Parent:

1:15c12a119814

Child:

3:c169d08a9d0b

diff -r 15c12a119814 -r dd64c2cf9066 PidController.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PidController.cpp	Tue Sep 05 05:18:52 2017 +0000
@@ -0,0 +1,77 @@
+#include "mbed.h"
+#include "PidController.h"
+
+PidController::PidController(){
+    elapsedTime =0;
+    mode = MANUAL;
+    }
+    
+int PidController::Calculate(float SP, float PV) 
+    {
+        float CV;                   //(mm/s) Control Variable
+        float IntegralAction;       // Integral Contribution to Output
+        float DerivativeAction;     // Derivative Contribution to Output
+    
+        if (mode == MANUAL)
+        {   
+            CV = SP;
+            accumError = 0;
+            lastError = 0;
+        }
+        else 
+        {   
+            //Calc error
+            error = SP -PV;  
+            IntegralAction = K_i*(accumError + error);
+            DerivativeAction = K_d*(error - lastError);
+            
+            //-- PID Calculation
+            CV = bias + K_p*error, + IntegralAction - DerivativeAction;
+            
+            //-- Clamp Integral if Output is Saturated
+            if ((CV > maxLimit) || (CV < minLimit))
+            {
+                accumError = accumError;
+                }
+            else
+            {
+                accumError += error;
+                }
+            
+            //-- Set Error for Next Scan
+            lastError = error;  
+        }
+           
+        
+        //Check to See Output is Within Limits
+        if (CV > maxLimit){CV= maxLimit;}
+        if (CV < minLimit){CV= minLimit;}
+        
+        
+        if (collectDiagnostics){BuildDiagMessage(SP,PV, CV*scalar, K_p*error, IntegralAction, DerivativeAction);}
+        
+        //Convert from mm/s to 0-100%
+        return (int)(CV*scalar)*10;
+    }
+
+void PidController::UpdateSettings(float Bias, float PropGain, float IntGain, float DiffGain, float OutputMin, float OutputMax, float OutputScale){
+    bias = Bias;
+    K_p = PropGain;
+    K_i = IntGain;
+    K_d = DiffGain;
+    minLimit = OutputMin;
+    maxLimit = OutputMax;
+    scalar = OutputScale;
+    }
+
+void PidController::StartDiag(void){
+    elapsedTime =0;
+    }
+    
+void PidController::BuildDiagMessage(float SP, float PV, float PWM, float PropAction, float IntAction, float DifAction){
+    sprintf(diagMsg, "P %d %0.4f, %0.4f %0.1f", // %0.4f %0.4f %0.4f\n", 
+                    elapsedTime, SP, PV, PWM);// PropAction, IntAction, DifAction);
+    elapsedTime += RATE;
+    if (elapsedTime > 32000){EndDiag();}
+    }
+