Diff: PidController.cpp

Revision:

6:99403113343f

Parent:

5:1206105e20bd

--- a/PidController.cpp	Tue Oct 31 03:46:43 2017 +0000
+++ b/PidController.cpp	Mon May 07 05:15:19 2018 +0000
@@ -1,37 +1,42 @@
 #include "mbed.h"
 #include "PidController.h"
 
-PidController::PidController(char c){
+PidController::PidController(){
     elapsedTime =0;
     mode = MANUAL;
-    diagChar = c;
     }
     
-float PidController::Calculate(float SP, float PV, float ManualMV) 
+float PidController::Calculate(float SP, float PV) 
     {
         float CV;                   //(mm/s) Control Variable
+        float ProportionalAction;
         float IntegralAction;       // Integral Contribution to Output
         float DerivativeAction;     // Derivative Contribution to Output
     
         if (mode == MANUAL)
         {   
-            CV = ManualMV;      //Write Manual Manipulated Variable
+            CV = SP;                    //Write Directly to Output
             accumError = 0;
             prevError = 0;
         }
         else 
         {   
             //Calc error
-            error = SP - PV;  
+            error = SP - PV; 
+            ProportionalAction =  K_p*error ;
             IntegralAction = K_i*(accumError + error);
-            //DerivativeAction = K_d*(PV - lastInput);
             DerivativeAction = K_d*(error - prevError);
             
             //-- PID Calculation
             if (SP)
             { 
-                CV =  bias + K_p*error + IntegralAction + DerivativeAction;
+                CV =  bias + ProportionalAction + IntegralAction + DerivativeAction;
                 if (CV>0) {CV = sqrt(CV);}
+                else if (CV<0)
+                {
+                    CV = sqrt(CV*-1.0);
+                    CV = CV*-1.0;
+                    }
             }
             else 
             {
@@ -40,23 +45,27 @@
             }
             
             //-- Only allow the Controller to integrate if the output isnt saturated
-            if ((CV < maxLimit) || (CV > minLimit))
-            {
-                accumError += error;
-                }
+            if ((CV < maxLimit) || (CV > minLimit)){accumError += error;}
 
             //-- Save Current Input for Next Loop
-            //lastInput = PV;  
             prevError = error;
-            
-            //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);}
+        if (collectDiagnostics)//{BuildDiagMessage(SP,PV, CV, K_p*error, IntegralAction, DerivativeAction);}
+        {
+            int_to_byte(diagMsg, elapsedTime);
+            float_to_byte(diagMsg +2, &SP);
+            float_to_byte(diagMsg +6, &PV);
+            float_to_byte(diagMsg +10, &CV);
+            float_to_byte(diagMsg +14, &ProportionalAction);
+            float_to_byte(diagMsg +18, &IntegralAction);
+            float_to_byte(diagMsg +22, &DerivativeAction);
+            }
         
         return CV;
     }
@@ -89,10 +98,18 @@
     return;
     }
 
-void PidController::BuildDiagMessage(float SetPoint, float ProcessVar, float PWM, float PropAction, float IntAction, float 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;
+void PidController::GetDiagnosticsMessage(char *data){
+    memcpy(data, &diagMsg, 27 );
     return;
     }
+//-- Helper Functions
+    
+//-- Convert floating point to byte array
+void PidController::float_to_byte(char *data, float *val) {
+      memcpy(data, val, sizeof(float)); 
+}
 
+//-- Convert integer to byte array
+void PidController::int_to_byte(char *data, uint16_t val) {
+      memcpy(data, &val, sizeof val); 
+}