;===========================================================================
; File Name:	Cur_mod.asm
;
; Module Name:	CURRENT_MODEL	
;
; Initialization Routine: CURRENT_MODEL_INIT
;
; Originator:	Digital Control Systems Group
;			Texas Instruments
;
; 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-->|                 |
; 				|_________________|
;
;
; Target:		x240/243/2407
;
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 10-31-2000	Release	Rev 1.0
;------------------------------------------------------------
; 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_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
***********************************************
*	END Current Model
***********************************************                                                            
	ret