cnc-club.ru

Код: Выделить всё

bool PIDv2::Compute(unsigned int now)
{
    if(!inAuto)
        return false;
    if((now - lastTime)>= SampleTime)
    {
        lastTime = now;

        /*Compute all the working error variables*/
        double input = *myInput;
        double p_error = *mySetpoint - input;

        /* if output is in limit, don't let integrator wind up */
        if(!isClipping)
            i_error += ki * p_error;

        if(i_error != 0.0)
        {
          if(i_error > i_limit)
            i_error = i_limit;
          else if(i_error < -i_limit)
            i_error = -i_limit;
        }
        else
        {
          if(i_error > outMax)
              i_error= outMax;
          else if(i_error < outMin)
              i_error= outMin;
        }

        double d_error = (p_error - lastError);
        lastError = p_error;

        /*Compute PID Output*/
        double output = kp * p_error + i_error + kd * d_error;

        if(output > outMax)
        {
            isClipping = true;
            output = outMax;
        }
        else if(output < outMin)
        {
            isClipping = true;
            output = outMin;
        }
        else
            isClipping = false;

        *myOutput = output;

        return true;
    }
    else
        return false;
}