qep_drv.asm

有速度pid的异步电机vf控制的dsp调试程序

;===========================================================================
; File:		qep_drv.asm
; Module Name:	QEP_THETA_DRV
;
; Initialization Routine: QEP_THETA_DRV_INIT
;	
; Synchronization Routine: QEP_INDEX_ISR_DRV
;
; Originator:	Digital Control Systems Group
;			Texas Instruments
;
;
; Description:	This module determines the rotor position and generates a 
;		direction (of rotation) signal from the shaft position encoder 
;		pulses. 
;	QEP_THETA_DRV: Calculates rotor displacement from the shaft encoder
;	QEP_INDEX_ISR_DRV: Called in by the index triggered ISR to
;                        synchronize T2 counter to encoder index pulse
;
;				 |~~~~~~~~~~~~~~~~~~~|
; QEP A/B(EV H/W)o------>|			   |----->o  theta_elec
; polepairs      o------>|   QEP_THETA_DRV   |----->o  theta_mech
; cal_angle      o------>|  			   |----->o  dir_QEP
; mech_scale     o------>|  			   |
;			  	 |-------------------|
; QEP Index (EV H/W) o-->| QEP_INDEX_ISR_DRV |----->o  index_sync_flg
;				 |                   |----->o  QEP_cnt_idx
;				 |___________________|
;
; Target:		x240/24x/240x
;
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 9-15-2000	Release	Rev 1.0	
;===========================================================================

;---------------------------------------------------------------------------
; Reference/Prototype
;---------------------------------------------------------------------------
;		.ref	QEP_THETA_DRV,QEP_THETA_DRV_INIT	; function call
;		.ref	polepairs,cal_angle,mech_scale	; Inputs
;		.ref	theta_elec,theta_mech,dir_QEP		; Outputs
;
;		.ref	QEP_INDEX_ISR_DRV				; function call
;		.ref	index_sync_flg,QEP_cnt_idx		; Output

;---------------------------------------------------------------------------
; Global Defintions
;---------------------------------------------------------------------------
		.def	QEP_THETA_DRV,QEP_THETA_DRV_INIT	; function call
		.def	polepairs,cal_angle,mech_scale	; Inputs
		.def	theta_elec,theta_mech,dir_QEP		; Outputs

		.def	QEP_INDEX_ISR_DRV				; function call
		.def	index_sync_flg,QEP_cnt_idx		; Output

;-------------------------------------------
; Define Related Peripherals
;-------------------------------------------
        .include  x24x_app.h

;---------------------------------------
; Variables
;---------------------------------------
theta_elec	.usect "qep_drv",1
theta_mech	.usect "qep_drv",1
dir_QEP	.usect "qep_drv",1
polepairs	.usect "qep_drv",1
QEP_cnt_idx	.usect "qep_drv",1
cal_angle	.usect "qep_drv",1
index_sync_flg	.usect "qep_drv",1
mech_scale	.usect "qep_drv",1
theta_raw	.usect "qep_drv",1
tmp   	.usect "qep_drv",1
;---------------------------------------------------------------
; Parameters
;---------------------------------------------------------------
POLEPAIRS_	.set	2		 ; # pole pairs
CAL_ANGLE_	.set	0		 ; 0 mech degs
MECH_SCALE_	.set	16776		 ; 0.9999/Total_count (Q26) (total count = 4000 for 1000 line encoder)

;----------------------------------------------------
; Initialization
;----------------------------------------------------
QEP_THETA_DRV_INIT:
		ldp	#polepairs
		SPLK	#POLEPAIRS_,polepairs
		splk	#CAL_ANGLE_,cal_angle
		SPLK	#MECH_SCALE_,mech_scale
		.if (x240)
		ldp	#OCRC>>7
		lacl	OCRC
		or	#01110000b
		sacl	OCRC
		.endif
		.if (x243|x2407)
		ldp	#OCRA>>7
		lacl	OCRA
		or	#00111000b
		sacl	OCRA
		.endif
		ldp	#T2PER>>7
		SPLK	#0FFFFh,T2PER			; compatible across '240/24x/240x
		SPLK	#1001100001110000b,T2CON	; D-Up/Dn, /1, src=QEP
		SPLK	#1111000000000100b,CAPCON	; Tmr2, CAP3 pos edge, enbl
		RET

;---------------------------------------------------------
; Angular Position Calculation Routine
;---------------------------------------------------------
QEP_THETA_DRV
		ldp	#GPTCON>>7
		lacc	GPTCON,2        ;bit14 is direction
		ldp #tmp
		sach tmp
		lacc tmp
		and #0000000000000001b; Get direction bit
		sacl	dir_QEP	; save
		ldp	#T2CNT>>7	; read count of pulses
		lacc	T2CNT		;
		ldp	#cal_angle	;
		add	cal_angle	; add offset amount
		sacl	theta_raw	;
		LT	theta_raw	;
		MPY	mech_scale	; Q0*Q26=Q26(32bit)
		PAC			;
		and	#7FFFh,11	; modulo(360 in Q26)
		SACH	theta_mech,5; save as Q(26-16+5)=Q15
		LT	theta_mech	;
		MPY	polepairs	; Q15*Q0=Q15(32bit)
		PAC
		and	#7FFFh	; modulo(360 in Q15)
		SACL	theta_elec	; save as Q15
		RET

;---------------------------------------------------------------------------
; Synchronization Routine
;---------------------------------------------------------------------------
QEP_INDEX_ISR_DRV:
		ldp	#T2CNT>>7
		bldd	T2CNT,#QEP_cnt_idx	; Read out QEP counter value
		SPLK	#0,T2CNT			; Clear counter
		ldp	#index_sync_flg
		SPLK	#0Fh,index_sync_flg	; Indicate Index mark alignment.
		RET