sincosph.asm

无刷直流电机的无传感器控制TI程序

;===========================================================================
; File Name:	sincosph.asm
;
; Module Name: 	SINCOSPH					    
;
; Initialization Routine: SINCOSPH_INIT
;
; Originator:	Digital Control Systems Group
;			Texas Instruments
;
; Description:	Sine & Cosine function generator with phase control	
;
;			|~~~~~~~~~~~~~~~|
;	gain_cs o------>|		|
;			|	        |----->o  sine_a1 
;			| SINECOSPH     |----->o  sine_a2
;	freq o--------->|		|	
;			|		|
;	phase o-------->|		|
;			|		|
;			|_______________|
;
;
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 9-15-2000	Release	Rev 1.0		
;===========================================================================
;(To use this Module, copy this section to main system file)
;		.ref	SINCOSPH, SINCOSPH_INIT			;function call
;		.ref	gain_cs, freq, phase, direction		;Inputs 
;		.ref	Vcomp1, Vcomp2				;Inputs (modified by compen.asm)
;		.ref	sine_a1, sine_a2,						;Outputs
;===========================================================================
		.def	SINCOSPH, SINCOSPH_INIT			;function call
		.def	gain_cs, freq, phase 			;Inputs 
		.def	Vcomp1, Vcomp2				;Inputs (modified by compen.asm)
		.def	sine_a1, sine_a2						;Outputs
;===========================================================================
		.include 	"x24x_app.h"
		.ref	SINTAB_360    

		

STEP_ANGLE3_MAX	.set	333	;corresponds to 102Hz frequency.
              
ALPHA_a1	.usect	"sincos" ,1   
sine_a1		.usect	"sincos" ,1
sine_a2		.usect	"sincos" ,1
gain_cs		.usect	"sincos" ,1
freq		.usect	"sincos" ,1
phase		.usect	"sincos" ,1
sg6_freq_max	.usect	"sincos" ,1
STEP_ANGLE6	.usect	"sincos" ,1
ENTRY13		.usect	"sincos" ,1  
GPR		.usect	"sincos" ,1      
Vcomp1		.usect	"sincos" ,1
Vcomp2		.usect	"sincos" ,1    
GPRC 		.usect	"sincos" ,1
              
;====================
SINCOSPH_INIT:
;====================
		LDP	#ALPHA_a1
		SPLK	#0, ALPHA_a1 		;Start at 0 deg
		SPLK	#STEP_ANGLE3_MAX, sg6_freq_max
		SPLK	#3FFFh, gain_cs		;Init amplitude to 50%
		SPLK	#3FFFh, freq		;Init freq to 50%
		SPLK	#090, 	phase		;Init phase to 90 deg
		RET

;====================
SINCOSPH:
;====================    
		LDP	#freq
		SPLK	#0B6h, GPR
		LT	GPR
		MPY	phase
		PAC   
		SACL	phase
		

;Normalise the freq input to appropriate step angle  
		LDP	#freq
		LT	freq				;sg6_freq is in Q15
		MPY	sg6_freq_max		;sg6_freq_max is in Q0
		PAC						;P = Q0 x Q15 = Q15 (in 32bit word)
		SACH	STEP_ANGLE6,1	;shift 1 to restore Q0 format
		
;-------------------------------------------------------
;Calculations for the first sine wave (sine_a1)
;-------------------------------------------------------
SG_a1		LACC	ALPHA_a1
		ADD	STEP_ANGLE6		;Inc angle.
		SACL	ALPHA_a1
		LACC	ALPHA_a1,8   
		SACH	ENTRY13   
		
		LACC	#SINTAB_360    
		ADD	ENTRY13
		TBLR	sine_a1			;sine_a=Sin(ALPHA) in Q15

	;scale the output
		LT	sine_a1				;sine_b is in Q15
		
;		LDP	#Vcal
;		LACC	compensa
;		SUB	#1
;		BCND	nocomp1, LT   
		
;		LDP	#gain_cs
;		LACC	Vcomp1
;		SACL	gain_cs
		
nocomp1	LDP	#gain_cs
		MPY	gain_cs				;sg_gain is in Q15
		PAC						;P = sg_gain * sine_b
		SACH	sine_a1,1		;shift 1 to restore Q15 format

;-------------------------------------------------------
;Calculations for the second sine wave  (sine_a2)
;-------------------------------------------------------
SG_a2		LACC	ALPHA_a1
		ADD	phase		;add phase shift
		SACL	GPRC
		LACC	GPRC,8
		SACH	ENTRY13
		LACC	#SINTAB_360
		ADD	ENTRY13
		TBLR	sine_a2		;sine_a=Sin(ALPHA) in Q15

;scale the output
		LT	sine_a2			;sine_b is in Q15 
		
;		LDP	#Vcal
;		LACC	compensa
;		SUB	#1
;		BCND	nocomp2, LT   
		
;		LDP	#gain_cs
;		LACC	Vcomp2
;		SACL	gain_cs
		
nocomp2		LDP	#gain_cs
		MPY	gain_cs			;sg_gain is in Q15
		PAC				;P = sg_gain * sine_b
		SACH	sine_a2,1	;shift 1 to restore Q15 format

		RET