pid_reg3.asm

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

;=====================================================================================
; File name:        PID_REG3.ASM                     
;                    
; Originator:	Digital Control Systems Group
;			Texas Instruments
;
; Description:  Digital PID controller with anti-windup correction               
; 
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 02-08-2001	Release	Rev 1.0
;================================================================================
; Applicability: F240,F241,C242,F243,F24xx.  (Peripheral Independent).
;================================================================================
; Routine Name: pid_reg3_calc 				  					    Type: C Callable
;  
;  C prototype : void pid_reg3_calc(struct PIDREG3 *p);
;
;        The struct object is defined in the header file "pid_reg3.h" as follows:
;
; typedef struct {  int  pid_ref_reg3;   	/* Input: Reference input (Q15) */
;				  int  pid_fdb_reg3;   	/* Input: Feedback input (Q15) */
;				  int  e_reg3;			/* Variable: Error  (Q14)  */
;				  int  Kp_reg3;			/* Parameter: Proportional gain (Q15) */
;				  int  up_reg3;			/* Variable: Proportional output (Q14) */
;				  int  ui_hi_reg3;		/* Variable: Integral output  (Q30)  */
;				  int  ui_lo_reg3;		/* Variable: Integral output  (Q30)  */	
;				  int  ud_lo_reg3;		/* Variable: Derivative output  (Q30)  */	
;				  int  ud_hi_reg3;		/* Variable: Derivative output  (Q30)  */
;				  int  uprsat_reg3;	 	/* Variable: Pre-saturated output (Q14) */
;				  int  pid_out_max;		/* Parameter: Maximum output  (Q15)  */
;				  int  pid_out_min;		/* Parameter: Minimum output  (Q15)  */
;				  int  pid_out_reg3;   	/* Output: PID output  (Q15)  */
;				  int  saterr_reg3;		/* Variable: Saturated difference (Q14) */
;				  int  Ki_reg3;			/* Parameter: Integral gain  (Q31-16bit) */
;				  int  Kc_reg3;			/* Parameter: Integral correction gain (Q15) */
;				  int  Kd_reg3; 		/* Parameter: Derivative gain  (Q14)  */
;				  int  up1_reg3;		/* Variable: Previous proportional output (Q14) */
;		 	 	  int  (*calc)();	  	/* Pointer to calculation function */ 
;				 } PIDREG3;	          
;	 		 
;        Frame Usage Details:
;     step   |      a      |      b       |      c       |     d     
;____________|_____________|______________|______________|_____________
;     AR0  	 |             |              | 			 |     
;
;================================================================================
                .def        _pid_reg3_calc
;================================================================================
__pid_reg3_calc_framesize .set 0000h
;================================================================================
_pid_reg3_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,__pid_reg3_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->pid_ref_reg3, AR2->pid_ref_reg3
;----------------------------------------------------------------------------------
		ADRK	#3		; ARP=AR0, AR0->FR0, AR2->pid_ref_reg3
;----------------------------------------------------------------------------------
		MAR		*,AR2  	; ARP=AR2, AR0->FR0, AR2->pid_ref_reg3
;----------------------------------------------------------------------------------
		SETC 	SXM		; Turn sign extension mode on
						; ARP=AR2, AR0->FR0, AR2->pid_ref_reg3
;----------------------------------------------------------------------------------
		SETC	OVM		; Set overflow mode
						; ARP=AR2, AR0->FR0, AR2->pid_ref_reg3
;----------------------------------------------------------------------------------
 		SPM     0       ; Reset product mode
       					; ARP=AR2, AR0->FR0, AR2->pid_ref_reg3
;----------------------------------------------------------------------------------
    	LACC	*+,15	; ACC = pid_ref_reg3  (Q30)
    					; ARP=AR2, AR0->FR0, AR2->pid_fdb_reg3
;----------------------------------------------------------------------------------
     	SUB		*+,15	; ACC = pid_ref_reg3 - pid_fdb_reg3 (Q30)
    					; ARP=AR2, AR0->FR0, AR2->e_reg3
;----------------------------------------------------------------------------------
      	SACH	*		; e_reg3 = pid_ref_reg3 - pid_fdb_reg3 (Q14)
    					; ARP=AR2, AR0->FR0, AR2->e_reg3
;----------------------------------------------------------------------------------
     	LT		*+		; TREG = e_reg3  (Q14)
     					; ARP=AR2, AR0->FR0, AR2->Kp_reg3 	
;----------------------------------------------------------------------------------
      	MPY		*+  	; PREG = Kp_reg3*e_reg3  (Q29)
     					; ARP=AR2, AR0->FR0, AR2->up_reg3 
;----------------------------------------------------------------------------------
     	PAC				; ACC = Kp_reg3*e_reg3  (Q29)
     					; ARP=AR2, AR0->FR0, AR2->up_reg3         
;----------------------------------------------------------------------------------
     	SACH	*+,1	; up_reg3 = Kp_reg3*e_reg3  (Q14)
     					; ARP=AR2, AR0->FR0, AR2->ui_hi_reg3 	
;----------------------------------------------------------------------------------
      	LACC	*+,16	; ACC = ui   (Q30)
      					; ARP=AR2, AR0->FR0, AR2->ui_lo_reg3 
;----------------------------------------------------------------------------------
     	ADDS	*+		; ACC = ui   (Q30)
     					; ARP=AR2, AR0->FR0, AR2->ud_lo_reg3	
;----------------------------------------------------------------------------------
      	ADDS	*+		; ACC = ui + ud  (Q30)
      					; ARP=AR2, AR0->FR0, AR2->ud_hi_reg3 	
;----------------------------------------------------------------------------------
      	ADDH	*		; ACC = ui + ud  (Q30)
      					; ARP=AR2, AR0->FR0, AR2->ud_hi_reg3
;----------------------------------------------------------------------------------
    	SBRK	#4		; ARP=AR2, AR0->FR0, AR2->up_reg3
;----------------------------------------------------------------------------------
		ADDH	*		; ACC = up + ui + ud  (Q30)
						; ARP=AR2, AR0->FR0, AR2->up_reg3      	
;----------------------------------------------------------------------------------
      	ADRK	#5		; ARP=AR2, AR0->FR0, AR2->uprsat_reg3
;----------------------------------------------------------------------------------
		SACH	*		; uprsat_reg3 = up + ui + ud  (Q14)
						; ARP=AR2, AR0->FR0, AR2->uprsat_reg3  
;----------------------------------------------------------------------------------
       	LACC	*+,16	; ACC = uprsat_reg3   (Q14)
       					; ARP=AR2, AR0->FR0, AR2->pid_out_max
;----------------------------------------------------------------------------------
  		SUB		*-,15	; ACC = uprsat_reg3 - pid_out_max  (Q14)
       					; ARP=AR2, AR0->FR0, AR2->uprsat_reg3
;----------------------------------------------------------------------------------
      	BCND	SAT_MAX_REG3,GT ; Branch to SAT_MAX_REG3 if uprsat_reg3 > pid_out_max  
      	                ; ARP=AR2, AR0->FR0, AR2->uprsat_reg3
;----------------------------------------------------------------------------------
     	LACC	*,16	; ACC = uprsat_reg3   (Q14)
       					; ARP=AR2, AR0->FR0, AR2->uprsat_reg3
;----------------------------------------------------------------------------------
       	ADRK	#2		; ARP=AR2, AR0->FR0, AR2->pid_out_min
;----------------------------------------------------------------------------------
     	SUB		*,15	; ACC = uprsat_reg3 - pid_out_min   (Q14)
       					; ARP=AR2, AR0->FR0, AR2->pid_out_min
;----------------------------------------------------------------------------------
		SBRK	#2		; ARP=AR2, AR0->FR0, AR2->uprsat_reg3 
;----------------------------------------------------------------------------------
       	BCND	SAT_MIN_REG3,LT ; Branch to SAT_MIN_REG3 if uprsat_reg3 < pid_out_min  
      	                ; ARP=AR2, AR0->FR0, AR2->uprsat_reg3     	
;----------------------------------------------------------------------------------
      	LACC	*,16	; ACC = uprsat_reg3   (Q30)
      					; ARP=AR2, AR0->FR0, AR2->uprsat_reg3 
;----------------------------------------------------------------------------------
      	ADRK	#3		; ARP=AR2, AR0->FR0, AR2->pid_out_reg3 
;----------------------------------------------------------------------------------
    	SACH	*,1		; pid_out_reg3 = uprsat_reg3   (Q15)