.include "F2407REGS.H"
        	.mmregs
        	.global _c_int0
        	.sect "vecotrs"  
        	b	_c_int0
_c_int1		b	_c_int1
_c_int2		b	_c_int2
_c_int3		b	_c_int3
_c_int4		b	_c_int4
_c_int5		b	_c_int5
_c_int6		b	_c_int6
				.space	16*6

stack			.usect	"blockb2",15
dac_val			.usect	"blockb2",5
		
sintab			.usect	"table",256
				.sect	"table_f"
sintab_flash	.include	sine.tab

				.data
vkcurrent		.set		00b38h
vsqrt3inv		.set		093dh
vsqrt32			.set		0ddbh
pwmprd			.set		258h
tonmax			.set		0
maxduty			.set		pwmprd-2*tonmax

*****************pi current regulation parameters**********
vki				.set		07ah	;4.12 format=0.03
vkpi			.set		999h	;4.12 format=0.60
vkcor			.set		0cch	;4.12 format=0.05 kcor=ki/kpi
*****************pi speed regulators parameters**********
vkispeed		.set		7ah
vkpispeed		.set		06800h	;4.12 format=6.5
vkcorspeed		.set		12h
****************pi field weakening regulators parameters
vkiweak			.set		7ah
vkpiweak		.set		999h
vkcorweak		.set		0cch
****************var and vdr	limitations
vbase			.set		01000h
vmin			.set		0ec00h
vmax			.set		01400h
*****************is adn idr limitations
vismax			.set		0bb5h
idrmin			.set		0f44bh
idrmax			.set		0000h
*****************initialization phase iqr
iqrinit			.set		09c1h

****************encoder variables and constants
vkencoder		.set		0c000h

encpulses		.set		4096
****************speed and estimated speed calculation constant*********
nbase			.set		1000h
kspeed			.set		0be7h
SPEEDSTEP		.set		28
				.bss		tmp,1  ;temporary variable to use in isr only!!!!
				.bss		option,1
				.bss		daout,1
				.bss		daouttmp,1
*****************dac displaying table starts here
				.bss		i1,1
				.bss		i2,1
				.bss		i3,1
				.bss		ua,1
				.bss		ub,1
				.bss		uc,1
				.bss		seno1,1
				.bss		t1,1
				.bss		t2,1
				.bss		coseno,1
				.bss		va,1
				.bss		vb,1
				.bss		vc,1
				.bss		vdc,1
				.bss		taon,1
				.bss		tbon,1
				.bss		tcon,1
				.bss		theta,1
				.bss		ialfa,1
				.bss		ibeta,1
				.bss		valfar,1
				.bss		vbetar,1
				.bss		idr,1
				.bss		iqr,1
				.bss		id,1
				.bss		iq,1
				.bss		vdr,1
				.bss		vqr,1
				.bss		epiq,1
				.bss		epid,1
				.bss		xiq,1
				.bss		xid,1
				.bss		n,1
				.bss		n_ref,1
				.bss		epispeed,1
				.bss		xispeed,1
				.bss		x,1
				.bss		y,1
				.bss		z,1
				.bss		sectordisp,1
				.bss		initphase,1
				.bss		encoder,1
				.bss		vr,1
				.bss		iqrmin,1
				.bss		iqrmax,1
**********************end dac displaying table
				.bss		sector,1
				.bss		serialtmp,1
				.bss		da1,1
				.bss		da2,1
				.bss		da3,1
				.bss		da4,1
				.bss		vdcinvtc,1
				.bss		epvr,1
				.bss		xvr,1
				.bss		indice1,1
				.bss		upi,1
				.bss		elpi,1
				.bss		faultreset,1
				.bss		tmp1,1
				.bss		accb,2
				.bss		acc_tmp,2
				.bss		encoderold,1
				.bss		encincr,1
				.bss		speedtmp,1
				.bss		speedstep,1
				.bss		kcurrent,1
				.bss		sqrt3inv,1
				.bss		sqrt32,1
				.bss		ki,1
				.bss		kpi,1
				.bss		kcor,1
				.bss		kispeed,1
				.bss		kpispeed,1
				.bss		kcorspeed,1
				.bss		kiweak,1
				.bss		kpiweak,1
				.bss		kcorweak,1
				.bss		ismax,1
				.bss		kencoder,1
************************end variables and constants initializations

				.text
***************************************************************
*			macro
*inputs: argument in *ar5 must be positive
*outputs:resule in acclow
*notes: this function uses the Newton-Raphson method:
*			x(n)=0.5*[x(n-1)+N/x(n-1)]
*this function uses *(ar5+1) & *(ar5+2) locations
****************************************************************
isqrt			.macro
				clrc 		sxm
				lacc		*+
				sfr
				sacl		*+
				splk		#10,*
isqrt?:			sbrk		#2
				lacc		*+
				rpt			#15
				subc		*
				and			#0FFFFh
				add			*
				sfr
				sacl		*+
				
				lacc		*
				sub			#1
				sacl		*
				bcnd		isqrt?,neq
				
				
				mar			*-
				lacc		*
				mar			*-
				setc		sxm
				.endm
				
*******************************************************
*_c_int2 interrupt service routine
*synchronization of the conrtol algorithm with the pwm
*underflow interrupt
*******************************************************
_c_int2:
******************************************************
**********context saving
******************************************************
				larp		ar4
				mar			*-
				sst			#1,*-
				sst			#0,*-
				sach		*-
				sacl		*-
************end context saving
				mar			*,ar5
				ldp			#DP_EV ;*********************
				lacc		ivra
				ldp			#tmp
				sacl		tmp
				sub			#20h
				bcnd		pdproutine,eq
				sub			#9
				bcnd		controlroutine,eq
				b			contextrestorereturn
*************end int2 interrupt service routine
contextrestorereturn:
				larp		ar4
				mar			*+
				lacl		*+
				add			*+,16
				lst			#0,*+ 
				lst			#1,*+
				clrc		INTM	
				ret
*************end context restore and return
pdproutine:
				ldp			#ifra>>7
				splk		#001h,IFRA
				ldp			#DP_EV	;*********
				splk		#0fffh,actr
				splk		#0207h,COMCONA
				splk		#8207h,COMCONA
				ldp			#DP_PF2
				splk		#0ff00h,PBDATDIR
				rpt			#200
				nop
				splk		#0ff02h,PBDATDIR
				b			contextrestorereturn
**************end pdproutine
**Current sampling----AD conversion
controlroutine:
				ldp			#DP_PF2
				splk		#0ff08h,PCDATDIR
				ldp			#DP_PF1
				splk		#186dh,ADCCNTL1
*********current sampling
conversion:		bit			ADCCNTL1,8
				bcnd		conversion,tc
				lacc		ADC_FIFO1,10
				ldp			#i2
				sach		i2
				ldp			#DP_PF1
				lacc		ADC_FIFO2,10
				ldp			#i3
				sach		i3
*************fault enable and test EVM led on/off
				lacc		faultreset
				bcnd		initcontrol,eq
				ldp			#DP_EV
				splk		#0999h,ACTR
				ldp			#faultreset
				splk		#0,faultreset
*************initialization phase
initcontrol:
				lacl		initphase
				bcnd		noinit1,neq
				setc		xf
				lacc		#0fc00h
				
				sacl		theta
				lacc		#iqrinit
				
				sacl		iqr
				lacc		#0
				sacl		idr
				sacl		encoder
				sacl		encoderold
				sacl		n
				sacl		speedtmp
				lacc		#speedstep
				
				sacl		speedstep
				ldp			#DP_EV
				splk		#1,T3CNT
				ldp			#initphase
				b			go
**************end initialization phase
noinit1:
*************encoder pulses reading
				clrc		xf
				ldp			#DP_EV
				lacc		T3CNT
				neg
				ldp			#i1
				sacl		tmp
				sub			encoderold
				
				sacl		encincr
				add			encoder
				bcnd		encmagzero,gt,eq  ;**********
				add			#encpulses
				
encmagzero:
				sacl		encoder
				sub			#encpulses
				bcnd		encminmax,lt
				sacl		encoder
encminmax:	
				lacc		tmp
				sacl		encoderold
*************end encoder pulses reading

*****************
**theta calculation
*****************
				lt			encoder
				mpyu		kencoder
				pac
				sach		theta,2
				lacl		theta
				and			#0fffh
				sacl		theta
**end theta calculation

**************calculate speed and update reference speed variables
				lacc		speedstep
				sub			#1
				sacl		speedstep
				bcnd		nocalc,gt
				
				lt			speedtmp
				mpy			#kspeed
				pac
				rpt			#7
				sfr
				sacl		n
				lacc		#0
				sacl		speedtmp
				lacc		#SPEEDSTEP
				sacl		speedstep
**************end speed calculation from encoder pulses
***speed regulator with integral component correction
				lacc		n_ref
				sub			n
				sacl		epispeed
				lacc		xispeed,12
				lt			epispeed
				mpy			kpispeed
				apac
				sach		upi,4
				
				bit			upi,0
				bcnd		upimagzeros,ntc
				lacc		iqrmin
				sub			upi
				bcnd		neg_sat,gt
				lacc		upi
				b			limiters
neg_sat:
				lacc		iqrmin
				b			limiters
upimagzeros:
				lacc		iqrmax
				sub			upi
				bcnd		pos_sat,lt
				lacc		upi
				b			limiters
pos_sat:
				lacc		iqrmax
limiters:		
				sacl		iqr
				sub			upi
				sacl		elpi
				lt			elpi
				mpy			kcorspeed
				pac
				lt			epispeed
				mpy			kispeed
				apac
				add			xispeed,12
				sach		xispeed,4
*************end speed regulator withe integral component correction
***field-weaking algorithm with pi regulator
***calculation of sqrt(vdr^2+vqr^2)
***only if n>nbase/2
*******************************************************************
				lacc 		#nbase
				sfr
				sub			n
				bcnd		nocalc,geq
				
				lar			ar5,#60h
				zac
				mpy			#0
				mar			*,ar5
				spm			2
				sqra		vdr
				sqra		vqr
				apac
				sach		*
				isqrt
				spm			0
				sacl		vr,6
***********voltage regulator with integral component correction
				lacc		#vbase
				sub			vr
				sacl		epvr
				lacc		xvr,12
				lt			epvr
				mpy			kpi
				apac
				sach		upi,4
				
				bit			upi,0
				bcnd		upimagzerov,ntc
				lacc		#idrmin
				sub			upi
				bcnd		neg_satv,gt
				lacc		upi
				b			limiterv
				
neg_satv:
				lacc		#idrmin
				b			limiterv
upimagzerov:
				lacc		#idrmax
				sub			upi
				bcnd		pos_satv,lt
				lacc		upi
				b			limiterv
pos_satv:
				lacc		#idrmax
limiterv:
				sacl		idr
				sub			upi
				sacl		elpi
				lt			elpi
				mpy			kcor
				pac
				lt			epvr
				mpy			ki
				apac
				add			xvr,12
				sach		xvr,4
***************end voltage regulator with integral component correction
**************************************************************************
**field-weakening algorithe iqr limitation
**for pi regulator
**calculation of sqrt (ismax^2-idr^2)
**output iqrmax
**************************************************************************
				lar			ar5,#60h
				zac
				mpy			#0
				mar			*,ar5
				spm			2
				sqra		idr
				sqrs		ismax
				apac
				sach		*
				isqrt
				spm			0
				sacl		iqrmax,6
				neg
				sacl		iqrmin,6
**********end field weakening routines
******************************************************************************
****encoder update
************************************************************************
nocalc:
				lacc		speedtmp
				add			encincr
				sacl		speedtmp
*****end measured speed and reference speed variables updating  
go:
*****************sampled current scaling to nominal current 1000h<->i_nominal
********************************************************************
				ldp			#i1
				lacc		i2
				add			#045h
				and			#3ffh
				sub			#512
				
				sacl		tmp
				spm			3
				lt			tmp
				mpy			kcurrent
				pac
				sfr
				sfr
				neg
				sacl		i2
				
				
				lacc		i3
				add			#03ch
				
				and			#3ffh
				sub			#512
				sacl		tmp
				lt			tmp
				mpy			kcurrent
				pac
				sfr
				sfr
				neg
				sacl		i3
				
				add			i2