Diff: PidController.cpp

Revision:

5:1206105e20bd

Parent:

4:b590bd8fec6f

Child:

6:99403113343f

--- a/PidController.cpp	Wed Oct 04 01:13:49 2017 +0000
+++ b/PidController.cpp	Tue Oct 31 03:46:43 2017 +0000
@@ -1,13 +1,13 @@
 #include "mbed.h"
 #include "PidController.h"
 
-PidController::PidController(bool sqrt){
+PidController::PidController(char c){
     elapsedTime =0;
     mode = MANUAL;
-    squareRootOuptut = sqrt;
+    diagChar = c;
     }
     
-int PidController::Calculate(float SP, float PV, float ManualMV) 
+float PidController::Calculate(float SP, float PV, float ManualMV) 
     {
         float CV;                   //(mm/s) Control Variable
         float IntegralAction;       // Integral Contribution to Output
@@ -17,35 +17,37 @@
         {   
             CV = ManualMV;      //Write Manual Manipulated Variable
             accumError = 0;
+            prevError = 0;
         }
         else 
         {   
             //Calc error
             error = SP - PV;  
             IntegralAction = K_i*(accumError + error);
-            DerivativeAction = K_d*(PV - lastInput);
+            //DerivativeAction = K_d*(PV - lastInput);
+            DerivativeAction = K_d*(error - prevError);
             
             //-- PID Calculation
-            if (SP) { 
-                CV =  K_p*error + IntegralAction - DerivativeAction;
-                if ((CV > 0) &&  squareRootOuptut)CV = sqrt(CV);
-                }
+            if (SP)
+            { 
+                CV =  bias + K_p*error + IntegralAction + DerivativeAction;
+                if (CV>0) {CV = sqrt(CV);}
+            }
             else 
+            {
+                CV= 0;
+                accumError = 0;
+            }
             
-            CV= 0;
-            
-                
             //-- Only allow the Controller to integrate if the output isnt saturated
             if ((CV < maxLimit) || (CV > minLimit))
             {
                 accumError += error;
                 }
-            
-            //-- Told to stop!
-            if (!SP) accumError = 0;
 
             //-- Save Current Input for Next Loop
-            lastInput = PV;  
+            //lastInput = PV;  
+            prevError = error;
             
             //Check to See Output is Within Limits
             if (CV > maxLimit){CV= maxLimit;}
@@ -56,38 +58,41 @@
         //-- Make message to send to GUI
         if (collectDiagnostics){BuildDiagMessage(SP,PV, CV, K_p*error, IntegralAction, DerivativeAction);}
         
-        return (int)(CV);
+        return CV;
     }
 
-void PidController::UpdateSettings(float Bias, float PropGain, float IntGain, float DiffGain, float OutputMin, float OutputMax, float OutputScale){
+void PidController::UpdateSettings(float Bias, float PropGain, float IntGain, float DiffGain, float OutputMin, float OutputMax){
     bias = Bias;
     K_p = PropGain;
     K_i = IntGain;
     K_d = DiffGain;
     minLimit = OutputMin;
     maxLimit = OutputMax;
-    scalar = OutputScale;
+    return;
     }
 
 void PidController::UpdateSettings(float OutputMin, float OutputMax){
     minLimit = OutputMin;
     maxLimit = OutputMax;
+    return;
     }
 
 
 void PidController::StartDiag(void){
     elapsedTime =0;
     collectDiagnostics = true;
+    return;
     }
 
 void PidController::EndDiag(void){
     collectDiagnostics = false;
-
+    return;
     }
 
 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, "%c %d %0.4f %0.4f %0.1f %0.4f %0.4f %0.4f\n", diagChar, elapsedTime, SetPoint, ProcessVar, PWM, PropAction, IntAction, DifAction);
+    sprintf(diagMsg, "%c %d %0.4f %0.4f %0.1f 0.0 0.0 0.0\n", diagChar, elapsedTime, SetPoint, ProcessVar, PWM); 
     elapsedTime += RATE;
-    if (elapsedTime > 32000){sprintf(diagMsg, "R");}
+    return;
     }