电流磁链转换.asm.txt

异步电机变频调速控制程序

;=====================================================================
; File Name	:	Cur_mod.asm
; Module Name	:	CURRENT_MODEL	
; Initialization Routine: CURRENT_MODEL_INIT
; Description:	Current model for field oriented control 
; 				of an AC induction machine
;
;		    			|~~~~~~~~~~~~~~~~~~~|
; i_cur_mod_d ->	|		           	|
; i_cur_mod_q ->	|  CURRENT_MODEL		|----->  theta_cur_mod
; 				|		          	|
; spd_cur_mod-->	|                 		|
; 				|___________________|
;
;=====================================================================
;------------------------------------------------------------
; Reference/Prototype
;------------------------------------------------------------
;			.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
;------------------------------------------------------------
; Global Definitions
;------------------------------------------------------------
			.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

;------------------------------------------------------------
; Variable Definitions
;------------------------------------------------------------
;public variables              
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

;private variables
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
;------------------------------------------------------------
; Default parameters
;------------------------------------------------------------   
Pole_pairs_number   
.set    	2

; Parameters of the motor used with the Encoder

;Kr_		.set	0eh   			;must be calculated according to the
;Kt_		.set	1b0h  			;motor parameters (check out .xls sheet)
;K_		.set	148h

; Parameters of the motor used with the Tachometer

;Kr_		.set	3h     			;must be calculated according to the
;Kt_		.set	62h    			;motor parameters (check out .xls sheet)
;K_		.set	148h	

Kr_		.set	40     			;must be calculated according to the
Kt_		.set	156    			;motor parameters (check out .xls sheet)
K_		.set	328	

;------------------------------------------------------------
; Initialization
;------------------------------------------------------------
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
; Tuning from Q15 (input variables format) to Q12 (working variables)
	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
***********************************************
* Current Model
***********************************************
	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
*** division (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
*** END division ***
	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)
*** rotor flux position calculation ***
	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_c			;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
***********************************************
*	END Current Model
***********************************************                                                            
		ret