;===========================================================================
; File Name:	pid_reg2.asm
;
; Module Name: 	PID_REG2				     
;
; Initialization Routine: PID_REG2_INIT
;
; Originator:	Digital Control Systems Group
;			Texas Instruments
;
; Description:	Implements a PI regulator with anti windup control
;
;				|~~~~~~~~~~~~~~~|
;	pid_fb_reg2  o--------->|		|
;	pid_ref_reg2 o--------->|   PID_REG2	|----->o pid_out_reg2
;				|		|
;				|_______________|
;
;
;
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 9-15-2000	Release	Rev 1.0
;===========================================================================
;(To use this Module, copy this section to main system file)
;		.ref	PID_REG2, PID_REG2_INIT			;function call
;		.ref	pid_fb_reg2, pid_ref_reg2						;Inputs
;		.ref	pid_out_reg2, 				;Output
;		.ref	pid_max_reg2, pid_min_reg2		
;		.ref	K0_reg2, K1_reg2, Kc_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
;===========================================================================
		.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	#0080h,K0_reg2  	;Q9
   		SPLK	#0140h,K1_reg2     	;Q13
		SPLK	#0506h,Kc_reg2          ;Q13
        	RET

;===============================================================================
PID_REG2:
;===============================================================================
                SPM	0    
		
		ldp	#pid_ref_reg2 
		LACC    pid_ref_reg2
		SUB     pid_fb_reg2
		SACL    En0_reg2
							 
		lacc	Un_reg2,9       ;ACC(32-bit)(Q24), Un(Q15)
		                
		LT	En0_reg2	
		MPY	K0_reg2		;Q9*Q15
										
		APAC			;Q24 

                rpt	#6
                norm	*  		;Q31
		sach	Upi_reg2   	;Q15
		
                LACC	Upi_reg2         
                ADD	pid_min_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*Q13
		pac   	                ;Q28
 
		lt	En0_reg2 	;Q15
		mpy	K1_reg2         ;Q15*Q13
		apac 			;Q28					 
                ADD	Un_reg2,13    	;Q28
		rpt	#2		             
	 	norm	* 		;Q31                 	
		sach	Un_reg2         ;Q15 

		RET