qep.asm

f2407:电机控制

;-------------------------------------------------------------------------
;function name: int qep(int qep_speed)
;output buffer: int speed  		Q12
;global variable definition and function prototype statement:
;		int speed;		//output buffer of qep function
;		extern void qep(int qep_speed);	//function prototype statement 
;constant calculation for FOCMAIN2.C:
;	Encoder output is 512CPR, normal speed of motor is about 1000r/min,
;speedstep=50, then the QEP pulse number of TMS320LF2407 in per period is:
;		nn=(1000*4*512*speedstep*Tpwm)/60=(1000*4*512*50*100e-6)/60=170.6666667 
;assume when speed of motor is rating, the output of 
;	this subroutine is 0x1000, speed=0x1000, then we can get the relations belows:
;		170.6666667*Kspeed=4096			
;		Kspeed=4096/nn=23.99926758=0x17ff (2**8)	

;2002. 7.10.	
;------------------------------------------------------------------------- 
		.global _speedstep
Kspeed  .set 017ffh    			;speed=T4CNT*Kspeed
		.def _qep
		.text
_qep:
		POPD	*+
		SAR		AR0,*+
		SAR		AR1,*
		LAR		AR0,#01h
		LAR		AR0,*0+,AR0
		LAR		AR2,*,AR2
		SBRK	#03				;STACK:speed/address/AR0/AR1
								;ARP:AR2, AR0:AR1, AR2:speed
		SPM		2  				;PREG LEFT SHIFT 4
		SETC	SXM
		LT		*,AR0
		LACC	#Kspeed
		SACL	*
		MPY		*
		PAC          
		SACH	*,4
		LACC	*,AR1
		SPM		0
		SBRK    #02h
		LAR		AR0,*-
		PSHD	*
		RET        
		.end