PID： LDP #DP_B01 ;数据页4——0200h～0280h
SETC SXM ;允许符号扩展
LACL VREF ;Vref，Q.15*256
SUB V ;V，Q.15*256
SACL VE2 ;e(k)= Vref－V，Q15*256
SUB VE1 ;e(k-1)，Q15*256
SACL PIDTMP1 ;Delta(e(k))=e(k)-e(k－1)，Q15/256
SUB VE1
ADD VE0 ;e(k－2)，Q15*256
SACL PIDTMP3 ;Delta(e(k)) －Delta(e(k－1))= e(k)-2e(k－1)+e(k－2)
;Q15*256
LT KP ;Kp，Q15*32
MPY PIDTMP1
PAC ;Kp*Delta(e(k))，Q30*8192
RPT #4
SFL ;Kp*Delta(e(k))，Q30*256
SACH PIDTMP1，1 ;Kp*Delta(e(k))，Q15*256
LT KI ;Ki，Q15*32
MPY VE2
PAC ;Ki*e(k)，Q30*8192
RPT #4
SFL ;Ki*e(k)，Q30*256
SACH PIDTMP2，1 ;Ki*e(k)，Q15*256
LT KD ;Kd，Q15*32
MPY PIDTMP3
PAC ;Kd* Delta(e(k))-Delta(e(k-1))，Q30*8192
RPT #4
SFL ;Kd* Delta(e(k))-Delta(e(k-1))，Q30*256
SACH PIDTMP3，1 ;Kd* Delta(e(k))-Delta(e(k-1))，Q15*256
LACL U ;u(k－1)，Q15*256
ADD PIDTMP1
