rmp_cntl.asm

TI的digital motor control lib的源代码。了解TI的编程规范

;=====================================================================================
; File name:        RMP_CNTL.ASM                      
;                    
; Originator:	Digital Control Systems Group
;			Texas Instruments
;
; Description:                                 
; This file contains source for the Ramp control function.
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 9-15-2000	Release	Rev 1.0
;================================================================================
; Applicability: F240,F241,C242,F243,F24xx.  (Peripheral Independent).
;================================================================================
; Routine Name: rmp_cntl_calc   					    Type: C Callable
;  
;  C prototype : void rmp_cntl_calc(struct RMPCNTL *p);
;
;        The struct object is defined in the header file "rmp_cntl.h" as follows:
;
; typedef struct { int  target_value; 	/* Input: Target input (Q15) */
;				  int  rmp_dly_max;		/* Parameter: Maximum delay rate */			
;		 	 	  int  rmp_lo_limit;	/* Parameter: Minimum limit (Q15) */					  
;				  int  rmp_hi_limit;	/* Parameter: Maximum limit (Q15) */
;				  int  rmp_delay_cntl;  /* Variable: Incremental delay  */	
;				  int  setpt_value;		/* Output: Target output (Q15) */				 
;				  int  s_eq_t_flg;		/* Output: Flag output */
;		  	  	  int  (*calc)();	  	/* Pointer to calculation function */ 
;				 } RMPCNTL;	            
;	 		 
;        Frame Usage Details:
;     step   |      a      |      b       |      c       |     d     
;____________|_____________|______________|______________|_____________
;     FR0  	 |  not used   |              | 			 |     
;
;
;
;================================================================================
                .def        _rmp_cntl_calc
;================================================================================
__rmp_cntl_calc_framesize .set 0000h
;================================================================================
_rmp_cntl_calc:
 													; Assume now ARP=AR1
     	POPD	*+									; Keep return address
       	SAR  	AR0,*+								; Keep old frame pointer (FP)
       	SAR   	AR1,*								; Keep old stack pointer (SP)
       	LARK  	AR0,__rmp_cntl_calc_framesize 		; Load AR0 with frame size	
       	LAR   	AR0,*0+,AR0							; AR0->FP0 (new FP), ARP=AR0

;================================================================================
		SBRK	#3		; ARP=AR0, AR0->FR0-3 (1st argument)		
;----------------------------------------------------------------------------------
		LAR		AR2,*	; ARP=AR0, AR0->target_value, AR2->target_value			
;----------------------------------------------------------------------------------
		ADRK	#3		; ARP=AR0, AR0->FR0, AR2->target_value 
;----------------------------------------------------------------------------------
		MAR		*,AR2  	; ARP=AR2, AR0->FR0, AR2->target_value
;----------------------------------------------------------------------------------
		SETC 	SXM		; Turn sign extension mode on
						; ARP=AR2, AR0->FR0, AR2->target_value
;----------------------------------------------------------------------------------
		SETC	OVM		; Set overflow mode
						; ARP=AR2, AR0->FR0, AR2->target_value
;----------------------------------------------------------------------------------
 		SPM     0       ; Reset product mode
       					; ARP=AR2, AR0->FR0, AR2->target_value 
;----------------------------------------------------------------------------------
      	LACC	*		; ACC = target_value  (Q15)
      					; ARP=AR2, AR0->FR0, AR2->target_value
;----------------------------------------------------------------------------------
      	ADRK	#5		; ARP=AR2, AR0->FR0, AR2->setpt_value
;----------------------------------------------------------------------------------
      	SUB		*-      ; ACC = target_value - setpt_value  (Q15)
      					; ARP=AR2, AR0->FR0, AR2->rmp_delay_cntl  
;----------------------------------------------------------------------------------
		BCND	SET_FLG,EQ ; If set point = target
						; Set s_eq_t_flg = 7FFFh then exit
						; ARP=AR2, AR0->FR0, AR2->rmp_delay_cntl	
;----------------------------------------------------------------------------------
		LACC	*		; ACC = rmp_delay_cntl
						; ARP=AR2, AR0->FR0, AR2->rmp_delay_cntl 
;----------------------------------------------------------------------------------
		ADD		#1		; ACC = rmp_delay_cntl + 1
						; ARP=AR2, AR0->FR0, AR2->rmp_delay_cntl  
;----------------------------------------------------------------------------------
		SACL	*		; rmp_delay_cntl = rmp_delay_cntl + 1
						; ARP=AR2, AR0->FR0, AR2->rmp_delay_cntl
;----------------------------------------------------------------------------------
    	SBRK	#3		; ARP=AR2, AR0->FR0, AR2->rmp_dly_max
;----------------------------------------------------------------------------------
		SUB		*-		; ACC = rmp_delay_cntl + 1 - rmp_dly_max 
						; ARP=AR2, AR0->FR0, AR2->target_value	
;----------------------------------------------------------------------------------
		BCND	SRC_EXIT, LT ; Branch to SRC_EXIT if ACC > 0
						; ARP=AR2, AR0->FR0, AR2->target_value
;----------------------------------------------------------------------------------
CHNG_VALUE	           	; ARP=AR2, AR0->FR0, AR2->target_value
;----------------------------------------------------------------------------------
       	LACC	*		; ACC = target_value  (Q15)
       					; ARP=AR2, AR0->FR0, AR2->target_value
;----------------------------------------------------------------------------------
		ADRK	#5		; ARP=AR2, AR0->FR0, AR2->setpt_value
;----------------------------------------------------------------------------------
		SUB		*		; ACC = target_value - setpt_value (Q15)
						; ARP=AR2, AR0->FR0, AR2->setpt_value
;----------------------------------------------------------------------------------
		BCND	INC_VALUE, GT ; Branch to incremental if target_value>setpt_value 
                        ; ARP=AR2, AR0->FR0, AR2->setpt_value
;----------------------------------------------------------------------------------
DEC_VALUE               ; ARP=AR2, AR0->FR0, AR2->setpt_value
;----------------------------------------------------------------------------------
    	LACC	*		; ACC = setpt_value  (Q15)
    					; ARP=AR2, AR0->FR0, AR2->setpt_value
;----------------------------------------------------------------------------------
       	SUB		#1		; ACC = setpt_value - 1  (Q15)
       					; ARP=AR2, AR0->FR0, AR2->setpt_value
;----------------------------------------------------------------------------------
       	SACL	*		; setpt_value = setpt_value - 1  (Q15)
       					; ARP=AR2, AR0->FR0, AR2->setpt_value
;----------------------------------------------------------------------------------
		SBRK	#3		; ARP=AR2, AR0->FR0, AR2->rmp_lo_limit 
;----------------------------------------------------------------------------------
		SUB		*		; ACC = setpt_value - 1 - rmp_lo_limit (Q15)
						; ARP=AR2, AR0->FR0, AR2->rmp_lo_limit
;----------------------------------------------------------------------------------
 		ADRK	#3		; ARP=AR2, AR0->FR0, AR2->setpt_value
;----------------------------------------------------------------------------------
      	BCND	SRC_1, GEQ ; Branch to SRC_1 if ACC >= 0
      	               	; ARP=AR2, AR0->FR0, AR2->setpt_value 	
;----------------------------------------------------------------------------------
        SBRK	#3		; ARP=AR2, AR0->FR0, AR2->rmp_lo_limit
;----------------------------------------------------------------------------------
       	LACC	*		; ACC = rmp_lo_limit  (Q15)
       					; ARP=AR2, AR0->FR0, AR2->rmp_lo_limit
;----------------------------------------------------------------------------------
       	ADRK	#3		; ARP=AR2, AR0->FR0, AR2->setpt_value    	
;----------------------------------------------------------------------------------
		SACL	*		; setpt_value = rmp_lo_limit  (Q15)
						; ARP=AR2, AR0->FR0, AR2->setpt_value 
;----------------------------------------------------------------------------------
      	B		SRC_1	; Branch to SRC_1
      					; ARP=AR2, AR0->FR0, AR2->setpt_value
;----------------------------------------------------------------------------------
INC_VALUE               ; ARP=AR2, AR0->FR0, AR2->setpt_value
;----------------------------------------------------------------------------------
  		LACC	*	   	; ACC = setpt_value  (Q15)
  						; ARP=AR2, AR0->FR0, AR2->setpt_value
;----------------------------------------------------------------------------------
      	ADD		#1		; ACC = set_value + 1  (Q15)
      					; ARP=AR2, AR0->FR0, AR2->setpt_value
;----------------------------------------------------------------------------------
      	SACL	*		; setpt_value = set_value + 1  (Q15)
      					; ARP=AR2, AR0->FR0, AR2->setpt_value
;----------------------------------------------------------------------------------
     	SBRK	#2		; ARP=AR2, AR0->FR0, AR2->rmp_hi_limit
;----------------------------------------------------------------------------------
		SUB		*		; ACC = set_value + 1 - rmp_hi_limit (Q15)
						; ARP=AR2, AR0->FR0, AR2->rmp_hi_limit
;----------------------------------------------------------------------------------
		ADRK	#2		; ARP=AR2, AR0->FR0, AR2->setpt_value
;----------------------------------------------------------------------------------
		BCND	SRC_1, LEQ ; Branch to SRC_1 if ACC <= 0
						; ARP=AR2, AR0->FR0, AR2->setpt_value
;----------------------------------------------------------------------------------
		SBRK	#2		; ARP=AR2, AR0->FR0, AR2->rmp_hi_limit
;----------------------------------------------------------------------------------
     	LACC	*		; ACC = rmp_hi_limit  (Q15)
     					; ARP=AR2, AR0->FR0, AR2->rmp_hi_limit
;----------------------------------------------------------------------------------
		ADRK	#2		; ARP=AR2, AR0->FR0, AR2->setpt_value
;----------------------------------------------------------------------------------
		SACL	*		; setpt_value = rmp_hi_limit (Q15)
						; ARP=AR2, AR0->FR0, AR2->setpt_value
;----------------------------------------------------------------------------------
SRC_1                   ; ARP=AR2, AR0->FR0, AR2->setpt_value
;----------------------------------------------------------------------------------
		SBRK	#1	   	; ARP=AR2, AR0->FR0, AR2->rmp_delay_cntl
;----------------------------------------------------------------------------------
      	SPLK	#0,*	; rmp_delay_cntl = 0
                        ; ARP=AR2, AR0->FR0, AR2->rmp_delay_cntl
;----------------------------------------------------------------------------------
		SBRK	#4		; ARP=AR2, AR0->FR0, AR2->target_value
;----------------------------------------------------------------------------------
SRC_EXIT                ; ARP=AR2, AR0->FR0, AR2->target_value
;----------------------------------------------------------------------------------
		B		RMPCNTL_END ; Branch to RMPCNTL_END	
						; ARP=AR2, AR0->FR0, AR2->target_value
;----------------------------------------------------------------------------------
       	ADRK	#4		; ARP=AR2, AR0->FR0, AR2->rmp_delay_cntl 
;----------------------------------------------------------------------------------
SET_FLG 				; ARP=AR2, AR0->FR0, AR2->rmp_delay_cntl
;----------------------------------------------------------------------------------
		ADRK	#2		; ARP=AR2, AR0->FR0, AR2->s_eq_t_flg
;----------------------------------------------------------------------------------
    	SPLK	#7FFFh,* ; s_eq_t_flg = 7FFFh
    					; ARP=AR2, AR0->FR0, AR2->s_eq_t_flg
;----------------------------------------------------------------------------------
RMPCNTL_END
;----------------------------------------------------------------------------------
_rmp_cntl_calc_exit:
       	MAR     *,AR1   ; can be removed if this condition is met on
       	                ; every path to this code. (i.e., ARP=AR1 here)

        CLRC	OVM
        CLRC	SXM

    	SBRK 	#(__rmp_cntl_calc_framesize +1)
        LAR  	AR0,*-
        PSHD	*
        
        RET