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:		x240/243/2407
;
;=====================================================================================
; 修改记录:
;-------------------------------------------------------------------------------------
; 最终修改日期:2005.4.5 	版本号:	Ver 1.0
;------------------------------------------------------------
; 变量声明
;------------------------------------------------------------
;			.ref	CURRENT_MODEL,CURRENT_MODEL_INIT; function call
;			.ref	i_cur_mod_D,i_cur_mod_Q		; Inputs  
;			.ref	spd_cur_mod					; Input
;			.ref	theta_cur_mod				; Outputs
;------------------------------------------------------------
; 标号定义
;------------------------------------------------------------
			.def	CURRENT_MODEL,CURRENT_MODEL_INIT; function call
			.def	i_cur_mod_D,i_cur_mod_Q		; Inputs  
			.def	spd_cur_mod					; Input
			.def	theta_cur_mod				; Outputs  
			.def    fs

;------------------------------------------------------------
; 变量定义
;------------------------------------------------------------
;全局变量              
i_cur_mod_D	.usect "cur_mod",1	
i_cur_mod_Q .usect "cur_mod",1                   
spd_cur_mod .usect "cur_mod",1                     
theta_cur_mod .usect "cur_mod",1


;局部变量
iSd			.usect "cur_mod",1		;stator current flux component
iSq			.usect "cur_mod",1		;stator current torque component
n			.usect "cur_mod",1		;rotor mechanical speed
Teta_cm 	.usect "cur_mod",1		;rotor flux position
i_mr		.usect "cur_mod",1		;magnetizing current
fs      	.usect "cur_mod",1		;rotor flux electrical speed
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
									;one sampling period
tmp	        .usect "cur_mod",1		;temporary variable
tmp1        .usect "cur_mod",1		;temporary variable
Teta_cm1	.usect "cur_mod",1		;vizualisation variable

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

; 电动机参数

Kr_	.set	0eh   ;must be calculated according to the
Kt_	.set	1b0h  ;motor parameters (check out .xls sheet)
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 as pu
    sfr                 ; check this conversion depending on your nominal speed
	ldp		#n
	sacl	n		    ;Q12, 1500rpm as pu
***********************************************
* 电流磁链模型开始
***********************************************
	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			;if i_mr=0 then 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	;slip frequency, 4.12 format
	lacc	tmp		;load tmp in low ACC
	add	n 
    
    RPT   #(Pole_pairs_number-1)
    sfr
    
	sacl	fs		;rotor flux speed, 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