pid_id.asm

无刷直流电机的无传感器控制TI程序

;===========================================================================
; File Name:		pid_id.asm
;
; Module Name: 		D_PID_REG				     
;                
; Initialization Routine:  D_PID_REG_INIT
;	
; Description:	
;
;					  |~~~~~~~~~~~~~~~~~|
;	   	        D_fb  o---------->|	            |
;	   	        D_ref o---------->|    D_PID_REG    |----->o  D_out
;					  |		    |
;					  |_________________|
;
;
; Originator:	Digital control systems Group - Texas Instruments
;
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 9-15-2000	Release	Rev 1.0	
;===========================================================================
;(To use this Module, copy this section to main system file)
;		.ref	D_PID_REG, D_PID_REG_INIT			;function call
;		.ref	D_fb, D_ref						;Inputs
;		.ref	D_out							;Outputs
;===========================================================================
		.def	D_PID_REG, D_PID_REG_INIT			;function call
		.def	D_fb, D_ref						;Inputs
		.def	D_out							;Outputs
;===========================================================================
		.include 	"x24x_app.h"  

	;Global vars

D_fb		.usect	"d_pid"	,1
D_ref		.usect	"d_pid"	,1
D_out		.usect	"d_pid"	,1

	;Local vars

MAX_POS_LIMIT	.set	400
MAX_NEG_LIMIT	.set	-100 


D_Un_H_0		.usect	"d_pid"	,1
D_Un_L_0		.usect	"d_pid"	,1
D_En0			.usect	"d_pid"	,1
D_En1			.usect	"d_pid"	,1	
D_En2			.usect	"d_pid"	,1	
D_K0			.usect	"d_pid"	,1	
D_K1			.usect	"d_pid"	,1	
D_K2			.usect	"d_pid"	,1	
D_PID_CNTR		.usect	"d_pid"	,1


		.def	D_Un_H_0
		.def	D_Un_L_0
		.def	D_En0
		.def  	D_En1
		.def  	D_En2
		.def	D_K0
		.def	D_K1
		.def	D_K2
        .def    D_PID_CNTR
;===============
D_PID_REG_INIT:
;===============

 		LDP		#D_K0
	    SPLK	#02000h,D_K0
		SPLK	#0e010h,D_K1
		SPLK	#0,D_K2

		SPLK	#0,D_En0
		SPLK	#0,D_En1
		SPLK	#0,D_En2

		SPLK	#0,D_Un_H_0
		SPLK	#0,D_Un_L_0

        RET

;===============
D_PID_REG:
;===============
  
		LDP		#D_PID_CNTR
	
		LACC 	D_PID_CNTR		;See if it is time to run PID
		SUB		#1			;dec CNTR
		SACL    D_PID_CNTR                ;update CNTR
		BCND	D_EXIT,NEQ		;if counter is still non-zero,skip.
	    LACC	#20			;if zero, reload and run fwd into pid.
        SACL	D_PID_CNTR		;

   
 		LDP		#D_ref
		SETC	SXM           		; Sign extension mode
        SETC	OVM              	; Overflow mode
        SPM	1                 	; SPM set for Q15 multiplication 	   


        LACC    D_ref
        SUB     D_fb
        SACL    D_En0

    
;===========================================================================
; Routine Name:  P I D _ R E G 				       Routine Type: SR
;
; Description :  Computes a speed command from the speed setpoint and a meas
;		 measured speed.
;
;===========================================================================
                ZALS    D_Un_L_0         ;ACC = Un-1
                ADDH    D_Un_H_0
                LT      D_En2            ;T = En2
MPYK2:          MPY     D_K2             ;P = K2.En-2
                LTD     D_En1            ;ACC = Un-2 + K2.En-2
MPYK1:          MPY     D_K1             ;P = K1.En-1
                LTD     D_En0            ;ACC = Un-2 + K1.En-1 + K2.En-2
MPYK0:          MPY     D_K0             ;P = K0.En-0
                APAC                   ;ACC = Un-2 + K0.En-0 + K1.En-1 + K2.En-2
UH_RAW:         SACH    D_Un_H_0         ;Un-0 = ACC
UL_RAW:         SACL    D_Un_L_0         ;This is the 32 bit accumulation.


;===========================================================================
                LACC    D_Un_H_0         ;else keep current value
                SUB     #MAX_POS_LIMIT  
                BCND    D_PLUS_OK,LEQ    ;If maxed out, saturate at max -ve 

	    		SPLK    #MAX_POS_LIMIT,D_Un_H_0
            	SPLK    #0,D_Un_L_0
                B       D_EXIT

D_PLUS_OK:      LACC    D_Un_H_0         ;else keep current value
                SUB     #MAX_NEG_LIMIT
                BCND    D_NEG_OK,GEQ    ;If maxed out, saturate at max +ve

                SPLK    #MAX_NEG_LIMIT,D_Un_H_0		;Saturation control
            	SPLK    #0,D_Un_L_0

;===========================================================================
D_NEG_OK:
D_EXIT:    LACC    D_Un_H_0  
           ADD     D_ref
	       SACL    D_out


                SPM     0
                CLRC    SXM



            RET