位置式PID控制算法模型（数字）

float T, Kp, Ti, Td;    //const
float A, B, C;

void calcConst()
{
    A = Kp * (1 + T / Ti + Td / T);
    B = -Kp * (1 + 2 * Td / T);
    C = Kp * Td / T;
}

float PID(float rt, float yt)
{
    static float e1 = 0.0f, e0 = 0.0f, uk_pre;
    float e2, deltaUk;

    e2 = rt - yt;
    deltaUk = A * e2 + B * e1 + C * e0;
    e0 = e1;
    e1 = e2;
    uk_pre += deltaUk;

    return uk_pre;
}