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控制(控制对象是动态油缸)