pid_reg1.asm

变频器程序

;=====================================================================================
; File name:        PID_REG1.ASM                     
;                    
; Originator:	Digital Control Systems Group
;			Texas Instruments
;
; Description:                                 
; PID Controller
;
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 9-15-2000	Release	Rev 1.0
;================================================================================
; Applicability: F240,F241,C242,F243,F24xx.  (Peripheral Independent).
;================================================================================
; Routine Name: pid_reg1_calc   					    Type: C Callable
;  
;  C prototype : void pid_reg1_calc(struct PIDREG1 *p);
;
;        The struct object is defined in the header file "pid_reg1.h" as follows:
;
; typedef struct 	{ int  pid_ref_reg1; 	/* Input: Reference input (Q15) */
;				  	  int  pid_fb_reg1;	 	/* Input: Feedback input (Q15) */
;				  	  int  Kp_reg1;			/* Parameter: Proportional gain (Q15) */			
;				  	  int  Ki_high_reg1;	/* Parameter: Integral gain (Q31) */
;				  	  int  Ki_low_reg1; 	/* Parameter: Integral gain (Q31) */		
;				  	  int  Kd_reg1;			/* Parameter: Derivative gain (Q15) */					  
;				  	  int  pid_out_max;		/* Parameter: Maximum PID output (Q15) */
;				  	  int  pid_out_min;		/* Parameter: Minimum PID output (Q15) */				 
;				  	  int  pid_e1_reg1;		/* History: Previous error at time = k-1 (Q15) */
;				  	  int  pid_e2_reg1;		/* History: Previous error at time = k-2 (Q15) */
;				  	  int  pid_out_reg1;	/* Output: PID output (Q15) */
;		  	  	  	  int  (*calc)();	/* Pointer to calculation function */ 
;					} PIDREG1;	                   
;	 		 
;        Frame Usage Details:
;     step   |      a      |      b       |      c       |     d     
;____________|_____________|______________|______________|_____________
;     FR0  	 |  sign_reg1  |              | 			 |     
;     FR1  	 | abs_e0_reg1 |              | 			 |     
;     FR2  	 | K0_low_reg1 |tmp2_low_reg1 |              |      	 	
;     FR3  	 | K0_high_reg1|tmp2_high_reg1|              |     
;     FR4  	 |   K1_reg1   |              |              |     
;
;
;================================================================================
                .def        _pid_reg1_calc
;================================================================================
__pid_reg1_calc_framesize .set 0005h
;================================================================================
_pid_reg1_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_reg1_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_reg1, AR2->pid_ref_reg1			
;----------------------------------------------------------------------------------
		ADRK	#3		; ARP=AR0, AR0->FR0, AR2->pid_ref_reg1 
;----------------------------------------------------------------------------------
		MAR		*,AR2  	; ARP=AR2, AR0->FR0, AR2->pid_ref_reg1
;----------------------------------------------------------------------------------
		SETC 	SXM		; Turn sign extension mode on
						; ARP=AR2, AR0->FR0, AR2->pid_ref_reg1
;----------------------------------------------------------------------------------
		SETC	OVM		; Set overflow mode
						; ARP=AR2, AR0->FR0, AR2->pid_ref_reg1
;----------------------------------------------------------------------------------
 		SPM     0       ; Reset product mode
       					; ARP=AR2, AR0->FR0, AR2->pid_ref_reg1 
;----------------------------------------------------------------------------------
      	LACC	*+		; ACC = pid_ref_reg1  (Q15)
      					; ARP=AR2, AR0->FR0, AR2->pid_fb_reg1
;----------------------------------------------------------------------------------
        SUB		*+,AR0	; ACC = pid_ref_reg1 - pid_fb_reg1 (Q15)
        				; ARP=AR2, AR0->FR0, AR2->Kp_reg1, ARP=AR0
;----------------------------------------------------------------------------------
		SACL	*+		; FR0=sign_reg1(pid_e0_reg1)=pid_ref_reg1-pid_fb_reg1 (Q15) 
						; ARP=AR0, AR0->FR1, AR2->Kp_reg1
;----------------------------------------------------------------------------------
		ABS				; ACC = |pid_ref_reg1 - pid_fb_reg1|  (Q15)
						; ARP=AR0, AR0->FR1, AR2->Kp_reg1
;----------------------------------------------------------------------------------
		SACL	*+,AR2	; FR1 = abs_e0_reg1 = |pid_ref_reg1 - pid_fb_reg1| (Q15) 
						; ARP=AR0, AR0->FR2, AR2->Kp_reg, ARP=AR2
;----------------------------------------------------------------------------------
		ADRK	#1		; ARP=AR2, AR0->FR2, AR2->Ki_high_reg1 
;----------------------------------------------------------------------------------
        LACC	*+,16	; ACC high = Ki_high_reg1  (Q31)
        				; ARP=AR2, AR0->FR2, AR2->Ki_low_reg1
;----------------------------------------------------------------------------------
        ADDS	*+		; ACC = Ki_high_reg1 + Ki_low_reg1  (Q31)
        				; ARP=AR2, AR0->FR2, AR2->Kd_reg1
;----------------------------------------------------------------------------------
		ADD		*,16	; ACC = Ki + Kd	 (Q31)		
                        ; ARP=AR2, AR0->FR2, AR2->Kd_reg1
;----------------------------------------------------------------------------------		
		SBRK	#3		; ARP=AR2, AR0->FR2, AR2->Kp_reg1	
;----------------------------------------------------------------------------------
        ADD		*,16,AR0 ; ACC = Kp + Ki + Kd (Q31)
        				; ARP=AR2, AR0->FR2, AR2->Kp_reg1, ARP=AR0
;----------------------------------------------------------------------------------
		SACL	*+		; FR2 = K0_low_reg1 = Kp + Ki + Kd (Q31)
						; ARP=AR0, AR0->FR3, AR2->Kp_reg1
;----------------------------------------------------------------------------------
		SACH	*+,AR2	; FR3 = K0_high_reg1 = Kp + Ki + Kd (Q31)
						; ARP=AR0, AR0->FR4, AR2->Kp_reg1, ARP=AR2
;----------------------------------------------------------------------------------
       	ADRK	#3		; ARP=AR2, AR0->FR4, AR2->Kd_reg1 
;----------------------------------------------------------------------------------
		LACC	*,16	; ACC = Kd_reg1	 (Q15)
					    ; ARP=AR2, AR0->FR4, AR2->Kd_reg1
;----------------------------------------------------------------------------------
        SFL				; ACC = 2*Kd_reg1  (Q15)
        				; ARP=AR2, AR0->FR4, AR2->Kd_reg1
;----------------------------------------------------------------------------------
       	SBRK	#3		; ARP=AR2, AR0->FR4, AR2->Kp_reg1
;----------------------------------------------------------------------------------
        ADD		*,16,AR0 ; ACC = 2*Kd_reg1 + Kp_reg1  (Q15)
        				; ARP=AR2, AR0->FR4, AR2->Kp_reg1, ARP=AR0
;----------------------------------------------------------------------------------
  		SACH	*		; FR4 = K1_reg1 = 2*Kd_reg1 + Kp_reg1  (Q15)
  						; ARP=AR0, AR0->FR4, AR2->Kp_reg1
;----------------------------------------------------------------------------------
        SBRK	#3		; ARP=AR0, AR0->FR1, AR2->Kp_reg1
;----------------------------------------------------------------------------------
       	LT		*+		; TREG = abs_e0_reg1  (Q15)
       					; ARP=AR0, AR0->FR2, AR2->Kp_reg1		
;----------------------------------------------------------------------------------
        MPYU	*		; PREG = abs_e0_reg1*K0_low_reg1
                        ; ARP=AR0, AR0->FR2, AR2->Kp_reg1
;----------------------------------------------------------------------------------
        SPH		*+		; FR2 = tmp2_low_reg1
        				; ARP=AR0, AR0->FR3, AR2->Kp_reg1	
;----------------------------------------------------------------------------------
     	MPYU	*-		; PREG = abs_e0_reg1*K0_high_reg1
     	                ; ARP=AR0, AR0->FR2, AR2->Kp_reg1
;----------------------------------------------------------------------------------
      	PAC				; ACC = abs_e0_reg1*K0_high_reg1
     	                ; ARP=AR0, AR0->FR2, AR2->Kp_reg1
;----------------------------------------------------------------------------------
		ADDS	*+		; ACC = abs_e0_reg1*K0_high_reg1 + tmp2_low_reg1
				        ; ARP=AR0, AR0->FR3, AR2->Kp_reg1
;----------------------------------------------------------------------------------
       	SACH	*-,1	; FR3 = tmp2_high_reg1 = abs_e0_reg1*K0	(Q31)
       	                ; ARP=AR0, AR0->FR2, AR2->Kp_reg1
;----------------------------------------------------------------------------------
     	SACL	*,1		; FR2 = tmp2_low_reg1 = abs_e0_reg1*K0	(Q31)