📄 pid.asm
字号:
SPLK #Ki_q_,Ki_q ; 积分增益初始化 (Q31-16bit) 截取位#23-#8
SPLK #Kd_q_,Kd_q ; 微分增益初始化 (Q14)
SPLK #Kc_q_,Kc_q ; 饱和校正增益初始化 (Q15)
SPLK Umax_q_,uq_max ; 最大输出电压初始化 (Q15)
SPLK Umin_q_,uq_min ; 最小输出电压初始化 (Q15)
SPLK #0,up1_q ; 比例增益初始化 (Q14)
SPLK #0,ui_hi_q ; 积分时间初始化 (Q30)
SPLK #0,ui_lo_q ; 积分时间初始化 (Q30)
SPLK #0,ud_hi_q ; 微分时间初始化 (Q30)
SPLK #0,ud_lo_q ; 微分时间初始化 (Q30)
RET
;------------------------------------------------------------
; 控制程序
;------------------------------------------------------------
PID_REG3_IQ
SETC SXM ; 允许符号扩展
SETC OVM ; 设置溢出保护
SPM 0 ; 复位乘积移位
LDP #iq_ref
; e(k) = ref(k)-fdb(k) => Q14 = Q15-Q15
LACC iq_ref,15 ; ACC = ref (Q30)
SUB iq_fdb,15 ; ACC = ref-fdb (Q30)
SACH e_q ; e = ref-fdb (Q14)
; up(k) = Kp*e(k) => Q14 = Q15*Q14
LT Kp_q ; TREG = Kp (Q15)
MPY e_q ; PREG = Kp*e (Q29)
PAC ; ACC = Kp*e (Q29)
SACH up_q,1 ; up = Kp*e (Q14)
; uprsat(k) = up(k)+ui(k-1)+ud(k-1) => Q14 = Q14+Q30+Q30
LACC ui_hi_q,16 ; ACC = ui (Q30)
ADDS ui_lo_q ; ACC = ui (Q30)
ADDS ud_lo_q ; ACC = ui+ud (Q30)
ADDH ud_hi_q ; ACC = ui+ud (Q30)
ADDH up_q ; ACC = up+ui+ud (Q30)
SACH uprsat_q ; uprsat = up+ui+ud (Q14)
; 检查是否饱和
LACC uprsat_q,16 ; ACC = uprsat (Q14)
SUB uq_max,15 ; ACC = uprsat-umax (Q14)
BCND SAT_MAX_Q,GT ; 转到 SAT_MAX，当 uprsat > umax
LACC uprsat_q,16 ; ACC = uprsat (Q14)
SUB uq_min,15 ; ACC = uprsat-umin (Q14)
BCND SAT_MIN_Q,LT ; 转到 SAT_MIN，当 uprsat < umin
LACC uprsat_q,16 ; ACC = uprsat (Q30)
SACH uq_out,1 ; uout = uprsat (Q15)
B UPDATE_Q
SAT_MAX_Q
LACC uq_max ; ACC = umax (Q15)
SACL uq_out ; uout = umax (Q15)
B UPDATE_D
SAT_MIN_Q
LACC uq_min ; ACC = umin (Q15)
SACL uq_out ; uout = umin (Q15)
UPDATE_Q
LACC uq_out,15 ; ACC = uout (Q30)
SUB uprsat_q,16 ; ACC = uout-uprsat (Q30)
SACH saterr_q ; saterr = uout-uprsat (Q14)
; ui(k) = ui(k-1)+Ki*up(k)+Kc*(uout-uprsat) => Q30 = Q30+Q31*Q14+Q15*Q14
SPM 3 ; 设置乘积移位6
LT Ki_q ; TREG = Ki (Q31-16bit)
MPY up_q ; PREG = Ki*up (Q38)
PAC ; ACC = Ki*up (Q32)
SFR ; ACC = Ki*up (Q31)
SFR ; ACC = Ki*up (Q30)
SPM 1 ; 设置乘积移位1
LT Kc_q ; TREG = Kc (Q15)
MPY saterr_q ; PREG = Kc*(uout-uprsat) (Q29)
APAC ; ACC = Ki*up+Kc*(uout-uprsat) (Q30)
ADDS ui_lo_q ; ACC = ui+Ki*up+Kc*(uout-uprsat) (Q30)
ADDH ui_hi_q ; ACC = ui+Ki*up+Kc*(uout-uprsat) (Q30)
SACL ui_lo_q ; ui = ui+Ki*up+Kc*(uout-uprsat) (Q30)
SACH ui_hi_q ; ui = ui+Ki*up+Kc*(uout-uprsat) (Q30)
; ud(k) = Kd*up(k)-Kd*up(k-1) => Q30 = Q14*Q14-Q14*Q14
LT Kd_q ; TREG = Kd (Q14)
MPY up_q ; PREG = Kd*up (Q28)
PAC ; ACC = Kd*up (Q29)
MPY up1_q ; PREG = Kd*up1 (Q28)
SPAC ; ACC = Kd*up-Kd*up1 (Q29)
SACL ud_lo_q,1 ; ud = Kd*up-Kd*up1 (Q30)
SACH ud_hi_q,1 ; ud = Kd*up-Kd*up1 (Q30)
; Update up
LACC up_q ; ACC = up (Q14)
SACL up1_q ; up1 = up (Q14)
spm 0
RET
*** Q轴 PID 电流调节器程序代码结束
*************************************************************
* PID 转速调节器
*************************************************************
;------------------------------------------------------------
;(使用子程序就把下面的变量声明文件复制到主程序中)
; .ref PID_REG3_SPD,PID_REG3_SPD_INIT ; 子程序调用
; .ref spd_fdb,spd_ref ; 输入
; .ref t_out ; 输出
; .ref Kp_spd,Ki_spd,Kd_spd,Kc_spd,t_max,t_min ; 参数
;------------------------------------------------------------
; 标号定义
;------------------------------------------------------------
.def PID_REG3_SPD,PID_REG3_SPD_INIT ; 子程序调用
.def spd_fdb,spd_ref ; 输入
.def t_out ; 输出
.def Kp_spd,Ki_spd,Kd_spd,Kc_spd,t_max,t_min ; 参数
;------------------------------------------------------------
; 变量定义
;------------------------------------------------------------
spd_fdb .usect "pid",1 ; 速度反馈
spd_ref .usect "pid",1 ; 速度参考
t_out .usect "pid",1 ; 控制输出
t_max .usect "pid",1 ; 最大输出 (Q15)
t_min .usect "pid",1 ; 最小输出 (Q15)
up_spd .usect "pid",1 ; 比例误差 (Q14)
up1_spd .usect "pid",1 ; 前一时刻比例误差 (Q14)
ui_hi_spd .usect "pid",1 ; 积分误差 (Q30)
ui_lo_spd .usect "pid",1
ud_hi_spd .usect "pid",1 ; 微分误差 (Q30)
ud_lo_spd .usect "pid",1
Kp_spd .usect "pid",1 ; 比例增益
Ki_spd .usect "pid",1 ; 积分增益
Kd_spd .usect "pid",1 ; 微分增益
Kc_spd .usect "pid",1 ; 积分饱和校正增益
e_spd .usect "pid",1 ; 转速误差
uprsat_spd .usect "pid",1 ; 饱和前控制输出
saterr_spd .usect "pid",1 ; 饱和误差
;------------------------------------------------------------
; PID参数设定
;------------------------------------------------------------
Kp_spd_ .set 18312 ; Q15, 比例增益
Ki_spd_ .set 4149 ; Q31, 积分增益
Kd_spd_ .set 0 ; Q14, 微分增益
Kc_spd_ .set 31858 ; Q15, 饱和校正增益
Umax_spd_ .set 07FFFh ; 最大电压
Umin_spd_ .set 08000h ; 最小电压
;------------------------------------------------------------
; 初始化程序
;------------------------------------------------------------
PID_REG3_SPD_INIT
LDP #Kp_spd
SPLK #Kp_spd_,Kp_spd ; 比例增益初始化 (Q15)
SPLK #Ki_spd_,Ki_spd ; 积分增益初始化 (Q31-16bit) 截取位#23-#8
SPLK #Kd_spd_,Kd_spd ; 微分增益初始化 (Q14)
SPLK #Kc_spd_,Kc_spd ; 饱和增益初始化 (Q15)
SPLK Umax_spd_,t_max ; 最大输出初始化 (Q15)
SPLK Umin_spd_,t_min ; 最小输出初始化 (Q15)
SPLK #0,up1_spd ; 比例误差初始化 (Q14)
SPLK #0,ui_hi_spd ; 积分时间初始化 (Q30)
SPLK #0,ui_lo_spd ; 积分时间初始化 (Q30)
SPLK #0,ud_hi_spd ; 微分时间初始化 (Q30)
SPLK #0,ud_lo_spd ; 微分时间初始化 (Q30)
RET
;------------------------------------------------------------
; 控制程序
;------------------------------------------------------------
PID_REG3_SPD
SETC SXM ; 允许符号扩展
SETC OVM ; 设置溢出保护
SPM 0 ; 复位乘积移位模式
LDP #spd_ref
; e(k) = ref(k)-fdb(k) => Q14 = Q15-Q15
LACC spd_ref,15 ; ACC = ref (Q30)
SUB spd_fdb,15 ; ACC = ref-fdb (Q30)
SACH e_spd ; e = ref-fdb (Q14)
; up(k) = Kp*e(k) => Q14 = Q15*Q14
LT Kp_spd ; TREG = Kp (Q15)
MPY e_spd ; PREG = Kp*e (Q29)
PAC ; ACC = Kp*e (Q29)
SACH up_spd,1 ; up = Kp*e (Q14)
; uprsat(k) = up(k)+ui(k-1)+ud(k-1) => Q14 = Q14+Q30+Q30
LACC ui_hi_spd,16 ; ACC = ui (Q30)
ADDS ui_lo_spd ; ACC = ui (Q30)
ADDS ud_lo_spd ; ACC = ui+ud (Q30)
ADDH ud_hi_spd ; ACC = ui+ud (Q30)
ADDH up_spd ; ACC = up+ui+ud (Q30)
SACH uprsat_spd ; uprsat = up+ui+ud (Q14)
; Check uprsat is saturated ?
LACC uprsat_spd,16 ; ACC = uprsat (Q14)
SUB t_max,15 ; ACC = uprsat-umax (Q14)
BCND SAT_MAX_SPD,GT ; 转到 SAT_MAX，当 uprsat > umax
LACC uprsat_spd,16 ; ACC = uprsat (Q14)
SUB t_min,15 ; ACC = uprsat-umin (Q14)
BCND SAT_MIN_SPD,LT ; 转到 SAT_MIN，当 uprsat < umin
LACC uprsat_spd,16 ; ACC = uprsat (Q30)
SACH t_out,1 ; uout = uprsat (Q15)
B UPDATE_SPD
SAT_MAX_SPD
LACC t_max ; ACC = umax (Q15)
SACL t_out ; uout = umax (Q15)
B UPDATE_SPD
SAT_MIN_SPD
LACC t_min ; ACC = umin (Q15)
SACL t_out ; uout = umin (Q15)
UPDATE_SPD
LACC t_out,15 ; ACC = uout (Q30)
SUB uprsat_spd,16 ; ACC = uout-uprsat (Q30)
SACH saterr_spd ; saterr = uout-uprsat(Q14)
; ui(k) = ui(k-1)+Ki*up(k)+Kc*(uout-uprsat) => Q30 = Q30+Q31*Q14+Q15*Q14
SPM 3 ; 设置乘积移位6
LT Ki_spd ; TREG = Ki (Q31-16bit)
MPY up_spd ; PREG = Ki*up (Q38)
PAC ; ACC = Ki*up (Q32)
SFR ; ACC = Ki*up (Q31)
SFR ; ACC = Ki*up (Q30)
SPM 1 ; Set left shifted 1 bit
LT Kc_spd ; TREG = Kc (Q15)
MPY saterr_spd ; PREG = Kc*(uout-uprsat) (Q29)
APAC ; ACC = Ki*up+Kc*(uout-uprsat) (Q30)
ADDS ui_lo_spd ; ACC = ui+Ki*up+Kc*(uout-uprsat) (Q30)
ADDH ui_hi_spd ; ACC = ui+Ki*up+Kc*(uout-uprsat) (Q30)
SACL ui_lo_spd ; ui = ui+Ki*up+Kc*(uout-uprsat) (Q30)
SACH ui_hi_spd ; ui = ui+Ki*up+Kc*(uout-uprsat) (Q30)
; ud(k) = Kd*up(k)-Kd*up(k-1) => Q30 = Q14*Q14-Q14*Q14
LT Kd_spd ; TREG = Kd (Q14)
MPY up_spd ; PREG = Kd*up (Q28)
PAC ; ACC = Kd*up (Q29)
MPY up1_spd ; PREG = Kd*up1 (Q28)
SPAC ; ACC = Kd*up-Kd*up1(Q29)
SACL ud_lo_spd,1 ; ud = Kd*up-Kd*up1 (Q30)
SACH ud_hi_spd,1 ; ud = Kd*up-Kd*up1 (Q30)
; Update up
LACC up_spd ; ACC = up (Q14)
SACL up1_spd ; up1 = up (Q14)
SPM 0
RET
*** PID 速度调节器代码结束
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -