PID.cpp

00001 #include "PID.h"
00002 #include "mbed.h"
00003 
00004 PID::PID(volatile double* Input, volatile double* Output, volatile double* Setpoint, double Kp, double Ki, double Kd, int POn, int ControllerDirection){
00005     PIDtimer.start();
00006     PIDtimer.reset();   
00007     myOutput = Output;
00008     myInput = Input;
00009     mySetpoint = Setpoint;
00010     inAuto = true;
00011     PID::SetOutputLimits(0, 1); //default output limit corresponds to
00012                                                 //the arduino pwm limits
00013     SampleTime = .1;                           //default Controller Sample Time is 0.1 seconds
00014 
00015     PID::SetControllerDirection(ControllerDirection);
00016     PID::SetTunings(Kp, Ki, Kd, POn);
00017 
00018     lastTime = PIDtimer.read()-SampleTime;
00019       //  printf("the values are %f, %f, %f, %f, %f, %f \r\n", *myOutput, *myInput, *mySetpoint, kp, ki, kd);
00020     }
00021     
00022 /* Compute() **********************************************************************
00023  *     This, as they say, is where the magic happens.  this function should be called
00024  *   every time "void loop()" executes.  the function will decide for itself whether a new
00025  *   pid Output needs to be computed.  returns true when the output is computed,
00026  *   false when nothing has been done.
00027  **********************************************************************************/
00028 bool PID::Compute()
00029 {
00030    if(!inAuto) return false;
00031    double now = PIDtimer.read();
00032    double timeChange = (now - lastTime);
00033    if(timeChange>=SampleTime)
00034    {
00035       /*Compute all the working error variables*/
00036       double input = *myInput;
00037       double error = *mySetpoint - input;
00038       double dInput = (input - lastInput);
00039       outputSum+= (ki * error);
00040         
00041       /*Add Proportional on Measurement, if P_ON_M is specified*/
00042       if(!pOnE) outputSum -= kp * dInput;
00043       if(outputSum > outMax) outputSum= outMax;
00044       else if(outputSum < outMin) outputSum= outMin;
00045 
00046       /*Add Proportional on Error, if P_ON_E is specified*/
00047        double output;
00048       if(pOnE) output = kp * error;
00049       else output = 0;
00050       /*Compute Rest of PID Output*/
00051       output += outputSum - kd * dInput;
00052 
00053         if(output > outMax) output = outMax;
00054       else if(output < outMin) output = outMin;
00055         *myOutput = output;
00056 
00057       /*Remember some variables for next time*/
00058       lastInput = input;
00059       lastTime = now;
00060       //printf("the values are %f, %f, %f, %f \r\n", *myOutput, *myInput, *mySetpoint, lastTime);
00061 
00062         return true;
00063    }
00064    else 
00065    {
00066        return false;
00067     }
00068 }
00069 
00070 /* SetTunings(...)*************************************************************
00071  * This function allows the controller's dynamic performance to be adjusted.
00072  * it's called automatically from the constructor, but tunings can also
00073  * be adjusted on the fly during normal operation
00074  ******************************************************************************/
00075 void PID::SetTunings(double Kp, double Ki, double Kd, int POn)
00076 {
00077    if (Kp<0 || Ki<0 || Kd<0) return;
00078 
00079    pOn = POn;
00080    pOnE = POn == P_ON_E;
00081 
00082    dispKp = Kp; dispKi = Ki; dispKd = Kd;
00083 
00084    double SampleTimeInSec = ((double)SampleTime)/1000;
00085    kp = Kp;
00086    ki = Ki * SampleTimeInSec;
00087    kd = Kd / SampleTimeInSec;
00088 
00089   if(controllerDirection ==REVERSE)
00090    {
00091       kp = (0 - kp);
00092       ki = (0 - ki);
00093       kd = (0 - kd);
00094    }
00095 }
00096 
00097 /* SetTunings(...)*************************************************************
00098  * Set Tunings using the last-rembered POn setting
00099  ******************************************************************************/
00100 void PID::SetTunings(double Kp, double Ki, double Kd){
00101     SetTunings(Kp, Ki, Kd, pOn); 
00102 }
00103 
00104 /* SetSampleTime(...) *********************************************************
00105  * sets the period, in Milliseconds, at which the calculation is performed
00106  ******************************************************************************/
00107 void PID::SetSampleTime(int NewSampleTime)
00108 {
00109    if (NewSampleTime > 0)
00110    {
00111       double ratio  = (double)NewSampleTime
00112                       / (double)SampleTime;
00113       ki *= ratio;
00114       kd /= ratio;
00115       SampleTime = (unsigned long)NewSampleTime;
00116    }
00117 }
00118 
00119 /* SetOutputLimits(...)****************************************************
00120  *     This function will be used far more often than SetInputLimits.  while
00121  *  the input to the controller will generally be in the 0-1023 range (which is
00122  *  the default already,)  the output will be a little different.  maybe they'll
00123  *  be doing a time window and will need 0-8000 or something.  or maybe they'll
00124  *  want to clamp it from 0-125.  who knows.  at any rate, that can all be done
00125  *  here.
00126  **************************************************************************/
00127 void PID::SetOutputLimits(double Min, double Max)
00128 {
00129    if(Min >= Max) return;
00130    outMin = Min;
00131    outMax = Max;
00132 
00133    if(inAuto)
00134    {
00135        if(*myOutput > outMax) *myOutput = outMax;
00136        else if(*myOutput < outMin) *myOutput = outMin;
00137 
00138        if(outputSum > outMax) outputSum= outMax;
00139        else if(outputSum < outMin) outputSum= outMin;
00140    }
00141 }
00142 
00143 /* SetMode(...)****************************************************************
00144  * Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
00145  * when the transition from manual to auto occurs, the controller is
00146  * automatically initialized
00147  ******************************************************************************/
00148 void PID::SetMode(int Mode)
00149 {
00150     bool newAuto = (Mode == AUTOMATIC);
00151     if(newAuto && !inAuto)
00152     {  /*we just went from manual to auto*/
00153         PID::Initialize();
00154     }
00155     inAuto = newAuto;
00156 }
00157 
00158 /* Initialize()****************************************************************
00159  *  does all the things that need to happen to ensure a bumpless transfer
00160  *  from manual to automatic mode.
00161  ******************************************************************************/
00162 void PID::Initialize()
00163 {
00164    outputSum = *myOutput;
00165    lastInput = *myInput;
00166    if(outputSum > outMax) outputSum = outMax;
00167    else if(outputSum < outMin) outputSum = outMin;
00168 }
00169 
00170 /* SetControllerDirection(...)*************************************************
00171  * The PID will either be connected to a DIRECT acting process (+Output leads
00172  * to +Input) or a REVERSE acting process(+Output leads to -Input.)  we need to
00173  * know which one, because otherwise we may increase the output when we should
00174  * be decreasing.  This is called from the constructor.
00175  ******************************************************************************/
00176 void PID::SetControllerDirection(int Direction)
00177 {
00178    if(inAuto && Direction !=controllerDirection)
00179    {
00180         kp = (0 - kp);
00181       ki = (0 - ki);
00182       kd = (0 - kd);
00183    }
00184    controllerDirection = Direction;
00185 }
00186 
00187 /* Status Funcions*************************************************************
00188  * Just because you set the Kp=-1 doesn't mean it actually happened.  these
00189  * functions query the internal state of the PID.  they're here for display
00190  * purposes.  this are the functions the PID Front-end uses for example
00191  ******************************************************************************/
00192 double PID::GetKp(){ return  dispKp; }
00193 double PID::GetKi(){ return  dispKi;}
00194 double PID::GetKd(){ return  dispKd;}
00195 int PID::GetMode(){ return  inAuto ? AUTOMATIC : MANUAL;}
00196 int PID::GetDirection(){ return controllerDirection;}