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

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