pid.asm

此源代码用于电力电子逆变器的同步锁相实现

;===========================================================================
; File Name:	pid_reg2.asm
;
; Module Name: 	PID_REG2				     
;
; Initialization Routine: PID_REG2_INIT
;
; Description:	Implements a PI regulator with anti windup control
;
;				|~~~~~~~~~~~~~~~|
;	pid_fb_reg2  o--------->|		|
;	pid_ref_reg2 o--------->|   PID_REG2	|----->o pid_out_reg2
;				|		|
;				|_______________|
;
;===========================================================================
		.def	PID_REG2, PID_REG2_INIT			;function call
		.def	pid_fb_reg2, pid_ref_reg2						;Inputs
		.def	pid_out_reg2, 				;Output
		.def	pid_max_reg2, pid_min_reg2
		.def	K0_reg2, K1_reg2, Kc_reg2   
		.def	En0_reg2,Un_reg2
;===========================================================================
		.include 	"x24x_app.h"  

;===========================================================================
;Variable Definitions for PID_REG2 module
;---------------------------------------------------------------------------

Un_reg2		.usect	"pid_reg2",1
En0_reg2	.usect	"pid_reg2",1
Upi_reg2	.usect	"pid_reg2",1
epi_reg2	.usect	"pid_reg2",1
K0_reg2		.usect	"pid_reg2",1
K1_reg2		.usect	"pid_reg2",1
Kc_reg2		.usect	"pid_reg2",1
pid_fb_reg2	.usect	"pid_reg2",1
pid_ref_reg2	.usect	"pid_reg2",1
pid_out_reg2	.usect	"pid_reg2",1
pid_max_reg2	.usect	"pid_reg2",1
pid_min_reg2	.usect	"pid_reg2",1	
               
;=========================================================================
PID_REG2_INIT:
;=========================================================================
        LDP    	#K0_reg2                      
		SPLK	#0,En0_reg2
		SPLK	#0,Un_reg2
		SPLK	#0,Upi_reg2
		SPLK	#0,epi_reg2
		SPLK	#7000h, pid_max_reg2	;Q15		
	;	SPLK	#09000h, pid_min_reg2	;Q15  
		SPLK	#8300h, pid_min_reg2
    ;	SPLK	#2000h,K0_reg2  	    ;Q15 0.25  
    	SPLK	#30000,K0_reg2			; Q15 1.2/2 1.2*0.00442*32767
   	;	 SPLK	#140,K0_reg2
   	;SPLK	#150,K1_reg2     	  ;Q15  
   		SPLK	#0,K1_reg2 
		SPLK		#65,Kc_reg2    
        
  ; 		SPLK	#65,K1_reg2
        RET

;===============================================================================
PID_REG2:
;===============================================================================
        SPM		0    
		SETC	SXM
		ldp		#pid_ref_reg2 
;=====================================
;PID SHU RU DE YI ZHONG FANG XIANG 		
		LACC    pid_ref_reg2
		SUB     pid_fb_reg2
		SACL    En0_reg2
;========================================

;=================================
;		LACC	pid_fb_reg2
;		SUB		pid_ref_reg2
;		sacl	En0_reg2
									 
		lacc	Un_reg2,15       ;ACC(32-bit)(Q30), Un(Q15)
;=============================================
;K0_reg2<1 shi de qing kuang		                
		LT	En0_reg2	
		MPY	K0_reg2		;Q15*Q15
										
		APAC			;Q30
;===============================================

;===============================================
;k0_reg2>1 shi de qing kuang
	;	LT		En0_reg2
	;	MPY		K0_reg2
	;	APAC
	;	APAC
;==================================================
      	sach	Upi_reg2,1   	;Q15
		
        LACC	Upi_reg2         
        ;ADD		pid_min_reg2   
  	   ; BCND  	SAT_MINUS_IO,LT  
  	   ADD		pid_max_reg2
  	   BCND		SAT_MINUS_IO,LT   
						  
        LACC  	Upi_reg2                          
		SUB		pid_max_reg2  
        BCND  	SAT_PLUS_IO, GEQ    
        lacc	Upi_reg2
		sacl	pid_out_reg2		;Q15
		B     	FWD_IO
                                             	;saturation control
SAT_MINUS_IO	
		lacc	pid_min_reg2
		sacl	pid_out_reg2                           
        B       FWD_IO  
                    
SAT_PLUS_IO      
        lacc	pid_max_reg2
		sacl	pid_out_reg2 
       
FWD_IO:  
                     
		LACC	pid_out_reg2	;Q15
		SUB		Upi_reg2     	;Q15  
		sacl	epi_reg2        ;Q15
	
		lt		epi_reg2        ;Q15 
		mpy		Kc_reg2       	;Q15*Q15
		pac   	                ;Q30
 
		lt		En0_reg2 	;Q15
		mpy		K1_reg2         ;Q15*Q15
		apac 			;Q30					 
        ADD		Un_reg2,15    	;Q30
		sach	Un_reg2,1         ;Q15 

		RET