PID.cpp

00001 #include "PID.h"
00002 #include "mbed.h"
00003 
00004 PID::PID(double* _Input, double* _Output, double* _Setpoint, double _Kp, double _Ki, double _Kd, PID_Mode _mode, Timer *_timer)
00005 {
00006 
00007     Input = _Input;
00008     Output = _Output;
00009     Setpoint = _Setpoint;
00010 
00011     timer = _timer;
00012     
00013     SampleTime = 100;
00014     lastTime = timer->read_ms();
00015     setOutputLimits(-1,1);
00016     setParameter_pid(_Kp,_Ki,_Kd);
00017     setPIDMode(_mode);
00018     Initialize();
00019     derror[0]=0;
00020     derror[1]=0;
00021 }
00022 
00023 void PID::control()
00024 {
00025     //timer->start();
00026     double nowtime =timer->read_ms();
00027     double input = *Input;
00028     
00029     if((nowtime - lastTime) > SampleTime)
00030     {    
00031         double error = *Setpoint - input;
00032         double dInput = input - lastInput;
00033         derror[0] = error - lastError;
00034         outputSum += (Ki*error);
00035         double output = 0;
00036         
00037         if(outputSum > outMax) outputSum= outMax;
00038         else if(outputSum < outMin) outputSum= outMin;
00039         
00040         switch(mode)
00041         {
00042             case pos_PID:
00043                 output = outputSum + Kp*error + Kd*derror[0];
00044                 break; 
00045                 
00046             case vec_PID:
00047                 output = *Output + Kp*derror[0] + Kd*error + Ki*(derror[0] - derror[1]);
00048                 
00049             case P_D:
00050                 output = Kp*error - Kd*dInput;
00051                 break;
00052             
00053             case PI_D:
00054                 output = outputSum + Kp*error - Kd*dInput;
00055                 break;
00056             
00057             case I_PD:
00058                 output = outputSum - Kp*input - Kd*dInput;
00059                 break;
00060                 
00061         }
00062         derror[1] = derror[0];
00063         *Output = output;
00064         lastTime = nowtime;
00065         lastInput = input;
00066     }
00067 }
00068 
00069 void PID::setParameter_pid(double new_Kp, double new_Ki, double new_Kd)
00070 {
00071     Kp = new_Kp;
00072     Ki = new_Ki;
00073     Kd = new_Kd;
00074 }
00075 
00076 void PID::setParameter_KuPu(double new_Ku, double new_Pu)
00077 {
00078     Ku = new_Ku;
00079     Pu = new_Pu;
00080     Kp = 0.60 * Ku;
00081     Ti = 0.50 * Pu;
00082     Td = 0.125 * Pu;
00083     Ki = (1 / Ti) * Kp;
00084     Kd = Td * Kp;
00085 }
00086 
00087 void PID::setPIDMode(PID_Mode _mode)
00088 {
00089     mode = _mode;
00090     Initialize();
00091 }
00092 
00093 void PID::setOutputLimits(double Min, double Max)
00094 {
00095     if(Min >= Max) return;
00096     outMin = Min;
00097     outMax = Max;
00098 
00099     if(*Output > outMax) *Output = outMax;
00100     else if(*Output < outMin) *Output = outMin;
00101     if(outputSum > outMax) outputSum = outMax;
00102     else if(outputSum < outMin) outputSum = outMin;
00103 }
00104 
00105 void PID::reset(double target)
00106 {
00107     outputSum = 0;
00108 }
00109 
00110 void PID::Initialize()
00111 {
00112     outputSum = *Output;
00113     lastInput = *Input;
00114     lastError = *Setpoint - lastInput;
00115     if(outputSum > outMax) outputSum = outMax;
00116     else if(outputSum < outMin) outputSum = outMin;
00117 }