svpwm.asm

DSP motor 控制程序

				;* bit 1 1: F. Comp. Unit 2 in PWM mode
				;* PWM3/CMP3 & PWM4/CMP4 are PWM outputs
				;* bit 0 1: F. Comp. Unit 1 in PWM mode
				;* PWM1/CMP1 & PWM2/CMP2 are PWM outputs

; Define GP Timer compare output polarities and GP Timer actions
        SPLK #0000000001010101b,GPTCON
				;* bits 12-11 00: No GP Timer 3 event starts ADC
				;* bits 10-9 00: No GP Timer 2 event starts ADC
				;* bits 8-7 00: No GP Timer 1 event starts ADC
				;* bit 6 1: Enable GP Timer Compare outputs
				;* bits 5-4 01: GP Timer 3 comp output active low
				;* bits 3-2 01: GP Timer 2 comp output active low
				;* bits 1-0 01: GP Timer 1 comp output active low

		SPLK	 #1010100010000010b,T3CON
				; bit 15 1: FREE = 1
				; bit 14 0: SOFT = 0
				; bits 13-11 101: continuous-up/dn count mode
				; bits 10-8 000: Prescaler = /1
				; bit 7 1: Use Timer ENABLE of GP Timer 1
				; bit 6 0: Disable timer (counting operation
				; bits 5-4 00: Select internal CLK
				; bits 3-2 00: Load GP Timer comp register on underflow
				; bit 1 1: GP Timer compare enabled
				; bit 0 0: Use own PR
				; Configure GP Timer 2
		SPLK	 #1010100010000010b,T2CON
				; bit 15 1: FREE = 1
				; bit 14 0: SOFT = 0
				; bits 13-11 101: continuous-up/dn count mode
				; bits 10-8 000: Prescaler = /1
				; bit 7 1: Use Timer ENABLE of GP Timer 1
				; bit 6 0: Disable timer (counting operation)
				; bits 5-4 00: Select internal CLK
				; bits 3-2 00: Load GP Timer comp register on underflow
				; bit 1 1: GP Timer compare enabled
				; bit 0 0: Use own PR
				; Configure GP Timer 1
		SPLK	 #1010100000000010b,T1CON
				; bit 15 1: FREE = 1
				; bit 14 0: SOFT = 0
				; bits 13-11 101: continuous-up/dn count mode
				; bits 10-8 000: Prescaler = /1
				; bit 7 0: Reserved
				; bit 6 1: Disable Timer (counting operation)
				; bits 5-4 00: Select internal CLK
				; bits 3-2 00: Load GP Timer comp register on underflow
				; bit 1 1: GP Timer compare enabled
				; bit 0 0: reserved
		SPLK	 #0fffh,IFRA ; Clear all Group A interrupt flags
		SPLK	 #0ffh,IFRB ; Clear all Group B interrupt flags
		SPLK	 #0fh,IFRC ; Clear all Group C interrupt flag
		SPLK	 #0h,IMRA ; Mask all Grp A ints
		SPLK	 #04h,IMRB ; Mask all but GPT2 UF Grp B ints
		SPLK	 #00h,IMRC ; Mask all Grp C ints
		
********************************************************************
** Initialize variables **
********************************************************************
; Point to B1 page 0

		LDP	 #6
		SPLK	 #1,one 		; +1 => one
		SPLK	 #T_sample_,T_sample	; sampling period
		SPLK	 #A_W_,A_W 		; D8, ADC to set W ratio
		SPLK	 #A_U_,A_U 		; D1, ADC to set U ratio
		SPLK	 #min_W_,min_W 		; lower limit on set W
		SPLK	 #max_U_,max_U 		; upper limit on set U
		SPLK	 #min_U_,min_U 		; lower limit on set U
		SPLK	 #0,THETAL 		; theta low byte
		SPLK	 #0,THETAH 		; theta high byte
		
		LAR	     AR0,#theta_60 		; point to 1st destination
		LAR	     AR1,#(32-1) 		; 32 entries
		LACC	 #angles_ 		; point to 1st data item
		LARP	 AR0 			;
INITB
		TBLR	 *+,1 			; move and point to next destination
		ADD	     one 			; point to next data item
		BANZ	 INITB,0 ;
; Init table 1st and last entries and table pointer
		SPLK	 #TB_TH,theta_1stent
		SPLK	 #1,SP
		SPLK	 #TB_S,SIN_1stent
		SPLK	 #(TB_S+180),SIN_lastent
		
********************************************************************
** More house keeping **
********************************************************************
; set LED display on EVM
; ~~~~~~~~~~~~~~~~~~~~~~
;		splk	 #01h,LED_data ;
;		out	 LED_data,LED_addr 	; set LED display
;		splk	 #LED_freq,LED_count 	; reset sub-divider counter
;		splk	 #1,LED_dir 		; set LED display direction point to memory page 0
		LDP	 #0
		SPLK	 #0ffh,IFR 		; Clear all core interrupt flags
		SPLK	 #0eh,IMR 		; Unmask all EV interrupts to CPU
		setC	 OVM 		; set overflow protect mode
		setC	 SXM 		; set sign extension (allow) mode
		EINT	 		; Enable global interrupt
					; Point at EV reg page
		LDP	 #232
					; Start all three GP Timers
		SPLK	 #1010100001000010b,T1CON
		
********************************************************************
** Start of main loop **
********************************************************************
MAIN
; point at B1 page 0
		ldp	 #6

; Wait for sampling period start.
w_sample:
    	LACC	 period_flag 	; Load sampling period start flag
		BZ	 w_sample 	; Wait if not set
		SPLK	 #0,period_flag ; Reset the flag if it is set
					; Toggle xf for debug purpose
		setc	 xf 		;

; Update LED display on EVM
;		lacc	 LED_count 	;
;		sub	 one 		; update sub_divide counter
;		sacl	 LED_count 	; time to update LED display?
;;		BNZ	 LED_nc 	; no
;		splk	 #LED_freq,LED_count 	; yes, reset subdivide counter
;		bit	 LED_dir,BIT0 		; left shift?
;		bcnd	 right_shift,NTC 	; no
;		lacc	 LED_data,1 		; yes
;		sacl	 LED_data 		; left shift one bit
;		bit	 LED_data,BIT7 		; time to change direction?
;		bcnd	 LED_update,NTC 	; no
;		splk	 #0,LED_dir 		; yes
;		b	 LED_update 		;
;right_shift: 
;    	lacc	 LED_data,15 		;
;		sach	 LED_data 		; right shift one bit
;		bit	 LED_data,BIT0 		; time to change direction?
;		bcnd	 LED_update,NTC 	; no
;		splk	 #1,LED_dir 		; yes
;LED_update: 	
;        out	 LED_data,LED_addr 	; update LED display
;LED_nc

********************************************************************
** Read ref frequency **
********************************************************************
; Point at Sys Mod reg page 0
		LDP	 #0E0h
; Get ADC data and re-start ADC.
		lacl	 ADCFIFO1 		; Get ADC data, 0 ACC high
		SPLK	 #0101100111111111b, ADC_CNTL0	; re-start ADC
; point at B1 page 0
		ldp	 #6
; Right shift ADC data by one bit to get D0 representation.
		sfr	 	; shift right by one bit
		sacl	 adc0_7 ; save (D0)

; Replace adc0_7 with debug data. For debug purpose only. 
		SPLK	 #debug_data,adc0_7 ;
		
********************************************************************
** Calculate radian frequency **
********************************************************************
		LT	 adc0_7 	; load ADC data: D0
		MPY	 A_W 		; D0*D10=D(10+1)
		PAC	            ;
		SACH	 S_W 		; radian frequency: D11

		SUBH	 min_W 		; compare W with its limit
		BGZ	 W_in_limit 	; continue if within limit
		LACC	 min_W 		; saturate if not
		SACL	 S_W 		;
W_in_limit:

********************************************************************
** Calculate magnitude of ref voltage Uout **
********************************************************************
; Note const. V/Hz is implied
		MPY	 A_U 		; D0*D1=D(1+1)
		PAC	 		;
		SACH	 S_U 		; set U: D2
		SUBH	 max_U 		; compare Uout with its upper limit
		BLEZ	 U_in_uplimit 	; continue if within limit
		LACC	 max_U 		; saturate if not
		SACL	 S_U 		;
U_in_uplimit
		LACC	 S_U 		;
		SUB	 min_U 		; compare Uout with its lower limit
		BGEZ	 U_in_lolimit 	; continue if within limit
		LACC	 min_U 		; saturate if not
		SACL	 S_U 		;
U_in_lolimit

********************************************************************
** Obtain theta (phase of Uout) through 32 bit integration **
********************************************************************
		LT	 S_W 			;angular speed
		MPY	 T_sample 		; sample period D-9*D11=D(2+1)
		PAC	 			;
		ADDS	 THETAL 		;
		ADDH	 THETAH 		;no ADDH?
		SACH	 THETAH 		;
		SACL	 THETAL 		; accumulate: D3+D3=D3
		SUBH	 theta_360 		; compare with 2*pi: D3-D3=D3
		BLEZ	 Theta_in_limit 	; continue if within limit
		SACH	 THETAH 		; mod(2*pi, THETA) if not
Theta_in_limit
		ZALH	 THETAH 		;
		ADDS	 THETAL 		;
		ADD	 one,15 		;
		SACH	 theta_r 		; round up to upper 16 bits
		
********************************************************************
** Determine quadrant **
********************************************************************
; assume THETA (THETAH) is in quadrant 1
		LACC	 one			; assume THETA (THETAH) is in quadrant 1
		SACL	 SS 			; 1=>SS, sign of SIN(THETA)
		SACL	 SC 			; 1=>SC, sign of COS(THETA)
		LACC	 theta_r 		;
		SACL	 theta_m 		; THETA=>theta_m
		SUB	 theta_90 		;
		BLEZ	 E_Q 			; jump to end if 90>=THETA
; assume THETA (THETAH) is in quadrant 2 if not
		splk 	#-1,SC 			; -1=>SC
		LACC	 theta_180 		;
		SUB	 theta_r 		; 180-THETA
		SACL	 theta_m 		; =>theta_m
		BGEZ	 E_Q 			; jump to end if 180>=THETA
; assume THETA (THETAH) is in quadrant 3 if not
		splk	 #-1,SS 		; -1=>SS
		LACC	 theta_r 		;
		SUB	 theta_180 		; THETA-180
		SACL	 theta_m 		; =>theta_m
		LACC	 theta_270 		;
		SUB	 theta_r 		;
		BGEZ	 E_Q 			; jump to end if 270>=THETA
; THETA (THETAH) is in quadrant 4 if not
		splk	 #1,SC 			; 1=>SC
		LACC	 theta_360 		;
		SUB	 theta_r 		;
		SACL	 theta_m 		; 360-THETAH=>theta_m
E_Q:

********************************************************************
** Obtain theta table entry **
********************************************************************
		LACC	 theta_1stent 		;the entry of theta
		ADD	 SP 			;the enty of SIN
		TBLR	 GPR0 			; get table(SP)
		LACC	 theta_m 		;
		SUB	 GPR0 			; compare theta_m with table(SP) 
		BZ	 look_end 		; end look-up if equal
		BGZ	 inc_SP 		; increase SP if bigger
		
dec_SP 		LACC	 SP 			; decrease SP otherwise
		SUB	 one 			;
		SACL	 SP 			; SP-1=>SP
		ADD	 theta_1stent 		; point to SP-1
		TBLR	 GPR0 			; get table(SP-1)
		LACC	 theta_m 		;
		SUB	 GPR0 			; compare theta_m with table(SP-1)
		BLZ	 dec_SP 		; decrease SP further if smaller
		B	 look_end 		; jump to end if not
		
inc_SP 		LACC	 SP 			;
		ADD	 one 			;
		SACL	 SP 			; SP+1=>SP
		ADD	 theta_1stent 		; point to SP+1	
		TBLR	 GPR0 			; get table(SP+1)
		LACC	 theta_m 		;
		SUB	 GPR0 			; compare theta_m with table(SP+1)
		BGZ	 inc_SP 		; increase further if bigger
		
look_end 					; end if not

********************************************************************
** Get sin(theta) **
********************************************************************
		LACC	 SIN_1stent 		;
		ADD	 SP 			; 
		TBLR	 sin_theta 		; get sin(THETA)
		LT	 SS 			;
		MPY	 sin_theta 		; modify sign: D15*D1=D(16+1)
		PAC	 			;
		SACL	 sin_theta 		; left shift 16 bits and save: D1
		
********************************************************************
** Get cos(theta) **
********************************************************************
		LACC	 SIN_lastent 		; get end of sin table
		SUB	 SP 			;
		TBLR	 cos_theta 		; get cos(THETA)
		LT	 SC 			;
		MPY	 cos_theta 		; modify sin: D15*D1=D(16+1)
		PAC	 			;
		SACL	 cos_theta 		; left shift 16 bits and save: D1
		
********************************************************************
** Calculate Ud & Uq **
********************************************************************
		LT	 S_U 			;
		MPY	 cos_theta 		; Uref*cos(THETA): D2*D1=D(3+1)
		PAC	 			;
		SACH	 Ud 			;
		MPY	 sin_theta 		; Uref*sin(THETA): D2*D1=D(3+1)
		PAC	 			;
		SACH	 Uq 			;
		
********************************************************************
** Determine sector **
********************************************************************
		MAR	 *,AR0 			; ARP points to AR0
		LAR	 AR0,#1 		; assume S=1
		LACC	 theta_r 		;
		SUB	 theta_60 		; compare with 60 (in sector; 1?)
		BLEZ	 E_S 			; jump to end if yes
		MAR	 *+ 			; assume S=2 if not
		LACC	 theta_r 		;
		SUB	 theta_120 		; compare with 120 (in sector; 2?)
		BLEZ	 E_S 			; jump to end if yes
		MAR	 *+ 			; assume S=3 if not.
		LACC	 theta_r 		;
		SUB	 theta_180 		; compare with 180 (in sector; 3?)
		BLEZ	 E_S 			; jump to end if yes
		MAR	 *+ 			; assume S=4 if not
		LACC	 theta_r 		;
		SUB	 theta_240 		; compare with 240 (in sector; 4?)
		BLEZ	 E_S 			; jump to end if yes
		MAR	 *+ 			; assume S=5 if not
		LACC	 theta_r 		;
		SUB	 theta_300 		; compare with 300 (in sector; 5?)
		BLEZ	 E_S 			; jump to end if yes
		MAR	 *+ 			; S=6 if not
E_S		SAR	 AR0,S 			;
		
*****************************************************
** Calculate T1 & T2 based on 			**
**	Tpwm Uout = V1*T1 + V2*T2 		**
** or 						**
** 	[T1 T2] = Tpwm [V1 V2]' Uout 		**
** 	[0.5*T1 0.5*T2] = Tp [V1 V2]' Uout 	**
** 	= Mdec(S) Uout 				**
** where 					**
** 	Mdec(S) = Tp [V1 V2]' 			**
** 	Uout = Trans([Ud Uq]) 			**
** Mdec is obtained through table look-up. 	**
** Note that timer period is half of PWM period, **
** i.e. Tp=0.5 Tpwm. 				**
*****************************************************

		LACC	 #(decpar_1stent-4)
		ADD	 S,2 			;
		SACL	 GPR0 			; get the pointer
		LAR	 AR0,GPR0 		; point to parameter table
		LT	 Ud 			; calculate 0.5*T1
		MPY 	*+ 			; M(1,1) Ud: D4*D10=D(14+1)
		PAC	 			;
		LT	 Uq ;
		MPY	 *+ 			; M(1,2) UqP D4*D10=D(14+1)
		APAC	 			; 0.5*T1: D15+D15=D15
		BGEZ	 cmp1_big0 		; continue if bigger than zero
		ZAC	 			; zero it if less than zero
		
cmp1_big0 	
        SACH	 cmp_1 			;
		LT	 Ud 			; Calculate 0.5*T2
		MPY	 *+ 			; M(2,1) Ud: D4*D10=D(14+1)
		PAC	 			;
		LT	 Uq 			;
		MPY	 *+ 			; M(2,2) Uq: D4*D10=D(14+1)
		APAC	 			; 0.5*T2: D15+D15=D15
		BGEZ	 cmp2_big0 		; continue if bigger than zero
		ZAC	 			; zero it if less than zero
		
cmp2_big0
    	SACH	 cmp_2 			;
		LACC	 #max_cmp_ 		; Calculate 0.5*T0
		SUB	 cmp_1 			;
		SUB	 cmp_2 			; 0.5*T0 = Tp - 0.5*T1-0.5*T2: D15
		BGEZ	 cmp0_big0 		; continue if bigger than zero
		ZAC	 			; zero it if less than zero
cmp0_big0
     	SACL	 cmp_0 			;
		LACC	 cmp_0,15 		; left shift 15 (right shift 1) bit
		SACH	 cmp_0 			; 0.25*T0
		
********************************************************************
** Determine the channel toggling sequence and load compare values**
********************************************************************
		LACC	 #(first_-1) 		;
		ADD	 S 			; point to entry in look up table
		TBLR	 CL 			; get the channel to toggle the 1st
		LAR	 AR0,CL 		; point to the 1st channel
		LACC	 cmp_0 			;
		SACL	 * 			; cmp_0=>the 1st channel
		LACC	 #(second_-1) 		;
		ADD	 S 			; point to entry in look up table
		TBLR	 CM 			; get the channel to toggle the 2nd
		LAR	 AR0,CM 		; point to the 2nd channel
		LACC	 cmp_0 			;
		ADD	 cmp_1 			; cmp_0+cmp_1
		SACL	 * 			; => the 2nd channel
		LACC	 #CMPR3 		;
		SUB	 CL 			;
		ADD	 #CMPR2 		;
		SUB	 CM 			;
		ADD	 #CMPR1 		;
		SACL	 GPR0 			; get the channel to toggle the 3rd
		LAR	 AR0,GPR0 		; point to the 3rd channel
		LACC	 cmp_0 			;
		ADD	 cmp_1 			;
		ADD	 cmp_2 			; cmp_0+cmp_1+cmp_2
		SACL	 * 			; =>the 3rd channel
		
**************************************
** Reset wd timer **
**************************************