Mính đã tìm hiểu cách viết dạng Matlab hay C từ thuật toán PID là thế này rồi.

Simple PID
PID = GainP * actual error + GainI * SUM(previous errors) + GainD * (actual error - last error)
error = sp(set point) - pv(process value)
float pid (float sp, float pv)
{
err_old = err;
err = sp - pv;
// note
P_err = err;
I_err += err_old;
D_err = err - err_old;
return 0.1*P_err + 0.3*I_err + 0.02*D_err;
}
//note. Dead zone example: if (err<2 && err>-2){err=0;}

Tuy nhiên còn nhiều cách viết khác để lựa chọn cho phù hợp. Còn lại mình kết hợp tìm thuật toán PID + Mờ để viết là ok.