PidController.cpp

00001 #include "mbed.h"
00002 #include "PidController.h"
00003 
00004 PidController::PidController(char c){
00005     elapsedTime =0;
00006     mode = MANUAL;
00007     diagChar = c;
00008     }
00009     
00010 float PidController::Calculate(float SP, float PV, float ManualMV) 
00011     {
00012         float CV;                   //(mm/s) Control Variable
00013         float IntegralAction;       // Integral Contribution to Output
00014         float DerivativeAction;     // Derivative Contribution to Output
00015     
00016         if (mode == MANUAL)
00017         {   
00018             CV = ManualMV;      //Write Manual Manipulated Variable
00019             accumError = 0;
00020             prevError = 0;
00021         }
00022         else 
00023         {   
00024             //Calc error
00025             error = SP - PV;  
00026             IntegralAction = K_i*(accumError + error);
00027             //DerivativeAction = K_d*(PV - lastInput);
00028             DerivativeAction = K_d*(error - prevError);
00029             
00030             //-- PID Calculation
00031             if (SP)
00032             { 
00033                 CV =  bias + K_p*error + IntegralAction + DerivativeAction;
00034                 if (CV>0) {CV = sqrt(CV);}
00035             }
00036             else 
00037             {
00038                 CV= 0;
00039                 accumError = 0;
00040             }
00041             
00042             //-- Only allow the Controller to integrate if the output isnt saturated
00043             if ((CV < maxLimit) || (CV > minLimit))
00044             {
00045                 accumError += error;
00046                 }
00047 
00048             //-- Save Current Input for Next Loop
00049             //lastInput = PV;  
00050             prevError = error;
00051             
00052             //Check to See Output is Within Limits
00053             if (CV > maxLimit){CV= maxLimit;}
00054             if (CV < minLimit){CV= minLimit;}
00055         }
00056            
00057         
00058         //-- Make message to send to GUI
00059         if (collectDiagnostics){BuildDiagMessage(SP,PV, CV, K_p*error, IntegralAction, DerivativeAction);}
00060         
00061         return CV;
00062     }
00063 
00064 void PidController::UpdateSettings(float Bias, float PropGain, float IntGain, float DiffGain, float OutputMin, float OutputMax){
00065     bias = Bias;
00066     K_p = PropGain;
00067     K_i = IntGain;
00068     K_d = DiffGain;
00069     minLimit = OutputMin;
00070     maxLimit = OutputMax;
00071     return;
00072     }
00073 
00074 void PidController::UpdateSettings(float OutputMin, float OutputMax){
00075     minLimit = OutputMin;
00076     maxLimit = OutputMax;
00077     return;
00078     }
00079 
00080 
00081 void PidController::StartDiag(void){
00082     elapsedTime =0;
00083     collectDiagnostics = true;
00084     return;
00085     }
00086 
00087 void PidController::EndDiag(void){
00088     collectDiagnostics = false;
00089     return;
00090     }
00091 
00092 void PidController::BuildDiagMessage(float SetPoint, float ProcessVar, float PWM, float PropAction, float IntAction, float DifAction){
00093     //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);
00094     sprintf(diagMsg, "%c %d %0.4f %0.4f %0.1f 0.0 0.0 0.0\n", diagChar, elapsedTime, SetPoint, ProcessVar, PWM); 
00095     elapsedTime += RATE;
00096     return;
00097     }
00098