qep.asm

DSP关于F2407的实例程序

;=====================================================================
; File		:	QEP.asm
; Module Name	:	QEP
; Initialization Routine	: QEP_INIT
; Description	:	通过计算QEPA,QEPB,QEPZ得到转子实际转速OmigaR0，和转向QEP_dir。
;			当通过原点的时候有正脉冲，通过捕捉功能来产生中断，
;			给计数器重新赋初值。
; OmigaR0 = ( POS0 - POS1 ) / Tpwm,单位（1/mS）变换成可以被其它模块用的形式，OmigaR0=OmigaR0*7FFFH/2pi
;
;	 		 |~~~~~~~~~~~~~~~~~~|
;  POS0    	o------> |		    |
;  POS1		o------> |	QEP	    |----->o  OmigaR0
;  Tpwm		o------> |  		    |----->o  QEP_dir
;			 |    		    |
;			 |------------------|
;
; 编码器为1000线，每360度产生4000个脉冲。每个PWM开关周期进行一次计算。
; Modify: 2004.6.16
;=====================================================================
	 	.include "f2407.h"
		.global	QEP,QEP_INIT			; function call
		
		.global	POS0,POS1	 		; Inputs
		.global	OmigaR0,QEP_dir		; Outputs
		.global TEMP
		.global	QEP_C				; constant

;=====================================================================
QEP_INIT:
;=====================================================================
		LDP	#DP_EVA
		SPLK	#0000H,CAPCONA		; 使能QEP
		SPLK	#1830H,T2CON		; 还没有使能timer2。
		SPLK	#4000H,T2CNT		; 计数器初值。考虑到要增减计数，取个中间值。（不用超过8000，为负数了）
		
		LDP	#DP_DATA
		SPLK	#0,POS1			; 存第一个值。
		SPLK	#41,QEP_C		; 计算OmigaR0用到的常量  
		LDP	#DP_EVA
		SPLK	#1870H,T2CON		; 使能timer2。	
		RET	
;=====================================================================
QEP:
;=====================================================================
		SPM	0			;相乘为无符号数相乘，不需要移位。
;----------------------------------------------------		
; calculate QEP_dir		
		LDP	#DP_EVA			;检测转向
		BIT	GPTCONA,1		;通用定时器2的状态，1为增计数。
		BCND	CONVER,TC		;增计数时转移
		LACL	#0			;减计数，反向旋转。
		B	END_QEP_dir
CONVER:
		LACL	#1			;增计数，正向旋转。
END_QEP_dir:
		LDP	#DP_DATA
		SACL	QEP_dir			
;----------------------------------------------------
; calculate OmigaR0
; OmigaR0 = ( POS0 - POS1 ) / Tpwm       增减计数算式一样
; 转换成CAL_FLUX可用＝(POS0 - POS1)*5*(7FFFH/4000)=(POS0 - POS1)*41,（单位1/mS，7FFFh为360度）按Tpwm=0.2mS
; 令QEP_C=41,
; T2CNT>=4000+初值（增计数）或T2CNT<=-4000+初值（减计数）时重新装载计数值。
; 判断是否超过4000线。
		BCND	DOWNCOUNT,EQ		; ACC=0时转移,减计数。
; 增计数时	
		LDP	#DP_EVA
		LACC	T2CNT
		LDP	#DP_DATA
		SACL	POS0			; 存当前位置
		SUB	POS1
		SACL	TEMP			; 存差值
		LT	TEMP
		MPY	QEP_C			; 按5K开关频率、电机1500r/min、则相乘不会超出16位。
		PAC
		AND	#7FFFH			;保证不会为负数
		SACL	OmigaR0			;求得OmigaR0
; 判断是否超过4000线
		LACC	POS0
		SUB	#4FA0H			; 4FA0H-4000H=4000			
		BCND	END_DIS,LT		; 4000线以后就重新计数。
		ADD	#4000H
;		SACL	POS0
		SACL	POS1			; 下一次计算的POS1
		LDP	#DP_EVA
		SACL	T2CNT			; COUNTER=POS1
		B	END_QEP
; 减计数时
DOWNCOUNT:	
		LDP	#DP_EVA
		LACC	T2CNT
		LDP	#DP_DATA
		SACL	POS0			; 存当前位置
		SUB	POS1
		ABS				; 减计数，相减为负,取绝对值
		SACL	TEMP			; 存差值
		LT	TEMP
		MPY	QEP_C			; 按5K开关频率、电机1500r/min、则相乘不会超出16位。
		PAC	
		AND	#7FFFH			;保证不会为负数
		SACL	OmigaR0		;求得OmigaR0
; 判断是否超过4000线
		LACC	POS0
		SUB	#3060H			; 4000h－3060h＝4000
		BCND	END_DIS,GT		; 4000线以后就重新计数。
		ADD	#4000H			; 加上初值4000H,重新一轮循环
;		SACL	POS0		
		SACL	POS1			; 下一次计算的POS1
		LDP	#DP_EVA
		SACL	T2CNT			; COUNTER=POS1
		B	END_QEP
END_DIS:
		LACC	POS0
		SACL	POS1			; 下一次计算的POS1
END_QEP:	
		;正反转的处理。	
;----------------------------------------------------
		SPM 	0			; 复位
		RET