*************************************************************
* Speed PI Regulator
*************************************************************
;		.ref	pid_reg_spd,pid_reg_spd_init ; function call
;		.ref	spd_fdb,spd_ref		; Inputs
;		.ref	spd_out				; Outputs
;------------------------------------------------------------
; Global Definitions
;------------------------------------------------------------
		.def	pid_reg_spd,pid_reg_spd_init ; function call
		.def	spd_fdb,spd_ref		; Inputs
		.def	spd_out				; Outputs
;------------------------------------------------------------
; Variable Definitions
;------------------------------------------------------------

spd_ref	.usect "pid_spd",1
spd_fdb	.usect "pid_spd",1
spd_out	.usect "pid_spd",1      


;other variables : 
* PI regulators variable
upi	    .usect "pid_spd",1	;PI regulators (current and speed) 			;output
elpi	.usect "pid_spd",1	;PI regulators (current and speed) 			;limitation error
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	;speed error (used in speed 				;regulator)
xin	    .usect "pid_spd",1	;speed regulator integral 					;component
epiq	.usect "pid_spd",1	;q-axis current regulator error
epid	.usect "pid_spd",1	;d-axis current regulator error
xiq	    .usect "pid_spd",1	;q-axis current regulator integral 			;component
xid	    .usect "pid_spd",1	;d-axis current regulator integral 			;component

*** vSqref and VdSr limitations
Vmin	.set	0ec00h	;4.12 format=-1.25 pu 
Vmax	.set	1400h	;4.12 format=1.25 pu 

*** iSqref limitations
Isqrefmin	.set	-1310	;4.12 format=-0.8 pu, I_nom=10A
Isqrefmax	.set	1310	;4.12 format=0.8 pu, I_nom=10A
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 speed regulators parameters
	splk	#35h,Kin
	splk	#482bh,Kpin
	splk	#0bh,Kcorn
	ret
	
pid_reg_spd
;Interface avec les nouvelles variables
	ldp		#spd_fdb
	lacc	spd_fdb ;Q15, 6000rpm as pu
	sfr 
	ldp		#n
	sacl	n		;Q12, 1500rpm as pu

	ldp		#spd_ref	;Q15 ref with 1pu =1500rpm
	lacc	spd_ref
	sfr
	sacl	n_ref		;Q12 with 1500rpm as pu	
*****************************************************
* Speed regulator with integral component correction
*****************************************************
	ldp		#n_ref
	lacc	n_ref
	sub		n
	sacl	epin	;epin=n_ref-n, 4.12 format
	lacc	xin,12
	lt		epin
	mpy		Kpin
	apac
	sach	upi,4	;upi=xin+epin*Kpin, 4.12 format
			;here we start to saturate
	bit		upi,0
	bcnd	upimagzeros,NTC	;If value >0 we branch
	lacc	#Isqrefmin	;negative saturation
	sub		upi
	bcnd	neg_sat,GT	;if upi<ISqrefmin then branch to 
			;saturate
	lacc	upi	;value of upi is valid
	b	limiters
neg_sat
	lacc	#Isqrefmin	;set acc to -ve saturated value
	b	limiters
 
upimagzeros	;Value is positive
	lacc	#Isqrefmax	;positive saturation
	sub	upi
	bcnd	pos_sat,LT	;if upi>ISqrefmax then branch to 				;saturate
	lacc	upi	;value of upi valid
	b	limiters
pos_sat
	lacc	#Isqrefmax	;set acc to +ve saturated value

limiters
	sacl	iSqref	;Store the acc as reference value
;Interface avec la nouvelle variable de sortie
	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 format
	lt	elpi	;if there is no saturation elpi=0
	mpy	Kcorn
	pac
	lt	epin
	mpy	Kin
	apac
	add	xin,12
	sach	xin,4	;xin=xin+epin*Kin+elpi*Kcorn, 4.12 				;format
***********************************************************
* END Speed regulator with integral component correction
***********************************************************
	ret