PidController.cpp

00001 #include "mbed.h"
00002 #include "PidController.h"
00003 
00004 PidController::PidController(){
00005     elapsedTime =0;
00006     mode = MANUAL;
00007     }
00008     
00009 float PidController::Calculate(float SP, float PV) 
00010     {
00011         float CV;                   //(mm/s) Control Variable
00012         float ProportionalAction;
00013         float IntegralAction;       // Integral Contribution to Output
00014         float DerivativeAction;     // Derivative Contribution to Output
00015     
00016         if (mode == MANUAL)
00017         {   
00018             CV = SP;                    //Write Directly to Output
00019             accumError = 0;
00020             prevError = 0;
00021         }
00022         else 
00023         {   
00024             //Calc error
00025             error = SP - PV; 
00026             ProportionalAction =  K_p*error ;
00027             IntegralAction = K_i*(accumError + error);
00028             DerivativeAction = K_d*(error - prevError);
00029             
00030             //-- PID Calculation
00031             if (SP)
00032             { 
00033                 CV =  bias + ProportionalAction + IntegralAction + DerivativeAction;
00034                 if (CV>0) {CV = sqrt(CV);}
00035                 else if (CV<0)
00036                 {
00037                     CV = sqrt(CV*-1.0);
00038                     CV = CV*-1.0;
00039                     }
00040             }
00041             else 
00042             {
00043                 CV= 0;
00044                 accumError = 0;
00045             }
00046             
00047             //-- Only allow the Controller to integrate if the output isnt saturated
00048             if ((CV < maxLimit) || (CV > minLimit)){accumError += error;}
00049 
00050             //-- Save Current Input for Next Loop
00051             prevError = error;
00052         }
00053         
00054         //Check to See Output is Within Limits
00055         if (CV > maxLimit){CV= maxLimit;}
00056         if (CV < minLimit){CV= minLimit;}
00057            
00058         //-- Make message to send to GUI
00059         if (collectDiagnostics)//{BuildDiagMessage(SP,PV, CV, K_p*error, IntegralAction, DerivativeAction);}
00060         {
00061             int_to_byte(diagMsg, elapsedTime);
00062             float_to_byte(diagMsg +2, &SP);
00063             float_to_byte(diagMsg +6, &PV);
00064             float_to_byte(diagMsg +10, &CV);
00065             float_to_byte(diagMsg +14, &ProportionalAction);
00066             float_to_byte(diagMsg +18, &IntegralAction);
00067             float_to_byte(diagMsg +22, &DerivativeAction);
00068             }
00069         
00070         return CV;
00071     }
00072 
00073 void PidController::UpdateSettings(float Bias, float PropGain, float IntGain, float DiffGain, float OutputMin, float OutputMax){
00074     bias = Bias;
00075     K_p = PropGain;
00076     K_i = IntGain;
00077     K_d = DiffGain;
00078     minLimit = OutputMin;
00079     maxLimit = OutputMax;
00080     return;
00081     }
00082 
00083 void PidController::UpdateSettings(float OutputMin, float OutputMax){
00084     minLimit = OutputMin;
00085     maxLimit = OutputMax;
00086     return;
00087     }
00088 
00089 
00090 void PidController::StartDiag(void){
00091     elapsedTime =0;
00092     collectDiagnostics = true;
00093     return;
00094     }
00095 
00096 void PidController::EndDiag(void){
00097     collectDiagnostics = false;
00098     return;
00099     }
00100 
00101 void PidController::GetDiagnosticsMessage(char *data){
00102     memcpy(data, &diagMsg, 27 );
00103     return;
00104     }
00105 //-- Helper Functions
00106     
00107 //-- Convert floating point to byte array
00108 void PidController::float_to_byte(char *data, float *val) {
00109       memcpy(data, val, sizeof(float)); 
00110 }
00111 
00112 //-- Convert integer to byte array
00113 void PidController::int_to_byte(char *data, uint16_t val) {
00114       memcpy(data, &val, sizeof val); 
00115 }