float pid(float thempSet, float themp)
{

extern float errThemp[3];		

float	tauDiffFilter;		

float	tauOutFilter;		

float OutPID;
float KoeffNormOut;
float Differential;
float Integral;
float tmpReal;

KoeffNormOut = 255/30		;	

errThemp[2] = errThemp[1];
errThemp[1] = errThemp[0];

errThemp[0] = (themp - thempSet) * KoeffNormOut;

Differential += (((errThemp[0] - errThemp[2]) * 1			 * 0.5) - Differential) / tauDiffFilter;

Integral += (((errThemp[0] + 2*errThemp[1] + errThemp[2]) * 1		 * 0.25) / 1			);

if(Integral > (float)255)
Integral = (float)255;
else
if(Integral < (float)0)
Integral = (float)0;

tmpReal = errThemp[0] + Integral + Differential / 1		;

if(tmpReal > (float)255)
tmpReal = (float)255;
else 
if(tmpReal < (float)0)
tmpReal = (float)0;

OutPID += (tmpReal - OutPID) / tauOutFilter;	

return OutPID;
}