previous02_error := 0 //上上次偏差
previous01_error := 0 //上一次偏差
integral := 0 //积分和
pid_out := 0 //pid增量累加和
//循环
//采样周期为dt
loop:
//setpoint 设定值
//measured_value 反馈值
error := setpoint − measured_value //计算得到偏差
proportion := error - previous01_error //计算得到比例输出
integral := error × dt //计算得到积分累加和
derivative := (error − 2*previous01_error + previous02_error) / dt //计算得到微分
pid_delta := Kp × error + Ki × integral + Kd × derivative //计算得到PID增量
pid_out := pid_out + pid_delta //计算得到PID输出
//保存当前的偏差和上一次偏差作为下一次采样所需要的历史偏差
previous02_error := previous01_error
previous01_error := error //保存当前偏差为下一次采样时所需要的历史偏差
wait(dt) //等待下一次采用
goto loop
|
楼主
