PidController.cpp

00001 #include "mbed.h"
00002 #include "PidController.h"
00003 
00004 PidController::PidController(){
00005     elapsedTime =0;
00006     mode = MANUAL;
00007     }
00008     
00009 int PidController::Calculate(float SP, float PV) 
00010     {
00011         float CV;                   //(mm/s) Control Variable
00012         float IntegralAction;       // Integral Contribution to Output
00013         float DerivativeAction;     // Derivative Contribution to Output
00014     
00015         if (mode == MANUAL)
00016         {   
00017             CV = SP;
00018             accumError = 0;
00019             lastError = 0;
00020         }
00021         else 
00022         {   
00023             //Calc error
00024             error = SP -PV;  
00025             IntegralAction = K_i*(accumError + error);
00026             DerivativeAction = K_d*(error - lastError);
00027             
00028             //-- PID Calculation
00029             CV = bias + K_p*error, + IntegralAction - DerivativeAction;
00030             
00031             //-- Clamp Integral if Output is Saturated
00032             if ((CV > maxLimit) || (CV < minLimit))
00033             {
00034                 accumError = accumError;
00035                 }
00036             else
00037             {
00038                 accumError += error;
00039                 }
00040             
00041             //-- Set Error for Next Scan
00042             lastError = error;  
00043         }
00044            
00045         
00046         //Check to See Output is Within Limits
00047         if (CV > maxLimit){CV= maxLimit;}
00048         if (CV < minLimit){CV= minLimit;}
00049         
00050         
00051         if (collectDiagnostics){BuildDiagMessage(SP,PV, CV, K_p*error, IntegralAction, DerivativeAction);}
00052         
00053         //Convert from mm/s to 0-100%
00054         return (int)(CV*scalar);
00055     }
00056 
00057 void PidController::UpdateSettings(float Bias, float PropGain, float IntGain, float DiffGain, float OutputMin, float OutputMax, float OutputScale){
00058     bias = Bias;
00059     K_p = PropGain;
00060     K_i = IntGain;
00061     K_d = DiffGain;
00062     minLimit = OutputMin;
00063     maxLimit = OutputMax;
00064     scalar = OutputScale;
00065     }
00066 
00067 void PidController::StartDiag(void){
00068     elapsedTime =0;
00069     collectDiagnostics = true;
00070     }
00071 
00072 void PidController::EndDiag(void){
00073     collectDiagnostics = false;
00074 
00075     }
00076 
00077 void PidController::BuildDiagMessage(float SetPoint, float ProcessVar, float PWM, float PropAction, float IntAction, float DifAction){
00078     sprintf(diagMsg, "P %d %0.4f %0.4f %0.1f %0.4f %0.4f %0.4f\n", 
00079                     elapsedTime, SetPoint, ProcessVar, PWM, PropAction, IntAction, DifAction);
00080     elapsedTime += RATE;
00081     if (elapsedTime > 32000){sprintf(diagMsg, "R");}
00082     }
00083