📄 pid_i.asm
字号:
;============================================================
; 文件名: pid_i.asm
;
; 模块名: pid_reg_id, pid_reg_iq
;
; 初始化程序名: pid_reg_id_init, pid_reg_iq_init
;
; 公司: 达盛科技
;
; 功能描述:带积分饱和矫正的d轴 和 q 轴PI电流调节器
;
; |~~~~~~~~~~~~|
; i_fdb o------>| |
; i_ref o------>| pid_reg |----->o u_out
; u_int o------>| |
; Kp | |
; Ki o------>|____________|
; Kc
;
; 目标板cpu: 'c2xx
;=====================================================================================
; 更改记录:
;-------------------------------------------------------------------------------------
; 最后更新时间:2005.6.15 版本号: Ver 1.0
;====================================================================
*************************************************************
* D轴 PI 电流调节器
*************************************************************
;------------------------------------------------------------
; 变量声明
;------------------------------------------------------------
; .ref pid_reg_id,pid_reg_id_init ; 函数调用
; .ref id_fdb,id_ref,Kp_d,Ki_d,Kc_d ; 输入参量
; .ref ud_int ; 输入
; .ref ud_out ; 输出
;------------------------------------------------------------
;变量声明
;------------------------------------------------------------
.def pid_reg_id,pid_reg_id_init ; 函数调用
.def id_fdb,id_ref,Kp_d,Ki_d,Kc_d ; 输入
.def ud_int ; 输入
.def ud_out ; 输出
.def id_error,uprsat_d
;------------------------------------------------------------
; 变量定义
;------------------------------------------------------------
id_fdb .usect "pid",1 ; 励磁电流反馈
id_ref .usect "pid",1 ; 励磁电流参考
ud_out .usect "pid",1 ; 控制电压输出
ud_int .usect "pid",1 ; 误差积分
uintlo_d .usect "pid",1
Kp_d .usect "pid",1 ; 比利增益
Ki_d .usect "pid",1 ; 积分增益
Kc_d .usect "pid",1 ; 积分校正增益
id_error .usect "pid",1 ; 励磁电流误差
uprsat_d .usect "pid",1 ; 防积分饱和控制电压
saterr_d .usect "pid",1 ; 饱和误差
tmp_d .usect "pid",1 ; 临时变量
;------------------------------------------------------------
; 常量设置
;------------------------------------------------------------
Kp_d_ .set 2400 ; Q11, 比例增益
Ki_d_ .set 500;1271;2000;; ; Q25, 积分增益
Kc_d_ .set 32767 ; Q14, 饱和校正增益
Umax_d_ .set 07000h ; 最大输出电压
Umin_d_ .set 08FFFh ; 最小输出电压
;------------------------------------------------------------
; 初始化
;------------------------------------------------------------
pid_reg_id_init
ldp #Kp_d ;
SPLK #Kp_d_,Kp_d ;
SPLK #Ki_d_,Ki_d ;
SPLK #Kc_d_,Kc_d ;
SPLK #0,ud_int ; zero integral term
SPLK #0,uintlo_d
RET
;------------------------------------------------------------
; 控制程序
;------------------------------------------------------------
pid_reg_id
setc SXM ; 允许符号扩展
setc OVM ; 设置溢出标志
ldp #id_ref
LACC id_ref,16 ; Use ACCH for OV protection
SUB id_fdb,16 ;
SACH id_error ; Q15 id_ref - id_fdb
lacl uintlo_d ;
add ud_int,16 ; 32-bit Q30
spm 2 ; product l/s 4 for accumulation
LT id_error ;
mpy Kp_d ; Q15*Q11 -> 32-bit Q26
apac ; 32-bit Q30 uint + id_error*Kp_d
SACH uprsat_d ; save as Q14 uprsat_d
sacl tmp_d ;
adds tmp_d ;
add uprsat_d,16 ; Q30 -> Q31 with OV protection
sach tmp_d ; save to tmp_d as Q15
bit tmp_d,0
bcnd U_gmind,ntc
lacc tmp_d ;
sub #Umin_d_ ;
bcnd U_lmaxd,GEQ ; Continue if tmp_d>=U_min
lacc #Umin_d_ ; otherwise, saturate
B Nextd
U_gmind
lacc tmp_d ;
sub #Umax_d_ ;
BCND U_lmaxd,LEQ ; Continue if tmp_d<=U_max
lacc #Umax_d_ ; otherwise, saturate
b Nextd
U_lmaxd
lacc tmp_d ;
Nextd
sacl ud_out ;
Int_termd
lacc ud_out,15 ; Use ACCH for OV protection
SUB uprsat_d,16 ;
sach saterr_d ; save as Q14 saterr_d = ud_out-uprsat_d
lt id_error ;
mpy Ki_d ; Q15*Q25 -> Q40
pac ; Q40 -> Q44
sach tmp_d ;
lacc tmp_d ; Q44 -> Q28 (r/s 16 bits)
LT saterr_d ;
MPY Kc_d ; Q14*Q14 -> Q28 saterr_d * Kc_d
APAC ; Q28 Ki_d*id_error + Kc_d*saterr_d
norm * ;
norm * ; Q28 -> Q30 (with OV protection)
ADDS uintlo_d
ADD ud_int,16 ; uint + saterr_d*Kc_d + id_error*Ki_d
SACH ud_int
SACL uintlo_d ; save as 32-bit Q30
spm 0
RET
***D轴 PI 电流调节器结束
*************************************************************
* Q轴 PI 电流调节器
*************************************************************
;------------------------------------------------------------
; 便量声明
;------------------------------------------------------------
; .ref pid_reg_iq,pid_reg_iq_init ; function call
; .ref iq_fdb,iq_ref,Kp_q,Ki_q,Kc_q ; Inputs
; .ref uq_int ; Input
; .ref uq_out ; Outputs
;------------------------------------------------------------
; 变量声明
;------------------------------------------------------------
.def pid_reg_iq,pid_reg_iq_init ; function call
.def iq_fdb,iq_ref,Kp_q,Ki_q,Kc_q ; Inputs
.def uq_int,Umax_q_ ; Input;ZYG
.def uq_out ; Outputs
;------------------------------------------------------------
; 变量定义
;------------------------------------------------------------
iq_fdb .usect "pid",1 ; current feedback
iq_ref .usect "pid",1 ; current reference
uq_out .usect "pid",1 ; control voltage output
;Umax_q_ .usect "pid",1 ;ZYG
uq_int .usect "pid",1 ; error integral
uintlo_q .usect "pid",1
Kp_q .usect "pid",1 ; proportional gain
Ki_q .usect "pid",1 ; integral gain
Kc_q .usect "pid",1 ; integral correction gain
iq_error .usect "pid",1 ; current error
uprsat_q .usect "pid",1 ; control voltage prio saturation
saterr_q .usect "pid",1 ; saturation error
tmp_q .usect "pid",1 ; temp scrach
;------------------------------------------------------------
; 参数设置
;------------------------------------------------------------
Kp_q_ .set 500;776 ; Q11, proportional gain
Ki_q_ .set 800;1907 ; Q25, integral gain
Kc_q_ .set 24576 ; Q14, saturation correction gain
Umax_q_ .set 06000h ; maximum U
Umin_q_ .set 09FFFh ; minimum U
;------------------------------------------------------------
; 初始化
;------------------------------------------------------------
pid_reg_iq_init
ldp #Kp_q ;
SPLK #Kp_q_,Kp_q ;
SPLK #Ki_q_,Ki_q ;
SPLK #Kc_q_,Kc_q ;
SPLK #0,uq_int ; zero integral term
SPLK #0,uintlo_q
SPLK #6000H,Umax_q_
RET
;------------------------------------------------------------
; 控制程序
;------------------------------------------------------------
pid_reg_iq
setc SXM ; Allow sign extension
setc OVM ; Set overflow protection mode
ldp #iq_ref ;
LACC iq_ref,16 ; Use ACCH for OV protection
SUB iq_fdb,16 ;
SACH iq_error ; Q15 iq_ref - iq_fdb
lacl uintlo_q ;
add uq_int,16 ; 32-bit Q30
spm 2 ; product l/s 4 before accumulation
LT iq_error ;
mpy Kp_q ; Q15*Q11 -> 32-bit Q26
apac ; 32-bit Q30 uint + iq_error*Kp_q
SACH uprsat_q ; save as Q14 uprsat_q
sacl tmp_q ;
adds tmp_q ;
add uprsat_q,16 ; Q30 -> Q31 with OV protection
sach tmp_q ; save to tmp_q as Q15
bit tmp_q,0
bcnd U_gminq,ntc
lacc tmp_q ;
sub #Umin_q_ ;
bcnd U_lmaxq,GEQ ; Continue if tmp_q>=U_min
lacc #Umin_q_ ; otherwise, saturate
B Nextq
U_gminq
lacc tmp_q ;
sub #Umax_q_ ;
BCND U_lmaxq,LEQ ; Continue if tmp_q<=U_max
lacc #Umax_q_ ; otherwise, saturate
b Nextq
U_lmaxq
lacc tmp_q ;
Nextq
sacl uq_out ;
Int_termq
lacc uq_out,15 ; Use ACCH for OV protection
SUB uprsat_q,16 ;
sach saterr_q ; save as Q14 saterr_q = uq_out-uprsat_q
lt iq_error ;
mpy Ki_q ; Q15*Q26 -> Q40
pac ; Q40 -> Q44
sach tmp_q ;
lacc tmp_q ; Q44 -> Q28 (r/s 16 bits)
LT saterr_q ;
MPY Kc_q ; Q14*Q14 -> Q28 saterr_q * Kc_q
APAC ; Q28 Ki_q*iq_error + Kc_q*saterr_q
norm * ;
norm * ; Q28 -> Q30 (with OV protection)
ADDS uintlo_q
ADD uq_int,16 ; uint + saterr_q*Kc_q + iq_error*Ki_q
SACH uq_int
SACL uintlo_q ; save as 32-bit Q30
spm 0
RET
***Q轴 PI 电流调节器结束⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -