cur_mod.asm

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

;===========================================================================
; 文件名:	Cur_mod.asm
; 模块名:	CURRENT_MODEL	
; 初始化程序名: CURRENT_MODEL_INIT
; 公司:	达盛科技
; 功能描述:	三相异步电动机转子电流磁链计算模型
;
;		    	|~~~~~~~~~~~~~~~~~|
; i_cur_mod_d ->|		          |
; i_cur_mod_q ->|  CURRENT_MODEL  |----->  theta_cur_mod
; 				|		          |
; spd_cur_mod-->|                 |
; 				|_________________|
;
;
; 目标板cpu:		x2407
;
;=====================================================================================
; 修改记录:
;-------------------------------------------------------------------------------------
; 最终修改日期:2005.4.5 	版本号:	Ver 1.0
;------------------------------------------------------------
; 变量声明
;------------------------------------------------------------
;			.ref	CURRENT_MODEL,CURRENT_MODEL_INIT ;子程序
;			.ref	i_cur_mod_D,i_cur_mod_Q			 ;输入  
;			.ref	spd_cur_mod						 ;输入
;			.ref	theta_cur_mod					 ;输出
;------------------------------------------------------------
; 标号定义
;------------------------------------------------------------
			.def	CURRENT_MODEL,CURRENT_MODEL_INIT ;子程序
			.def	i_cur_mod_D,i_cur_mod_Q			 ;输入
			.def	spd_cur_mod						 ;输入
			.def	theta_cur_mod					 ;输出
			.def    fs								 ;输出

;------------------------------------------------------------
; 变量定义
;------------------------------------------------------------
;全局变量              
i_cur_mod_D		.usect "cur_mod",1	;定子电流d轴分量	
i_cur_mod_Q 	.usect "cur_mod",1	;定子电流q轴分量
spd_cur_mod 	.usect "cur_mod",1	;	                     
theta_cur_mod 	.usect "cur_mod",1	;转子磁链位置


;局部变量
iSd			.usect "cur_mod",1		;定子电流励磁分量
iSq			.usect "cur_mod",1		;定子电流转矩分量
n			.usect "cur_mod",1		;转子机械转速
Teta_cm 	.usect "cur_mod",1		;转子磁链位置
i_mr		.usect "cur_mod",1		;励磁电流
fs      	.usect "cur_mod",1		;转子磁链速度
tetaincr    .usect "cur_mod",1		;electrical angle variation within  
Kr          .usect "cur_mod",1      ;Q15
Kt          .usect "cur_mod",1      ;Q12
K           .usect "cur_mod",1      ;Q0
myK         .usect "cur_mod",1
my_fs       .usect "cur_mod",1  
p           .usect "cur_mod",1
									;一个采样周期
tmp	        .usect "cur_mod",1		;临时变量
tmp1        .usect "cur_mod",1		;临时变量1
Teta_cm1	.usect "cur_mod",1		;vizualisation variable

;------------------------------------------------------------
; 常量输入
;------------------------------------------------------------   
Pole_pairs_number   .set    2		;极对数

; 电动机参数

Kr_	.set	42;0eh   ;通过转子参数计算获得
Kt_	.set	1296;1b0h  
K_	.set	148h

;------------------------------------------------------------
; 初始化
;------------------------------------------------------------
CURRENT_MODEL_INIT
		
		ldp		#n
		splk	#0000h,n   			
		splk	#0000h,i_mr
		splk    #Kr_,Kr
		splk    #Kt_,Kt
		splk    #K_,K  
		splk    #3FFFh,theta_cur_mod 
		splk    #0000h,i_cur_mod_D
		splk    #0000h,i_cur_mod_Q 
		splk    #0000h,spd_cur_mod 
		splk    #0000h,iSd
		splk    #0000h,iSq
		splk    #0000h,Teta_cm
		splk    #0000h,fs
		splk    #0000h,tetaincr
		splk    #0000h,tmp
		splk    #0000h,tmp1
		splk    #0000h,Teta_cm1 
		splk    #Pole_pairs_number,p
		ret        
		
; Current model with i_mr on 16 bit
	
CURRENT_MODEL
; 将输入变量格式从 Q15 转变成 Q12 
	ldp		#i_cur_mod_D
	lacc	i_cur_mod_D
	sfr
	sfr
	sfr     
	ldp		#iSd
	sacl	iSd

	ldp		#i_cur_mod_Q
	lacc	i_cur_mod_Q
	sfr
	sfr
	sfr     
	ldp		#iSq
	sacl	iSq

	ldp		#spd_cur_mod
	lacc	spd_cur_mod ; Q15, 6000rpm 
    sfr                 ; 根据额定转速确定移位
	ldp		#n
	sacl	n		    ; Q12, 1500rpm 
***********************************************
* 电流磁链模型开始
***********************************************
	ldp		#iSd
	lacc	iSd
	sub		i_mr
	sacl	tmp
	lt		tmp
	mpy		#Kr
	pac
	sach	tmp,1
	lacc	tmp
	add		i_mr
	sacl	i_mr		;i_mr=i_mr+Kr*(iSd-i_mr), 4.12 f
	bcnd	i_mrnotzero,NEQ
	lacc	#0
	sacl	tmp			;如果 i_mr=0 则 tmp=iSq/i_mr=0
	b		i_mrzero
i_mrnotzero
*** 除法运算 (iSq/i_mr)
	lacc	i_mr
	bcnd	i_mrzero,EQ
	sacl	tmp1
	lacc	iSq
	abs
	sacl	tmp
	lacc	tmp,12
	rpt		#15
	subc	tmp1
	sacl	tmp			;tmp=iSq/i_mr
	lacc	iSq
	bcnd	iSqpos,GT
	lacc	tmp
	neg
	sacl	tmp			;tmp=iSq/i_mr, 4.12 format
iSqpos
i_mrzero
*** 除法运算结束 ***
	lt	tmp
	mpy	#Kt
	pac	
	sach	tmp,4		;转差频率, 4.12 格式
	lacc	tmp			;加载 tmp 到 ACC 低16位
	add	n 
    
    RPT   #(Pole_pairs_number-1)
    sfr
    
	sacl	fs			;转子磁链速度, 4.12 format,
	            		;fs=n+Kt*(iSq/i_mr)
*** 转子磁链位置计算 ***
	lacc	fs
	abs
	sacl	tmp
	lt	tmp
	mpy	#K
	pac
	sach	tetaincr,4
	bit	fs,0
	bcnd	fs_neg,TC
	lacl	tetaincr
	adds	Teta_cm
	sacl	Teta_cm
	b	fs_pos
fs_neg	
	lacl	Teta_cm
	subs	tetaincr
	sacl	Teta_cm
		;Teta_cm=Teta_cm+K*fs=Teta_cm+tetaincr
		;(0;360)<->(0;65535) 

fs_pos
   
	ldp		#theta_cur_mod		
	and		#7FFFh  
	sacl	theta_cur_mod
***********************************************
*	电流磁链模型结束
***********************************************                                                            
	ret