Diff: PidController.cpp

Revision:

4:b590bd8fec6f

Parent:

3:c169d08a9d0b

Child:

5:1206105e20bd

--- a/PidController.cpp	Wed Sep 06 23:45:40 2017 +0000
+++ b/PidController.cpp	Wed Oct 04 01:13:49 2017 +0000
@@ -1,12 +1,13 @@
 #include "mbed.h"
 #include "PidController.h"
 
-PidController::PidController(){
+PidController::PidController(bool sqrt){
     elapsedTime =0;
     mode = MANUAL;
+    squareRootOuptut = sqrt;
     }
     
-int PidController::Calculate(float SP, float PV) 
+int PidController::Calculate(float SP, float PV, float ManualMV) 
     {
         float CV;                   //(mm/s) Control Variable
         float IntegralAction;       // Integral Contribution to Output
@@ -14,44 +15,48 @@
     
         if (mode == MANUAL)
         {   
-            CV = SP;
+            CV = ManualMV;      //Write Manual Manipulated Variable
             accumError = 0;
-            lastError = 0;
         }
         else 
         {   
             //Calc error
-            error = SP -PV;  
+            error = SP - PV;  
             IntegralAction = K_i*(accumError + error);
-            DerivativeAction = K_d*(error - lastError);
+            DerivativeAction = K_d*(PV - lastInput);
             
             //-- PID Calculation
-            CV = bias + K_p*error, + IntegralAction - DerivativeAction;
+            if (SP) { 
+                CV =  K_p*error + IntegralAction - DerivativeAction;
+                if ((CV > 0) &&  squareRootOuptut)CV = sqrt(CV);
+                }
+            else 
             
-            //-- Clamp Integral if Output is Saturated
-            if ((CV > maxLimit) || (CV < minLimit))
-            {
-                accumError = accumError;
-                }
-            else
+            CV= 0;
+            
+                
+            //-- Only allow the Controller to integrate if the output isnt saturated
+            if ((CV < maxLimit) || (CV > minLimit))
             {
                 accumError += error;
                 }
             
-            //-- Set Error for Next Scan
-            lastError = error;  
+            //-- Told to stop!
+            if (!SP) accumError = 0;
+
+            //-- Save Current Input for Next Loop
+            lastInput = PV;  
+            
+            //Check to See Output is Within Limits
+            if (CV > maxLimit){CV= maxLimit;}
+            if (CV < minLimit){CV= minLimit;}
         }
            
         
-        //Check to See Output is Within Limits
-        if (CV > maxLimit){CV= maxLimit;}
-        if (CV < minLimit){CV= minLimit;}
-        
-        
+        //-- Make message to send to GUI
         if (collectDiagnostics){BuildDiagMessage(SP,PV, CV, K_p*error, IntegralAction, DerivativeAction);}
         
-        //Convert from mm/s to 0-100%
-        return (int)(CV*scalar);
+        return (int)(CV);
     }
 
 void PidController::UpdateSettings(float Bias, float PropGain, float IntGain, float DiffGain, float OutputMin, float OutputMax, float OutputScale){
@@ -64,6 +69,12 @@
     scalar = OutputScale;
     }
 
+void PidController::UpdateSettings(float OutputMin, float OutputMax){
+    minLimit = OutputMin;
+    maxLimit = OutputMax;
+    }
+
+
 void PidController::StartDiag(void){
     elapsedTime =0;
     collectDiagnostics = true;
@@ -75,8 +86,7 @@
     }
 
 void PidController::BuildDiagMessage(float SetPoint, float ProcessVar, 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, SetPoint, ProcessVar, PWM, PropAction, IntAction, DifAction);
+    sprintf(diagMsg, "P %d %0.4f %0.4f %0.1f %0.4f %0.4f %0.4f\n", elapsedTime, SetPoint, ProcessVar, PWM, PropAction, IntAction, DifAction);
     elapsedTime += RATE;
     if (elapsedTime > 32000){sprintf(diagMsg, "R");}
     }