qep_drv.asm

感应电机汇编源程序,已经完全调试通过,方便使用TI公司2407DSP的同行应用,高手可用.是我的心血,小心应用.

;=====================================================================
; File		:	qep_drv.asm
; Module Name	:	QEP_THETA_DRV
; Initialization Routine	: QEP_THETA_DRV_INIT
; Synchronization Routine	: QEP_INDEX_ISR_DRV
; 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
;				 			|___________________|
;=====================================================================
;		.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;在零标记出计数器T2CNT的数值
cal_angle		.usect "qep_drv",1;转子机械位置为0时，Timer2的设定值
index_sync_flg .usect "qep_drv",1;计数同步标记
mech_scale	.usect "qep_drv",1;把T2CNT的数值转换成机械角度的定标因子
theta_raw		.usect "qep_drv",1;

;---------------------------------------------------------------
; Parameters
;---------------------------------------------------------------
POLEPAIRS_	.set	2		 		; # pole pairs
CAL_ANGLE_	.set	333		 		; 30 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,14			;?????????????????
		and		#1,16				; Get direction bit
		ldp		#dir_QEP			;
		sach	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