📄 pid.asm
字号:
;=======================================================================
; 文件名: pid.asm
;
; 模块名: PID_REG3_ID, PID_REG3_IQ, PID_REG3_SPD
;
; 初始化程序名: PID_REG3_ID_INIT, PID_REG3_IQ_INIT, PID_REG3_SPD_INIT
;
; 公司: 达盛科技
;
; 功能: 三个带积分饱和校正功能的 PID 调节器
; i) d 轴电流调节器
; ii) q 轴电流调节器
; iii) 转速调节器
;
; |~~~~~~~~~~~~|
; fdb o------>| |
; ref o------>| |
; | pid_reg3 |----->o out
; Kp o------>| |
; Ki o------>| |
; Kd o------>| |
; Kc o------>|____________|
;
; 目标CPU: x2407
;=====================================================================================
; 更改纪录:
;-------------------------------------------------------------------------------------
; 最后更新时间：2005.08.10 版本号：Ver 1.0
;===========================================================================
*************************************************************
* D轴电流PID调节器
*************************************************************
;------------------------------------------------------------
; 变量声明
;------------------------------------------------------------
;(使用子程序就把下面的变量声明文件复制到主程序中)
; .ref PID_REG3_ID,PID_REG3_ID_INIT ; 子程序调用
; .ref id_fdb,id_ref ; 输入
; .ref ud_out ; 输出
; .ref Kp_d,Ki_d,Kd_d,Kc_d,ud_max,ud_min ; 参数
;------------------------------------------------------------
; 全局标号定义
;------------------------------------------------------------
.def PID_REG3_ID,PID_REG3_ID_INIT ; 子程序调用
.def id_fdb,id_ref ; 输入
.def ud_out ; 输出
.def Kp_d,Ki_d,Kd_d,Kc_d,ud_max,ud_min ; 参数
;------------------------------------------------------------
; 局部变量定义
;------------------------------------------------------------
id_fdb .usect "pid",1 ; 电流反馈
id_ref .usect "pid",1 ; 电流参考
ud_out .usect "pid",1 ; 控制电压输出
ud_max .usect "pid",1 ; 最大输出电压 (Q15)
ud_min .usect "pid",1 ; 最小输出电压 (Q15)
up_d .usect "pid",1 ; 比例误差 (Q14)
up1_d .usect "pid",1 ; 前一次比例误差 (Q14)
ui_hi_d .usect "pid",1 ; 积分误差 (Q30)
ui_lo_d .usect "pid",1
ud_hi_d .usect "pid",1 ; 微分误差 (Q30)
ud_lo_d .usect "pid",1
Kp_d .usect "pid",1 ; 比例增益
Ki_d .usect "pid",1 ; 积分增益
Kd_d .usect "pid",1 ; 微分增益
Kc_d .usect "pid",1 ; 积分饱和校正增益
e_d .usect "pid",1 ; 电流误差
uprsat_d .usect "pid",1 ; 饱和前控制电压
saterr_d .usect "pid",1 ; 饱和误差
;------------------------------------------------------------
; 参数设置
;------------------------------------------------------------
Kp_d_ .set 11178 ; Q15, 比例增益
Ki_d_ .set 8000;4194 ; Q31, 积分增益
Kd_d_ .set 0 ; Q14, 微分增益
Kc_d_ .set 20281 ; Q15, 饱和校正增益
Umax_d_ .set 05000h ; 输出电压上限
Umin_d_ .set 0b000h ; 输出电压下限
;------------------------------------------------------------
; 初始化
;------------------------------------------------------------
PID_REG3_ID_INIT
LDP #Kp_d
SPLK #Kp_d_,Kp_d ; 比例增益初始化 (Q15)
SPLK #Ki_d_,Ki_d ; 积分增益初始化 (Q31-16bit)截取位#23-#8
SPLK #Kd_d_,Kd_d ; 微分增益初始化 (Q14)
SPLK #Kc_d_,Kc_d ; 饱和校正增益初始化 (Q15)
SPLK Umax_d_,ud_max ; 最大输出电压初始化 (Q15)
SPLK Umin_d_,ud_min ; 最小输出电压初始化 (Q15)
SPLK #0,up1_d ; 比例误差初始化 (Q14)
SPLK #0,ui_hi_d ; 积分时间初始化 (Q30)
SPLK #0,ui_lo_d ; 积分时间初始化 (Q30)
SPLK #0,ud_hi_d ; 微分时间初始化 (Q30)
SPLK #0,ud_lo_d ; 微分时间初始化 (Q30)
RET
;------------------------------------------------------------
; 控制程序
;------------------------------------------------------------
PID_REG3_ID
SETC SXM ; 允许符号扩展
SETC OVM ; 设置溢出保护
SPM 0 ; 复位乘积移位模式
LDP #id_ref
; e(k) = ref(k)-fdb(k) => Q14 = Q15-Q15
LACC id_ref,15 ; ACC = ref (Q30)
SUB id_fdb,15 ; ACC = ref-fdb (Q30)
SACH e_d ; e = ref-fdb (Q14)
; up(k) = Kp*e(k) => Q14 = Q15*Q14
LT Kp_d ; TREG = Kp (Q15)
MPY e_d ; PREG = Kp*e (Q29)
PAC ; ACC = Kp*e (Q29)
SACH up_d,1 ; up = Kp*e (Q14)
; uprsat(k) = up(k)+ui(k-1)+ud(k-1) => Q14 = Q14+Q30+Q30
LACC ui_hi_d,16 ; ACC = ui (Q30)
ADDS ui_lo_d ; ACC = ui (Q30)
ADDS ud_lo_d ; ACC = ui+ud (Q30)
ADDH ud_hi_d ; ACC = ui+ud (Q30)
ADDH up_d ; ACC = up+ui+ud (Q30)
SACH uprsat_d ; uprsat = up+ui+ud (Q14)
; 检查是否饱和
LACC uprsat_d,16 ; ACC = uprsat (Q14)
SUB ud_max,15 ; ACC = uprsat-umax (Q14)
BCND SAT_MAX_D,GT ; 转到 SAT_MAX，当 uprsat > umax
LACC uprsat_d,16 ; ACC = uprsat (Q14)
SUB ud_min,15 ; ACC = uprsat-umin (Q14)
BCND SAT_MIN_D,LT ; 转到 SAT_MIN，当 uprsat < umin
LACC uprsat_d,16 ; ACC = uprsat (Q30)
SACH ud_out,1 ; uout = uprsat (Q15)
B UPDATE_D
SAT_MAX_D
LACC ud_max ; ACC = umax (Q15)
SACL ud_out ; uout = umax (Q15)
B UPDATE_D
SAT_MIN_D
LACC ud_min ; ACC = umin (Q15)
SACL ud_out ; uout = umin (Q15)
UPDATE_D
LACC ud_out,15 ; ACC = uout (Q30)
SUB uprsat_d,16 ; ACC = uout-uprsat (Q30)
SACH saterr_d ; 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_d ; TREG = Ki (Q31-16bit)
MPY up_d ; PREG = Ki*up (Q38)
PAC ; ACC = Ki*up (Q32)
SFR ; ACC = Ki*up (Q31)
SFR ; ACC = Ki*up (Q30)
SPM 1 ; 设置乘积移位1
LT Kc_d ; TREG = Kc (Q15)
MPY saterr_d ; PREG = Kc*(uout-uprsat) (Q29)
APAC ; ACC = Ki*up+Kc*(uout-uprsat) (Q30)
ADDS ui_lo_d ; ACC = ui+Ki*up+Kc*(uout-uprsat) (Q30)
ADDH ui_hi_d ; ACC = ui+Ki*up+Kc*(uout-uprsat) (Q30)
SACL ui_lo_d ; ui = ui+Ki*up+Kc*(uout-uprsat) (Q30)
SACH ui_hi_d ; ui = ui+Ki*up+Kc*(uout-uprsat) (Q30)
; ud(k) = Kd*up(k)-Kd*up(k-1) => Q30 = Q14*Q14-Q14*Q14
LT Kd_d ; TREG = Kd (Q14)
MPY up_d ; PREG = Kd*up (Q28)
PAC ; ACC = Kd*up (Q29)
MPY up1_d ; PREG = Kd*up1 (Q28)
SPAC ; ACC = Kd*up-Kd*up1 (Q29)
SACL ud_lo_d,1 ; ud = Kd*up-Kd*up1 (Q30)
SACH ud_hi_d,1 ; ud = Kd*up-Kd*up1 (Q30)
; 刷新 up
LACC up_d ; ACC = up (Q14)
SACL up1_d ; up1 = up (Q14)
spm 0
RET
*** D轴 PID 电流调节器程序代码结束
*************************************************************
* Q轴 PID 电流调节器
*************************************************************
;------------------------------------------------------------
;(使用子程序就把下面的变量声明文件复制到主程序中)
; .ref PID_REG3_IQ,PID_REG3_IQ_INIT ; 子程序调用
; .ref iq_fdb,iq_ref ; 输入
; .ref uq_out ; 输出
; .ref Kp_q,Ki_q,Kd_q,Kc_q,uq_max,uq_min ; 参数
;------------------------------------------------------------
; 标号定义
;------------------------------------------------------------
;变量定义.
.def PID_REG3_IQ,PID_REG3_IQ_INIT ; 子程序调用
.def iq_fdb,iq_ref ; 输入
.def uq_out ; 输出
.def Kp_q,Ki_q,Kd_q,Kc_q,uq_max,uq_min ; 参数
;------------------------------------------------------------
; 变量定义
;------------------------------------------------------------
iq_fdb .usect "pid",1 ; 电流反馈
iq_ref .usect "pid",1 ; 电流参考
uq_out .usect "pid",1 ; 控制电压输出
uq_max .usect "pid",1 ; 最大输出电压 (Q15)
uq_min .usect "pid",1 ; 最小输出电压 (Q15)
up_q .usect "pid",1 ; 比例误差 (Q14)
up1_q .usect "pid",1 ; 前一时刻比例误差(Q14)
ui_hi_q .usect "pid",1 ; 积分误差 (Q30)
ui_lo_q .usect "pid",1
ud_hi_q .usect "pid",1 ; 微分误差 (Q30)
ud_lo_q .usect "pid",1
Kp_q .usect "pid",1 ; 比例增益
Ki_q .usect "pid",1 ; 积分增益
Kd_q .usect "pid",1 ; 微分增益
Kc_q .usect "pid",1 ; 积分饱和校正增益
e_q .usect "pid",1 ; 电流误差
uprsat_q .usect "pid",1 ; 饱和前控制电压
saterr_q .usect "pid",1 ; 饱和误差
;------------------------------------------------------------
; PID参数
;------------------------------------------------------------
Kp_q_ .set 11178 ; Q15, 比例增益
Ki_q_ .set 8000;4194 ; Q31, 积分增益
Kd_q_ .set 0 ; Q14, 微分增益
Kc_q_ .set 20281 ; Q15, 饱和校正增益
Umax_q_ .set 06700h ; 最大电压
Umin_q_ .set 09900h ; 最小电压
;------------------------------------------------------------
; 初始化程序
;------------------------------------------------------------
PID_REG3_IQ_INIT
LDP #Kp_q
SPLK #Kp_q_,Kp_q ; 比例增益初始化 (Q15)
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -