PID
Dependents: Nucleo_spi 2015_denziben_i2c_S2 Nucleo_Motor Nucleo_Motor
Fork of PID by
Diff: PID.cpp
- Revision:
- 5:3519920d064d
- Parent:
- 3:34f4f22b18e7
- Child:
- 6:775c9421fe3b
--- a/PID.cpp Tue Aug 18 04:45:10 2015 +0000 +++ b/PID.cpp Wed Sep 23 06:01:32 2015 +0000 @@ -24,7 +24,7 @@ * THE SOFTWARE. * * @section DESCRIPTION - * + * * A PID controller is a widely used feedback controller commonly found in * industry. * @@ -48,62 +48,105 @@ * Includes */ #include "PID.h" - -PID::PID(float tauKp, float tauKi, float tauKd) +PID::PID(const PID& p) { - + timer = p.timer; + data = 0; + GAIN_P = p.GAIN_P; + GAIN_I = p.GAIN_I; + GAIN_D = p.GAIN_D; + setInterval(0.001); + s_dErrIntg=0; + dErr_prev=0;OutputLimits(1,0); +} +PID::PID(float tauKp, float tauKi, float tauKd,Timer *T) +{ + timer=T; data = 0; GAIN_P = tauKp; GAIN_I = tauKi; GAIN_D = tauKd; setInterval(0.001); - s_dErrIntg=0; dErr_prev=0; + s_dErrIntg=0; + dErr_prev=0;OutputLimits(1,0); +} + +void PID::InputLimits(float max,float min) +{ + //Make sure we haven't been given impossible values. + if (min >= max) { + return; + } + + + InMin = min; + InMax = max; + InSpan = InMax - InMin; +} +void PID::OutputLimits(float max,float min) +{ + if (min >= max) { + return; + } + + + OutMin = min; + OutMax = max; + OutSpan = OutMax - OutMin; } void PID::setInterval(double inter) { interval = inter; //start(); } -void PID::Start() +void PID::start() { - timer.start(); - T.attach(this,&PID::PIDctrl,interval); + timer->start(); + //T.attach(this,&PID::PIDctrl,interval); //printf("PID statr\n"); //wait(0.1); //PIDctrl(); - + } void PID::stop() { - timer.stop(); - T.detach(); + timer->stop(); + //T.detach(); } -void PID::Reset() +void PID::pid_reset() { - dTarget=0; - dPoint=0; - // PI制御ゲイン - data=0; - s_dErrIntg=0 ,dErr_prev=0; + dTarget=0; + dPoint=0; + // PI制御ゲイン + data=0; + s_dErrIntg=0 ,dErr_prev=0; } void PID::PIDctrl() { double dErr; double dRet; - + // 誤差 dErr = dTarget - dPoint; - - double dErrDiff = (dErr-dErr_prev)/timer.read(); - //printf("PID %f,%f,%f,%f\n",dErr,s_dErrIntg,dErrDiff,timer.read()); + float T=gettime(); + //printf("%f\t",T); + double dErrDiff = (dErr-data)/T; // 誤差積分 - s_dErrIntg += (dErr+dErr_prev )* timer.read() /2.0; + if(data>OutMax)s_dErrIntg=OutMax; + else if(data<OutMin)s_dErrIntg=OutMin; + else s_dErrIntg += (dErr+dErr_prev )* T /2.0; // 制御入力 dRet = GAIN_P * dErr + GAIN_I * s_dErrIntg + GAIN_D*dErrDiff; - timer.reset(); + dErr_prev = dErr; - data = dRet; + if(dRet>OutMax)data=OutMax; + else if(dRet<OutMin)data=OutMin; + else data = dRet; + + //printf("PID %f,%f,%f,%f,%f\r\n",data ,dErr,s_dErrIntg,dErrDiff,timer->read()); + } +