; File Name : svpwm_open_new.asm
; Description:
; This program implements an open-loop speed control algorithm for
; three-phase AC induction motors using constant V/Hz principle and
; space vector PWM technique.
;-------------------------------------------------------------------
; Notes
;-------------------------------------------------------------------
; 1. This program implements a sampling loop to carry out all the
; calculations. The PWM and sampling frequencies are
; independently controlled.
; 2. Constant V/Hz principle is used to generate the magnitude of
; voltage command from frequency input;
; 3. Space vector PWM technique is used to generate the pulse-width
; modulated signals controlling a three-phase voltage source power
; inverter so that desired voltage magnitude and frequency are
; applied to the phased of a three-phase AC induction motor.
; 4. Both the PWM and sampling frequencies have been chosen to be
; 20KHz.
; 5. Maximum scaling and 32 bit integration are used to maximize the
; accuracy of integer math involved to achieve a better dynamic response.
; 6. The D scaling notation used here is equivalent to the popular Q
; notation based on equation Dx=Q(15-x).
; 7. The motor is rated at 60Hz (that is, maximum duty ratio is
; achieved when input is 60Hz).
; 8. Frequency input is through an ADC interface, ADC value 0
; corresponds to 0Hz, ADC value 7fe0h corresponds to 120Hz.

*********************************************************************
		.include	F240.h

;---------------------------------------------------------------------
		.sect	".vectors"

RSVECT		B    START	; Reset Vector
INT1	  	B    PHANTOM	; Interrupt Level 1
INT2	  	B    EV_isr_B	; Interrupt Level 2
INT3	  	B    PHANTOM	; Interrupt Level 3
INT4	  	B    PHANTOM	; Interrupt Level 4
INT5	  	B    PHANTOM	; Interrupt Level 5
INT6	  	B    PHANTOM	; Interrupt Level 6
RESERVED  	B    PHANTOM	; Reserved
SW_INT8   	B    PHANTOM	; User S/W Interrupt
SW_INT9   	B    PHANTOM	; User S/W Interrupt
SW_INT10  	B    PHANTOM	; User S/W Interrupt
SW_INT11  	B    PHANTOM	; User S/W Interrupt
SW_INT12  	B    PHANTOM	; User S/W Interrupt
SW_INT13  	B    PHANTOM	; User S/W Interrupt
SW_INT14  	B    PHANTOM	; User S/W Interrupt
SW_INT15  	B    PHANTOM	; User S/W Interrupt
SW_INT16  	B    PHANTOM	; User S/W Interrupt
TRAP	  	B    PHANTOM	; Trap vector
NMINT	  	B    PHANTOM	; Non-maskable Interrupt
EMU_TRAP  	B    PHANTOM	; Emulator Trap
SW_INT20  	B    PHANTOM	; User S/W Interrupt
SW_INT21  	B    PHANTOM	; User S/W Interrupt
SW_INT22  	B    PHANTOM	; User S/W Interrupt
SW_INT23  	B    PHANTOM	; User S/W Interrupt

;-----------------------------------------------------
;	Variable  and constants initializations
;-----------------------------------------------------
	
ST0	    	.set 0 		; status register ST0
ST1 		.set 1 		; status register ST1
wd_rst_1 	.set 055h 	; watchdog timer reset strings
wd_rst_2 	.set 0aah 	;
LED_addr 	.set 0Ch 	; addr of LED display on EVM
;-------------------------------------------------------------------
; Variable definitions
;-------------------------------------------------------------------
;********************************************************************
;** Variables in B1 page 0 **
;********************************************************************
		.bss GPR0,1 		; temporary storage
		.bss one,1 		; +1
		.bss wd_period,1 	; watchdog timer period
		.bss wd_reset1,1 	; watchdog timer reset string 1
		.bss wd_reset2,1 	; watchdog timer reset string 2
		.bss period_flag,1 	; period start flag
		.bss adc0_7,1 		; adc 0, channel 0 data
		.bss adc0_6,1 		; adc 0, channel 1 data
		.bss adc0_5,1 		; adc 0, channel 2 data
		.bss adc1_15,1 		; adc 1, channel 0 data
		.bss adc1_14,1 		; adc 1, channel 1 data
		.bss adc1_13,1 		; adc 1, channel 2 data
		.bss A_W,1 		; D10, ADC data to set W ratio
		.bss A_U,1 		; D1, ADC data to set U ratio
		.bss S_W,1 		; set angular speed: D11
		.bss min_W,1 		; lower limit on set W (frequency)
		.bss S_U,1 		; normalized set voltage: D2
		.bss max_U,1 		; upper limit on set U: D2
		.bss min_U,1 		; lower limit on set U: D2
		.bss T_sample,1 	; sampling period: D-9
		.bss THETAH,1 		; D3, angular position higher word
		.bss THETAL,1 		; angular position lower word
		.bss theta_r,1 		; rounded THETAH
		.bss theta_m,1 		; D3, THETA mapped to 1st quadrant
		.bss theta_1stent,1 	; beginning of theta table
		.bss SS,1 		; sin sign modification: D15
		.bss SC,1 		; cos sign modification: D15
		.bss SP,1 		; sin table entry
		.bss SIN_1stent,1 	; beginning of sin table
		.bss SIN_lastent,1 	; end of sin table
		.bss sin_theta,1 	; sin(THETA): D1
		.bss cos_theta,1 	; cos(THETA): D1
		.bss Ud,1 		; voltage Ud: D4
		.bss Uq,1 		; voltage Uq: D4
		.bss S,1 		; D15, sector reference U is in
		.bss theta_60,1 	; 60: D3
		.bss theta_90,1 	; 90: D3
		.bss theta_120,1 	; 120: D3
		.bss theta_180,1 	; 180: D3
		.bss theta_240,1 	; 240: D3
		.bss theta_270,1 	; 270: D3
		.bss theta_300,1 	; 300: D3
		.bss theta_360,1 	; 360: D3
		.bss decpar_1stent,24 	; Decomposition matrices: D10
		.bss cmp_1,1 		; component on 1st basic sp vector
		.bss cmp_2,1 		; component on 2nd basic sp vector
		.bss cmp_0,1 		; component on 0 basic sp vector /2
		.bss CL,1 		; channel to toggle 1st
		.bss CM,1 		; channel to toggle 2nd
		.bss LED_dir,1 		; LED direction (1: left, 0: right)
		.bss LED_data,1 	; LED display
LED_freq 	.set 3000 		; LED update sub-divider
		.bss LED_count,1 	; sub-divider counter for LED
;********************************************************************
;** Variables in B2 **
;********************************************************************
ST0_save 	.SET 060h 		; saved status register ST0
ST1_save 	.set 061h 		; saved status register ST1
ACCH 		.SET 062h 		; saved accumulator high
ACCL 		.SET 063h 		; saved accumulator low
BSRS 		.SET 064h 		; saved BSR
WSTORE 		.SET 065h 		; working storage
;-------------------------------------------------------------------
; Program parameters
;-------------------------------------------------------------------
				; Debug data used to substitute ADC input to debug the program.
debug_data 	.set 01aa5h 	; 100Hz,6a9d)(50Hz,354b)(25Hz,1aa5
				; ADC to radian frequency conversion ratio given by
				; 120*2*pi/7fe0h(D0)=05721018.
				; 7fe0h corresponds to 120Hz (754.3512 rad/Sec)
adc_to_afrequency	.set 24222 	; D10
A_W_ 		.set adc_to_afrequency 	; D10
					; Min input frequency.
					; User's choice
min_afrequency 	.set 0 			; D11
min_W_ 		.set min_afrequency 	; D11
					; ADC to magnitude of reference voltage conversion ratio
					; 1.0/sqrt(2)/ADC(60Hz)(D0).
					; Motor is rated at 60Hz meaning max duty ratio is achieved at 60Hz.
adc_to_voltage 	.set 11630 		; D2
A_U_ 		.set adc_to_voltage 	; D2
					; Max magnitude of reference voltage
					; 1.0/sqrt(2)
max_voltage 	.set 5792 		; D2
max_U_ 		.set max_voltage 	; D2
					; Min magnitude of reference voltage given by
				; 1.0/sqrt(2)*min_f/60Hz
min_voltage 	.set 0 ; D2
min_U_ 		.set min_voltage ; D2
				; Timer 1 period which determines the PWM frequency.
T1_period_ 	.set 500
				; Tp = 2*500*50nS=50uS => Fp = 20KHz
				; Timer 2 period which determines the sampling frequency.
T2_period_ 	.set 500
				; Ts = 2*500*50nS=50uS => Fs = 20KHz
				; Max compare value
max_cmp_ 	.set 500
				; Sampling period
T_sample_ 	.set 00346h 	; D-9, Ts = 50uS, Fs = 20KHz

;-------------------------------------------------------------------
; Memory resident program data
;-------------------------------------------------------------------
		.data
;********************************************************************
;** Frequently used angles **
;********************************************************************
; The order between these angles and the decomposition matrices
; in the following must not be changed.

angles_ .WORD	 010c1h 	; pi/3: D3
		.WORD	 01922h 	; pi/2: D3
		.WORD	 02183h 	; 2*pi/3: D3
		.WORD	 03244h 	; pi
		.WORD	 04305h 	; 4*pi/3: D3
		.WORD	 04b66h 	; 3*pi/2: D3
		.WORD	 053c7h 	; 5*pi/3: D3
		.WORD	 06488h 	; 2*pi: D3
;********************************************************************
;** Decomposition matrices indexed by the sector THETA (Uout) is in**
;********************************************************************
		.word	 19595 	; D10
		.word	 -11314
		.word	 0
		.word	 22627
		.word	 -19595
		.word	 11314
		.word	 19595
		.word	 11314
		.word	 0
		.word	 22627
		.word	 -19595
		.word	 -11314
		.word	 0
		.word	 -22627
		.word	 -19595
		.word	 11314
		.word	 -19595
		.word	 -11314
		.word	 19595
		.word	 -11314
		.word	 19595
		.word	 11314
		.word	 0
		.word	 -22627
********************************************************************
** Addresses of compare registers corresponding to channels to **
** toggle the 1st in a given period indexed by the sector THETA **
** (Uout) is in. **
********************************************************************
first_ 		.WORD	 CMPR1 	;
		.WORD	 CMPR2 	;
		.WORD	 CMPR2 	;
		.WORD	 CMPR3 	;
		.WORD	 CMPR3 	;
		.WORD	 CMPR1 	;
;** Addresses of compare registers corresponding to channels to **
;** toggle the 2nd in a given period indexed by the sector THETA **
;** (Uout) is in. **
;********************************************************************
second_ 	.WORD	 CMPR2 ;
		.WORD	 CMPR1 ;
		.WORD	 CMPR3 ;
		.WORD	 CMPR2 ;
		.WORD	 CMPR1 ;
		.WORD	 CMPR3 ;


		.text
;********************************************************************
;** Start of main body of code **
;********************************************************************

.text
		NOP
;----------Board general settings--------
START:	setc	INTM    		;Disable interrupts
		splk	#0002h,IMR		;Mask all core interrupts except INT2

		lacc	IFR			;Read Interrupt flags
		sacl	IFR			;Clear all interrupt flags

		clrc	SXM			;Clear Sign Extension Mode
		clrc	OVM			;Reset Overflow Mode

		clrc	CNF			;Config Block B0 to Data mem
;		setc	OVM			;reflect arithmetic overflows			
;		setc	SXM			;sign extension mode
		spm	0

		ldp 	#00E0h			;DP for addresses 7000h-707Fh
		splk	#00BBh,CKCR1  		;CLKIN(OSC)=10MHz,CPUCLK=20MHz
		splk	#00C3h,CKCR0 		;CLKMD=PLL Enable,SYSCLK=CPUCLK/2
		splk	#40C0h,SYSCR 		;CLKOUT=CPUCLK		

;		splk	#006Fh, WDCR		;Disable WD if VCCP=5V (JP5 in pos. 2-3)

;		KICK_DOG				;Reset Watchdog
		ldp	#DP_PF1
		splk	#006fh,	WDCR
		splk	#05555h,WDKEY
		splk	#0AAAAh,WDKEY
		splk	#006fh,	WDCR

 		dint 			; Set global interrupt mask

;********************************************************************
;** System configuration **
;********************************************************************
; Configure system registers
; ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
				; Point at Sys Module reg page 0
		LDP	 #0E0h
				; Disable watchdog timer if VCCP pin is at 5V
		SPLK	 #06Fh, WD_CNTL
				; Reset watchdog timer
		SPLK	 #wd_rst_1,WDKEY
		SPLK	 #wd_rst_2,WDKEY
				; Set the source of CLKOUT to be CPUCLK
		SPLK	 #0100000011000000b,SYSCR
				; Clear all SYSSR register bits except HP0 (bit 5)
				; FLASH programming and WD disabled allowed when bit 5 is 1.
				; Note bit 5 is a read/clear bit. It can not be set.
		splk	 #00000000000000100000b,SYSSR
				; Configure PLL/Clocks to generate CPUCLK of 20MHz when CLKIN=10MHz
		SPLK	 #0000000010110001b,CKCR1
				; Disable and re-enable the PLL to make sure changes to CKCCR1
				; happen
		SPLK	 #0000000000000001b,CKCR0; Disable PLL
		SPLK	 #0000000011000001b,CKCR0; Re-enable PLL
				; Point to memory page 0 (B2)
		LDP	 #0
				; Configure wait state generator register so that no wait state is
				; added for any off chip access
		SPLK	 #1000b,WSTORE
		OUT	 WSTORE,0ffffh ; WSGR <= (WSTORE)
				; Point at Sys Module reg page 1
		LDP	 #0E1h
				; Configure i/o pins so that all pins shared by Event Manager
				; are configured as Event Manager pins
				; See comment lines for configuration of other pins
		SPLK	 #0ff00H,OPCRA
* IOPA3/ADCIN8 => IOPA3
* IOPA2/ADCIN9 => IOPA2
* IOPA1/ADCIN1 => IOPA1
* IOPA0/ADCIN0 => IOPA0
		SPLK	 #00f0H,OPCRB
* BIO_/IOPC3 => BIO_
* XF/IOPC2 => XF
* IOPC0/ADCSOC => IOPC0
				; Configure the directions of all digital i/o pins to be input
		SPLK	 #0000H,IOPA_DDR
		SPLK	 #0000H,IOPB_DDR
		SPLK	 #0000H,IOPC_DDR
		
********************************************************************
** Initialize peripherals **
********************************************************************
				; Initialize and start ADC
; ~~~~~~~~~~~~~~~~~~~~~~~~
				; Point at Sys Mod reg page 0
		LDP	 #0E0h
				; Set up ADC module with p/s=1, disable ext SOC and E.M. SOC,
				; enable both ADC modules, disable ADC interrupt,
				; select channel 15 (ADC1) and channel 7 (ADC0) and
				; start conversion.
				; ADC is re-started every time the previous conversion results are
				; read.
		SPLK	 #0000000000000011b,ADC_CNTL1
		SPLK	 #0101100111111111b, ADC_CNTL0
				; Point at Event Manager register page
		LDP	 #232
;-------------------------------------------------------------------
; Initialize Event Manager
;-------------------------------------------------------------------
				; Clear all Event Manager registers before proceeding further.
				; Good to have even when reset works properly.
		SPLK	 #0,T1CON ;
		SPLK	 #0,T2CON ;
		SPLK	 #0,T3CON ;
		SPLK	 #0,DBTCON ;
		SPLK	 #0,COMCON ;
		SPLK	 #0,CAPCON ;
		SPLK	 #0,T1CNT ;
		SPLK	 #0,T2CNT ;
		SPLK	 #0,T3CNT ;
				; Init GP Timer 1 period that determines the PWM frequency.
		SPLK	 #T1_period_,T1PER
				; Init GP Timer 2 period that determines the sampling frequency
				; of speed loop.
		SPLK	 #T2_period_,T2PER
				; Init GP Timer 3 period for other use
		SPLK	 #T1_period_,T3PER
				; Kill all F. Comp/PWM outputs.
		SPLK	 #T1_period_,CMPR1
		SPLK	 #T1_period_,CMPR2
		SPLK	 #T1_period_,CMPR3
				; Let GP Timer compare outputs toggle (to have more things I can
				; look at with an oscilloscope).
		SPLK	 #200,T1CMP
		SPLK	 #200,T2CMP
		SPLK	 #200,T3CMP
				; Define PWM output polarities.
		SPLK	 #0000100110011001b,ACTR
* bits 15 0: Dir = CCW (n/c)
* bits 14-12 000: D2D1D0 = 000 (n/c)
		SPLK	 #0,T1CON ;
		SPLK	 #0,T2CON ;
		SPLK	 #0,T3CON ;
		SPLK	 #0,DBTCON ;
		SPLK	 #0,COMCON ;
		SPLK	 #0,CAPCON ;
		SPLK	 #0,T1CNT ;
		SPLK	 #0,T2CNT ;
		SPLK	 #0,T3CNT ;
				; Init GP Timer 1 period that determines the PWM frequency.
		SPLK	 #T1_period_,T1PER
				; Init GP Timer 2 period that determines the sampling frequency
				; of speed loop.
		SPLK	 #T2_period_,T2PER
				; Init GP Timer 3 period for other use
		SPLK	 #T1_period_,T3PER
				; Kill all F. Comp/PWM outputs.
		SPLK	 #T1_period_,CMPR1
		SPLK	 #T1_period_,CMPR2
		SPLK	 #T1_period_,CMPR3
				; Let GP Timer compare outputs toggle (to have more things I can
				; look at with an oscilloscope).
		SPLK	 #200,T1CMP
		SPLK	 #200,T2CMP
		SPLK	 #200,T3CMP
				; Define PWM output polarities.
		SPLK	 #0000100110011001b,ACTR
				; bits 15 0: Dir = CCW (n/c)
				; bits 14-12 000: D2D1D0 = 000 (n/c)
				; bits 10-9 00: No GP Timer 2 event starts ADC
				; bits 8-7 00: No GP Timer 1 event starts ADC