pid_spd.asm

这是关于有刷直流电动机PWM控制方案的源码

;============================================================
; 文件名:	pid_spd.asm
;
; 模块名:	pid_reg_spd
;
; 初始化程序名: pid_reg_spd_init
;
; 公司:	达盛科技
;
; 功能描述:带积分饱和矫正的PI速度调节器
;		    
;		          |~~~~~~~~~~~~~~~|
; spd_fdb o------>|	              |
; spd_ref o------>|  pid_reg_spd  |----->o  spd_out
;                 |	              |
;   Kpin          |               |
;   Kin   o------>|_______________|
;   Kcorn
;
; 目标板cpu: 'c2xx 
;=====================================================================================
; 更改记录:
;-------------------------------------------------------------------------------------
; 最后更新时间:2005.6.15	版本号:	Ver 1.0
;====================================================================
*************************************************************
* 速度 PI 调节器
*************************************************************
;		.ref	pid_reg_spd,pid_reg_spd_init ; function call
;		.ref	spd_fdb,spd_ref		; Inputs
;		.ref	spd_out				; Outputs
;------------------------------------------------------------
; 变量声明
;------------------------------------------------------------
		.def	pid_reg_spd,pid_reg_spd_init ; function call
		.def	spd_fdb,spd_ref		; Inputs
		.def	spd_out				; Outputs
;------------------------------------------------------------
; 输入输出变量定义
;------------------------------------------------------------

spd_ref	.usect "pid_spd",1
spd_fdb	.usect "pid_spd",1
spd_out	.usect "pid_spd",1      

* PI 调节内部变量定义
upi	    .usect "pid_spd",1	;PI 调节器输出缓存
elpi	.usect "pid_spd",1	;PI 调节器饱和误差
Kcorn	.usect "pid_spd",1	;抗饱和增益系数
Ki    	.usect "pid_spd",1
Kcor	.usect "pid_spd",1
Kpin	.usect "pid_spd",1
Kin  	.usect "pid_spd",1
Kpi	    .usect "pid_spd",1	
epin	.usect "pid_spd",1	;speed error (used in speed 		;regulator)
xin	    .usect "pid_spd",1	;speed regulator integral 			;component
epiq	.usect "pid_spd",1	;q-axis current regulator error
epid	.usect "pid_spd",1	;d-axis current regulator error
xiq	    .usect "pid_spd",1	;q-axis current regulator integral 	;component
xid	    .usect "pid_spd",1	;d-axis current regulator integral 	;component

*** 转矩电流限制
Isqrefmin	.set	-5000	;4.12 format=-0.8 pu, I_nom=1A
Isqrefmax	.set	5000	;4.12 format=0.8 pu, I_nom=1A
vSqref	.usect "pid_spd",1
iSqref	.usect "pid_spd",1
iSd		.usect "pid_spd",1
iSq		.usect "pid_spd",1
n		.usect "pid_spd",1	
n_ref	.usect "pid_spd",1	
vSdref	.usect "pid_spd",1
iSdref	.usect "pid_spd",1


pid_reg_spd_init
	zac
	ldp		#spd_ref	
	sacl	xin
	sacl	upi
	sacl	elpi
	sacl    spd_out   
	sacl    upi
	sacl    elpi
    sacl    epin
    sacl    epiq
    sacl    epid	
    sacl    xiq
    sacl    xid	
    sacl    vSqref	
    sacl    iSqref
    sacl    iSd
    sacl    iSq
    sacl    n
    sacl    n_ref
    sacl    vSdref
    sacl    iSdref

*** 速度PI 参数
	splk	#100,Kin	;35H,1fh
	splk	#400,Kpin	;482bH,2800h
	splk	#14h,Kcorn;0BH
	ret
	
pid_reg_spd
;Interface avec les nouvelles variables
	setc    SXM	;ZYG 
	ldp		#spd_fdb
	lacc	spd_fdb ;Q15
	sfr 
	ldp		#n
	sacl	n		;Q12

	ldp		#spd_ref	;Q15
	lacc	spd_ref
	sfr
	ldp 	#n_ref
	sacl	n_ref		;Q12 
*****************************************************
* 带积分饱和矫正的速度调节
*****************************************************
	ldp		#n_ref
	lacc	n_ref
	sub		n
	sacl	epin	;epin=n_ref-n, 4.12 format
	lacc	xin,12
	lt		epin
	mpy		Kpin
	apac
	sach	upi,4	;upi=xin+epin*Kpin, 4.12 format
			;here we start to saturate
	bit		upi,0
	bcnd	upimagzeros,NTC	;If value >0 we branch
	lacc	#Isqrefmin	;negative saturation
	sub		upi
	bcnd	neg_sat,GT	;if upi<ISqrefmin then branch to 
			;saturate
	lacc	upi	;value of upi is valid
	b	limiters
neg_sat
	lacc	#Isqrefmin	;set acc to -ve saturated value
	b	limiters
 
upimagzeros	;Value is positive
	lacc	#Isqrefmax	;positive saturation
	sub	upi
	bcnd	pos_sat,LT	;if upi>ISqrefmax then branch to 				
	;saturate
	lacc	upi	;value of upi valid
	b	limiters
pos_sat
	lacc	#Isqrefmax	;set acc to +ve saturated value

limiters
	sacl	iSqref	;Store the acc as reference value
;Interface avec la nouvelle variable de sortie
	setc	OVM
	sfl
	sfl
	;sfl
	ldp		#spd_out
	sacl	spd_out
	;sub  #7fffh
	clrc	OVM
	ldp		#iSqref
	lacc	iSqref
;
	sub	upi
	sacl	elpi	;elpi=iSqref-upi, 4.12 format
	lt	elpi		;if there is no saturation elpi=0
	mpy	Kcorn
	pac
	lt	epin
	mpy	Kin
	apac
	add	xin,12
	sach	xin,4	;xin=xin+epin*Kin+elpi*Kcorn, 4.12 				
					;format
***********************************************************
* 带积分饱和矫正的速度调节程序结束
***********************************************************
	ret