pid_spd.asm

这是实现三相异步电动机FOC的DSP开发源代码

;============================================================
; 文件名:	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 ; 子程序调用
;		.ref	spd_fdb,spd_ref				 ; 输入
;		.ref	spd_out						 ; 输出
;------------------------------------------------------------
; 变量声明
;------------------------------------------------------------
		.def	pid_reg_spd,pid_reg_spd_init ; 子程序调用
		.def	spd_fdb,spd_ref				 ; 输入
		.def	spd_out						 ; 输出
;------------------------------------------------------------
; 输入输出变量定义
;------------------------------------------------------------

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	;转速误差
xin	    .usect "pid_spd",1	;速度积分
epiq	.usect "pid_spd",1	;q轴电流误差
epid	.usect "pid_spd",1	;d轴电流误差
xiq	    .usect "pid_spd",1	;q轴电流积分
xid	    .usect "pid_spd",1	;d轴电流积分
;------------------------------------------------------
*** 转矩电流限制
Isqrefmin	.set	-8000	;4.12 格式
Isqrefmax	.set	8000	;4.12 格式
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	#1fh,Kin	;积分时间设定
	splk	#3000h,Kpin	;比例增益设定
	splk	#14h,Kcorn	;饱和增益设定
	ret
;--------------------------------------------------------------------	
pid_reg_spd
;接口数据格式转换
	setc    SXM			;允许符号扩展 
	ldp		#spd_fdb
	lacc	spd_fdb 	;Q15, 6000rpm
	sfr 
	ldp		#n
	sacl	n			;Q12, 1500rpm

	ldp		#spd_ref	;Q15，1500rpm
	lacc	spd_ref
	sfr
	ldp 	#n_ref
	sacl	n_ref		;Q12，1500rpm
*****************************************************
* 带积分饱和矫正的速度调节
*****************************************************
	ldp		#n_ref
	lacc	n_ref
	sub		n
	sacl	epin			; epin=n_ref-n, 4.12 格式
	lacc	xin,12
	lt		epin
	mpy		Kpin
	apac
	sach	upi,4			; upi=xin+epin*Kpin, 4.12 format
			
	bit		upi,0			; 开始饱和判断
	bcnd	upimagzeros,NTC	; 如果 upi<0 则转到反向饱和判断
	lacc	#Isqrefmin		; 
	sub		upi
	bcnd	neg_sat,GT		; 如果 upi<ISqrefmin 则转到反向饱和处理
	lacc	upi				; 否则当前 upi 有效
	b	limiters
neg_sat
	lacc	#Isqrefmin		; set acc to -ve saturated value
	b	limiters
 
upimagzeros					; upi为正则判断是否正向饱和
	lacc	#Isqrefmax		; 
	sub	upi
	bcnd	pos_sat,LT		; 如果 upi>ISqrefmax 则转到正向饱和处理
	lacc	upi				; 否则 upi 有效
	b	limiters
pos_sat
	lacc	#Isqrefmax		; 用饱和值装载 acc 

limiters
	sacl	iSqref			; 将极限值设为参考值
	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 格式
	lt	elpi		;如果未饱和，则 elpi=0
	mpy	Kcorn
	pac
	lt	epin
	mpy	Kin
	apac
	add	xin,12
	sach	xin,4	;xin=xin+epin*Kin+elpi*Kcorn, 4.12 格式				
***********************************************************
* 带积分饱和矫正的速度调节程序结束
***********************************************************
	ret