Listing 2: The CalculateGain function

double PidControl::CalculateGain( double position )
{
    double error = Setpoint - position;
    ErrorSum += error;
    ProportionalGain = KProportional * error;
    IntegralGain = KIntegral * ErrorSum;
    if( fabs( IntegralGain ) > IntegralLimit ) {
       if( IntegralGain > 0 )
           IntegralGain = IntegralLimit;
       else
           IntegralGain = -IntegralLimit;
       ErrorSum -= error;
       }
    if( ++DifferentialCycleCount >= DifferentialCycle ) {
       DifferentialGain =  KDifferential * ( error - LastError );
       DifferentialCycleCount = 0;
       }
    LastError = error;
    Gain = ProportionalGain + IntegralGain + DifferentialGain;
    return Gain;
}
//End of File