polar_rect.asm

f2407:电机控制

;-------------------------------------------------
;function name: void polar_rectangular(polar_rect_struct *p, int uout, int costheta, int sintheta);
;function parameter:
;	polar_rect *p;		//output structure
;	int uout;		//Q12, amplitude of reference voltage
;	int costheta;		//Q15 
;	int sintheta;		//Q15
;typedef struct
;	{	int ualfa;      //ualfa=uout*cos(theta), Q12
;		int ubeta;      //ubeta=uout*sin(theta), Q12
;	}polar_rect;

		.def _polar_rectangular
		.global _polar_rectangular
_polar_rectangular_frame .set 1              ;size of local frame

_polar_rectangular
		POPD	*+
		SAR		AR0,*+
		SAR		AR1,*
		LAR		AR0,#_polar_rectangular_frame  
		LAR		AR0,*0+,AR0
		LAR		AR2,*,AR2			;C compatible
									;STACK:sintheta/costheta/uout/ *p/address/AR0/AR1 
									;ARP:AR2, AR0:AR1, AR2:AR1
		SETC	SXM 				;SIGN EXTENSION MODE ENABLE
		SBRK	#3 					;AR2:*P
		LAR		AR3,*-				;ARP:AR2, AR2:uout, AR3:ualfa
		LT		*-			
		MPY		*-,AR3				;uout*cos(theta)
									;ARP:AR3, AR2:sintheta, AR3:ualfa
		PAC
		SACH	*+,1,AR2			;ualfa=uout*cos(theta)
		MPY		*,AR3
		PAC	
		SACH	*,1,AR1				;ubeta=uout*sin(theta)
		SBRK	#_polar_rectangular_frame+1
		LAR		AR0,*-
		PSHD	*
		RET