;=====================================================================
; 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