PID.cpp

00001 #include "PID.h"
00002 #include <stdlib.h>
00003 
00004 namespace PID_SPACE
00005 {
00006     
00007     PID::PID(double deltaTime)
00008     {
00009         this->deltaTime = deltaTime;
00010         this->dataRangeLower = 0;
00011         this->dataRangeUpper = 100;
00012         kp = 0;
00013         ki = 0;
00014         kd = 0;
00015     }
00016     
00017     PID::PID(double deltaTime, double dataRangeLower, double dataRangeUpper)
00018     {
00019         this->deltaTime = deltaTime;
00020         this->dataRangeLower = dataRangeLower;
00021         this->dataRangeUpper = dataRangeUpper;
00022         kp = 0;
00023         ki = 0;
00024         kd = 0;
00025     }
00026     
00027     PID::PID(double deltaTime, double dataRangeLower, double dataRangeUpper, double KP, double KI, double KD)
00028     {
00029         this->deltaTime = deltaTime;
00030         this->dataRangeLower = dataRangeLower;
00031         this->dataRangeUpper = dataRangeUpper;
00032         kp = KP;
00033         ki = KI;
00034         kd = KD;
00035     }
00036     
00037     void PID::SetParam(double KP, double KI, double KD)
00038     {
00039         kp = KP;
00040         ki = KI;
00041         kd = KD;
00042     }
00043     
00044     double PID::SetPV(double sensorData, double targetData)
00045     {
00046         double p, i, d;
00047         
00048         diff[0] = diff[1];
00049         diff[1] = targetData - sensorData;
00050         integral += ((diff[1] + diff[0]) / 2.0) * deltaTime;
00051         
00052         p = kp * diff[1];
00053         i = ki * integral;
00054         d = kd * ((diff[1] - diff[0]) / deltaTime);
00055         mv = PID::limit(p + i + d, dataRangeLower, dataRangeUpper);
00056         return mv;
00057     }
00058     
00059     double PID::GetMV()
00060     {
00061         return mv;
00062     }
00063     
00064     double PID::limit(double data,double lower,double upper)
00065     {
00066         if(data < lower)        return lower;
00067         else if(data > upper)   return upper;
00068         else                    return data;
00069     }
00070     
00071 }
00072