void prpvtest::PID_Init(){
    cout << "PID_Init begin!" << endl;
    pid.SetSpeed = 0;
    pid.ActualSpeed = 0;
    pid.Err = 0;
    pid.Err_last = 0;
    pid.Err_Next = 0;
    pid.Kp = _Kpid[0];
    pid.Ki = _Kpid[1];
    pid.Kd = _Kpid[2];
    cout << pid.Kp << pid.Ki << pid.Kd << endl;
    cout << "PID_Init end!" << endl;
}

double prpvtest::PID_Calc(float speed,float realv){
    double u;
    double incrementSpeed;
    pid.SetSpeed = speed;
    pid.ActualSpeed = realv;
    pid.Err = pid.SetSpeed - pid.ActualSpeed;  
    u = pid.Kp*pid.Err;
    incrementSpeed = pid.Kp*(pid.Err - pid.Err_Next) + pid.Ki*pid.Err
        +pid.Kd * (pid.Err - 2 * pid.Err_Next + pid.Err_last);
    pid.Err_last = pid.Err_Next;
    pid.Err_Next = pid.Err;
    
    return incrementSpeed;
}

以上用C++代码实现了增量式PID控制（控制对象是动态油缸）