;====================================================================================
; File Name:	compwm.asm	
;
; Module Name: 	COMPWM	
;
; Initialization Routine: COMPWM_INIT
;
; Originator:	Digital Control Systems Group
;			Texas Instruments
;
; Description:		
;
;			|~~~~~~~~~~~~~~~|
;	Mfunc_c1 ------>|		|----->o  CMPR1 (EV register)
;	Mfunc_c2 ------>|COMPWM  	|----->o  CMPR2 (EV register)
;	Mfunc_c3 ------>|		|----->o  CMPR3 (EV register)
;	Mfunc_p	 ------>|		|----->o  T1PER (EV register)
;	limit	 ------>|		|
;			|		|
;	DC_RIPPLE ----->|		|
;	VDC_ACTUAL ---->|		|
;	VDC_HOT ------->|		|
;	VDC_TOP_REF --->|		|
;	VDC_BOT_REF --->|               |
;       		~~~~~~~~~~~~~~~~~
; To Select the target device see x24x_app.h file.
;
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 9-15-2000	Release	Rev 1.0
;===========================================================================
; DC_RIPPLE		is a switch 1 = compensation ON/ 0 = OFF
; VDC_ACTUAL	total measured DC bus voltage
; VDC_HOT		lower capacitor voltage (measured)
; VDC_TOP_REF	ideal top capacitor voltage
; VDC_BOT_REF	ideal bottom capacitor voltage
;
; Ideally VDC_TOP_REF = VDC_BOT_REF 
; Ideally VDC_ACTUAL = 2 * VDC_HOT
; VDC_TOP_ACTUAL = VDC_ACTUAL - VDC_HOT   
;===========================================================================
;(To use this Module, copy this section to main system file)
;		.ref	COMPWM
;		.ref	COMPWM_INIT			;function call
;		.ref	Mfunc_c1, Mfunc_c2, Mfunc_c3, Mfunc_p	;Inputs
;		.ref	limit	
;		.ref	DC_RIPPLE,VDC_TOP_REF, VDC_BOT_REF	;inputs
;		.ref	VDC_ACTUAL, VDC_HOT   			;inputs

;	Note: CMPR1, CMPR2, CMPR3, T1PER are defined in .h file
;===========================================================================
;Module definitions for external reference.
		.def	COMPWM				;function call
		.def	COMPWM_INIT			;function call
		.def	Mfunc_c1, Mfunc_c2, Mfunc_c3, Mfunc_p	;Inputs
		.def	limit					;Input
		.def	DC_RIPPLE,VDC_TOP_REF, VDC_BOT_REF	;inputs
		.def	VDC_ACTUAL, VDC_HOT   				;inputs



	.include	x24x_app.h



Mfunc_c1		.usect "spimcomp",1
Mfunc_c2		.usect "spimcomp",1
Mfunc_c3		.usect "spimcomp",1
Mfunc_p			.usect "spimcomp",1
period_max		.usect "spimcomp",1
period			.usect "spimcomp",1
plimit			.usect "spimcomp",1
limit			.usect "spimcomp",1
compare_1  		.usect "spimcomp",1
compare_2 		.usect "spimcomp",1
compare_3		.usect "spimcomp",1

DC_RIPPLE		.usect	"spimcomp",1
Ta_new			.usect	"spimcomp",1
Ta_DIFF			.usect	"spimcomp",1
Tb_new			.usect	"spimcomp",1
Tb_DIFF			.usect	"spimcomp",1
TPER			.usect	"spimcomp",1
TPER_Ta			.usect	"spimcomp",1
TPER_Tb			.usect	"spimcomp",1
SPEED_HI		.usect	"spimcomp",1
SPEED_LO		.usect	"spimcomp",1
VDC_ACTUAL		.usect	"spimcomp",1
VDC_HOT			.usect	"spimcomp",1   
VDC_TOP_ACTUAL		.usect	"spimcomp",1
VDC_TOP_REF		.usect	"spimcomp",1
VDC_BOT_REF		.usect	"spimcomp",1
INV_VDC_ACTUAL		.usect	"spimcomp",1
SCRATCH			.usect	"spimcomp",1
VY			.usect	"spimcomp",1
VX			.usect	"spimcomp",1	





;===================================
COMPWM_INIT:
;===================================

	LDP	#limit
	SPLK	#0, limit

;---target dependancy---------------
	.if (x240)	;target dependancy

FPERIOD	.set	1000		;1000 x 50nS = 50uS,  i.e. 20KHz PWM
 
	;Initialize period info
;		POINT_B0        		
		LDP		#period_max		
		SPLK	#FPERIOD, period_max
		SPLK	#7FFFh, Mfunc_p

		POINT_EV
		SPLK	#FPERIOD, T1PER				

			;5432109876543210
			;||||!!!!||||!!!!
		SPLK	#1001000001000000b,T1CON  	;Asym

	;Configure Simple & Full Compare registers
;		SPLK	#01F8h, DBTCON			;400nS @ 50nS clk
;		SPLK	#02F8h, DBTCON			;800nS @ 50nS clk
;		SPLK	#03F8h, DBTCON			;1.2uS @ 50nS clk
;		SPLK	#04F8h, DBTCON			;1.6uS @ 50nS clk
;		SPLK	#05F8h, DBTCON			;2.0uS @ 50nS clk
;		SPLK	#06F8h, DBTCON			;2.4uS @ 50nS clk
;		SPLK	#07F8h, DBTCON			;2.8uS @ 50nS clk
		SPLK	#08F8h, DBTCON			;3.2uS @ 50nS clk
;		SPLK	#0000h, DBTCON			;Disabled, i.e. Zero

		SPLK	#0000011001100110b,ACTR 	;Full Action Cntl
		SPLK	#0010011000000111b,COMCON	;Compare Cntl
		SPLK	#1010011000000111b,COMCON	;Compare Cntl
			;||||!!!!||||!!!!
			;5432109876543210
;		POINT_B0
;		RET
	.endif
;-----------------------------------

;---target dependancy---------------
	.if (x243)	;target dependancy

;FPERIOD	.set	1000		;1000 x 50nS = 50uS,  i.e. 20KHz PWM
;FPERIOD	.set	1334		; 1334 x 50nS = 66uS,  i.e 15kHz PWM
FPERIOD		.set	667
 
	;Initialize period info
;		POINT_B0        		
		LDP		#period_max		
		SPLK	#FPERIOD, period_max
		SPLK	#7FFFh, Mfunc_p

		POINT_EV
		SPLK	#FPERIOD, T1PER			;temporary init only		

			    ;5432109876543210
			    ;||||!!!!||||!!!!
;		SPLK	#1001000001000000b,T1CON  	;Asym
 		SPLK	#1000100001000000b,T1CON	;Symm
	;Configure Simple & Full Compare registers
;		SPLK	#01ECh, DBTCON			;400nS @ 50nS clk
;		SPLK	#02ECh, DBTCON			;800nS @ 50nS clk
		SPLK	#03ECh, DBTCON			;1.2uS @ 50nS clk
;		SPLK	#04ECh, DBTCON			;1.6uS @ 50nS clk
;		SPLK	#05ECh, DBTCON			;2.0uS @ 50nS clk
;		SPLK	#06ECh, DBTCON			;2.4uS @ 50nS clk
;		SPLK	#07ECh, DBTCON			;2.8uS @ 50nS clk
;		SPLK	#08ECh, DBTCON			;3.2uS @ 50nS clk
;		SPLK	#0000h, DBTCON			;Disabled, i.e. Zero

;		SPLK	#0000011001100110b,ACTR 	;Full Action Cntl
 		SPLK	#0000001001100110b,ACTR 	;Full Action
		SPLK	#1010011000000000b,COMCON	;Compare Cntl
			;||||!!!!||||!!!!
			;5432109876543210

	;Enable Primary function on CMP1-CMP6 PWM outputs	
		POINT_PF2					;OCRA.6 - OCRA.11
		LACC	OCRA 
		OR	#0000001111000000b      
;		OR	#0000111111000000b 		; for 3 Phase
			;||||!!!!||||!!!!
			;5432109876543210
		SACL	OCRA
;		POINT_B0
;		RET
	.endif
;-----------------------------------

;---target dependancy---------------
	.if (x2407)	;target dependancy

FPERIOD	.set	2000		;2000 x 25nS = 50uS,  i.e. 20KHz PWM

	;Initialize period info

		LDP		#period_max		
		SPLK	#FPERIOD, period_max
		SPLK	#7FFFh, Mfunc_p

		POINT_EV
		SPLK	#FPERIOD, T1PER			;temporary init only		

			    ;5432109876543210
			    ;||||!!!!||||!!!!
		SPLK	#1001000001000000b,T1CON  	;Asym
;		SPLK	#1000100001000000b,T1CON  	;Sym

	;Configure Simple & Full Compare registers
;		SPLK	#01ECh, DBTCON			;200nS @ 25nS clk
;		SPLK	#02ECh, DBTCON			;400nS @ 25nS clk
;		SPLK	#03ECh, DBTCON			;600nS @ 25nS clk
;		SPLK	#04ECh, DBTCON			;0.8uS @ 25nS clk
;		SPLK	#05ECh, DBTCON			;1.0uS @ 25nS clk
		SPLK	#06ECh, DBTCON			;1.2uS @ 25nS clk
;		SPLK	#07ECh, DBTCON			;1.4uS @ 25nS clk
;		SPLK	#08ECh, DBTCON			;1.6uS @ 25nS clk
;		SPLK	#0000h, DBTCON			;Disabled, i.e. Zero

		SPLK	#0000011001100110b,ACTR 	;Full Action Cntl
		SPLK	#1010011000000000b,COMCON	;Compare Cntl
			;||||!!!!||||!!!!
			;5432109876543210

;Enable Primary function on PWM1-PWM6 PWM outputs	
		POINT_PF2					;OCRA.6 - OCRA.11
		LACC	OCRA
		OR	#0000111111000000b
			;||||!!!!||||!!!!
			;5432109876543210
		SACL	OCRA
;		POINT_B0
;		RET
	.endif
;-----------------------------------
		LDP	#DC_RIPPLE
		SPLK	#0	,DC_RIPPLE
		SPLK	#3000h	,VDC_TOP_REF
		SPLK	#3000h	,VDC_BOT_REF
		SPLK	#6000h	,VDC_ACTUAL
		SPLK	#3000h	,VDC_HOT
		RET





;===================================
COMPWM:
;===================================
;Convert "Period" (Q15) modulation function to Q0

		LDP		#period_max		
		LT		Mfunc_p
		MPY		period_max	;P = period x period_max
		PAC
		SACH	period,1
		LACC	period
		SFR
		SACL	period	;period = period / 2
		
;Bipolar case
;------------
DCG1		SPM	1
;Convert "Compare 1" (Q15) modulation function to Q0
		LT	Mfunc_c1
		MPY	period	; P = Mfunc_c1 x period/2
		PAC			;
		ADDH	period	; offset by period/2     		
		SACH	compare_1    
		
		LACC	period  	
		SUB	compare_1     
		BCND	negcycle, GT
		LACC	period,1
		SUB	compare_1
		SACL	plimit    
		LACC	limit
		SUB	plimit
		BCND	update, LT
		LACC	period,1
		SUB	#10
		SACL	compare_1
		B	update
		
negcycle
		LACC	compare_1
		SUB		limit
		BCND	update, GEQ
		LACC	#10
		SACL	compare_1
		
update	
;		LACC	compare_1
;
;
;		POINT_EV 
;		SACL	CMPR1   
;		


;Convert "Compare 2" (Q15) modulation function to Q0

		LDP		#period_max		
DCG2	LT	Mfunc_c2
		MPY	period	; P = Mfunc_c2 x period/2
		PAC			;
		ADDH	period	; offset by period/2
		SACH	compare_2  
		
		LACC	period
		SUB		compare_2
		BCND	negcycle2, GT
		LACC	period,1
		SUB		compare_2
		SACL	plimit
		LACC	limit
		SUB		plimit
		BCND	update2, LT
		LACC	period,1
		SUB		#10
		SACL	compare_2
		B		update2
		
negcycle2

		LACC	compare_2
		SUB		limit
		BCND	update2, GEQ
		LACC	#10
		SACL	compare_2

update2	
;
;Convert "Compare 3" (Q15) modulation function to Q0

		LDP	#period_max		  
DCG3		LT	Mfunc_c3
		MPY	period	; P = Mfunc_c3 x period/2
		PAC			;
		ADDH	period	; offset by period/2
;		SACH	compare_3   

		SACH	compare_3    
		
		LACC	period  	
		SUB		compare_3     
		BCND	negcycle, GT
		LACC	period,1
		SUB		compare_3
		SACL	plimit    
		LACC	limit
		SUB		plimit
		BCND	update3, LT
		LACC	period,1
		SUB		#10
		SACL	compare_3
		B		update3
		
negcycle3
		LACC	compare_3
		SUB		limit
		BCND	update3, GEQ
		LACC	#10
		SACL	compare_3
		
update3	LACC	compare_3


		POINT_EV 
		SACL	CMPR3   

		SPM	0

			LDP	#DC_RIPPLE
			LACC	DC_RIPPLE
			SUB	#1
			BCND	NORMAL, LT

			B	COMPENSATE
	
NORMAL	
			LDP	#compare_1
			LACC	compare_1
			POINT_EV
			SACL	CMPR1
			LDP	#compare_2
			LACC	compare_2
			POINT_EV
			SACL	CMPR2
			POINT_B0
			RET

COMPENSATE  
            LDP		#VDC_ACTUAL
			LACC	VDC_ACTUAL
			SUB		VDC_HOT
			SACL	VDC_TOP_ACTUAL	; SCRATCH = VDCtop_actual

			POINT_EV
			LACC	T1PER
			LDP		#TPER
			SACL	TPER

			LACC	TPER
			SUB	compare_1
			SACL	TPER_Ta		; (T_period - Ta)
			LACC	TPER
			SUB	compare_2
			SACL	TPER_Tb		; (T_period - Tb)

			LACC	#07FFFh
			RPT	#15
			SUBC	VDC_ACTUAL
			SACL	SPEED_HI
			XOR	SPEED_HI
			OR	#0FFFFh

			RPT	#15
			SUBC	VDC_ACTUAL
			SACL	SPEED_LO
			LACC	SPEED_LO
			ADDH	SPEED_HI
			RPT	#4
			SFR
			AND	#7FFFh	
			SACL	INV_VDC_ACTUAL	;(1/VDC_ACTUAL) in Q11

;
;	Calculating compensated duty ratio for PWM of main phase
;

			LT	compare_1			; in Q0
			MPY	VDC_BOT_REF		; in Q15
			PAC
			SACH	SCRATCH,1		; SCRATCH = (Ta)*Vbottom_ref in Q0

			LT	TPER_Ta		; in Q0
			MPY	VDC_TOP_REF		; in Q15
			PAC
			SACH	VX,1			; VX = (Tperiod - Ta)*Vtop_ref in Q0
	
			LACC	VX
			SUB	SCRATCH
			SACL	VX			; V =(Tperiod - Ta)*Vtop_ref - (Ta)*Vbottom_ref

			LT	VDC_TOP_ACTUAL
			MPY	TPER
			PAC
			SACH	SCRATCH,1		; SCRATCH = Tperiod*VDCtop_actual

			LACC	SCRATCH
			SUB	VX
			SACL	VX			; VX = Tperiod*VDCtop_actual - V*

			LT	VX			; in Q0
			MPY	INV_VDC_ACTUAL	; in Q11	
			PAC
			SACH	SCRATCH, 5		; shifts are made to change in Q0

			LACC	SCRATCH
			SACL	Ta_new		

			SETC	SXM
MXA:			LACC	Ta_new
			SUB	TPER
			BCND	REPTa, GEQ
			LACC	Ta_new
			ADD	#0
			BCND	REOTa, LEQ
			B	GOTa
REPTa 		LACC	TPER
			SUB	#5h			; off set for A
			SACL	Ta_new
			B	GOTa
REOTa			ZAC
			LACC	#5h			; off set for A
			SACL	Ta_new
GOTa			NOP
			CLRC	SXM

			LACC	compare_1
			SUB	Ta_new
			BCND	OPPP, LT
			SACL	Ta_DIFF
			B	ARE
OPPP			LACC	Ta_new
			SUB	compare_1
			SACL	Ta_DIFF
ARE			NOP


;
;	Calculating compensated duty ratio for PWM of aux phase
;

			LT	compare_2
			MPY	VDC_BOT_REF
			PAC
			SACH	SCRATCH,1		; SCRATCH = (Tb)*Vbottom_ref

			LT	TPER_Tb
			MPY	VDC_TOP_REF
			PAC
			SACH	VY,1		; VY = (Tperiod - Tb)*Vtop_ref
	
			LACC	VY
			SUB	SCRATCH
			SACL	VY	; V* =(Tperiod - Tb)*Vtop_ref - (Tb)*Vbottom_ref


			LACC	VDC_ACTUAL
			SUB	VDC_HOT
			SACL	SCRATCH	; SCRATCH = VDCtop_actual

			LT	SCRATCH
			MPY	TPER
			PAC
			SACH	SCRATCH,1	; SCRATCH = Tperiod*VDCtop_actual
	
			LACC	SCRATCH
			SUB	VY
			SACL	VY		; VY = Tperiod*VDCtop_actual - V*

			LT	VY
			MPY	INV_VDC_ACTUAL
			PAC
			SACH	SCRATCH, 5		; shifts are made to change in Q0

			LACC	SCRATCH
			SACL	Tb_new		; 


			SETC	SXM
			LACC	Tb_new
			SUB	TPER
			BCND	REPTb, GT
			LACC	Tb_new
			ADD	#0
			BCND	REOTb, LEQ
			B	GOTb
REPTb 		LACC	TPER
			SUB	#5h			; off set for B
			SACL	Tb_new
			B	GOTb
REOTb		ZAC
			LACC	#5h			; off set for B
			SACL	Tb_new

GOTb			NOP
			CLRC	SXM

			LACC	compare_2
			SUB	Tb_new
			BCND	OPPP1, LT
			SACL	Tb_DIFF
			B	ARE1
OPPP1			LACC	Tb_new
			SUB	compare_2
			SACL	Tb_DIFF
ARE1			NOP


			LDP	#Ta_new
			LACC	Ta_new

			POINT_EV
			SACL	CMPR1


			LDP	#Tb_new
			LACC	Tb_new

			POINT_EV
			SACL	CMPR2
			POINT_B0
			RET