;===========================================================================
; File Name:	pid_reg1.asm	
;
; Module Name: 	PID_REG1				     
;
; Initialization Routine: PID_REG1_INIT
;
; Originator:	Digital Control Systems Group
;			Texas Instruments
;
; Description:	Digital PID controller without anti-windup correction
;
;				|~~~~~~~~~~~~~~~~~~~|
;				| 		    |
;	pid_ref_reg1 o--------->|Q15   PID_REG1	 Q15|----->o pid_out_reg1
;	pid_fb_reg1  o--------->|Q15		    |
;				|___________________|
;
; Target dependency:	C2xx core only
;
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 9-15-2000	Release Rev 1.0
;===========================================================================
;(To use this Module, copy this section to main system file)
;		.ref	PID_REG1, PID_REG1_INIT					;function call
;		.ref	pid_fb_reg1, pid_ref_reg1				;Inputs
;		.ref	pid_out_reg1							;Output
;===========================================================================
		.def	PID_REG1, PID_REG1_INIT					;function call
		.def	pid_fb_reg1, pid_ref_reg1				;Inputs
		.def	pid_out_reg1							;Output
;===========================================================================
Kp_REG1_		.set	2000h	      ; for Kp_reg1 
Ki_HI_REG1_		.set	0010h	      ; for Ki_low_reg1   (Ki=0 for PD)
Ki_LO_REG1_		.set	0BEEFh	      ; for Ki_high_reg1  (Ki=0 for PD)
Kd_REG1_		.set	0000h	      ; for Kd_reg1       (Kd=0 for PI)
PID_OUT_MAX_    .set	0666h	      ; for pid_out_max  
PID_OUT_MIN_    .set	0000h	      ; for pid_out_min 

		.include 	"x24x_app.h"  

;===========================================================================
;Variable Definitions for PID_REG1 module
;---------------------------------------------------------------------------
Kp_reg1			.usect	"pid_reg1",1     		; Kp = Q15
Ki_low_reg1		.usect	"pid_reg1",1            ; Ki = Q31
Ki_high_reg1	.usect	"pid_reg1",1            ; Ki = Q31
Kd_reg1			.usect	"pid_reg1",1            ; Kd = Q15
K0_low_reg1		.usect	"pid_reg1",1            ; K0 = Q31
K0_high_reg1    .usect	"pid_reg1",1            ; K0 = Q31
K1_reg1			.usect	"pid_reg1",1            ; K1 = Q15
pid_fb_reg1		.usect	"pid_reg1",1
pid_ref_reg1	.usect	"pid_reg1",1
pid_out_reg1	.usect	"pid_reg1",1
pid_out1_reg1   .usect	"pid_reg1",1
pid_e0_reg1		.usect	"pid_reg1",1
pid_e1_reg1		.usect	"pid_reg1",1
pid_e2_reg1		.usect	"pid_reg1",1
tmp1_low_reg1	.usect	"pid_reg1",1
tmp1_high_reg1	.usect	"pid_reg1",1
tmp2_low_reg1	.usect	"pid_reg1",1
tmp2_high_reg1	.usect	"pid_reg1",1
tmp3_reg1		.usect	"pid_reg1",1
abs_e0_reg1		.usect	"pid_reg1",1
sign_reg1       .usect	"pid_reg1",1

;=========================================================================
PID_REG1_INIT:
;=========================================================================
        LDP		#Kp_reg1                      		
 		
 		SPLK	#Kp_REG1_,Kp_reg1
		SPLK	#Ki_LO_REG1_,Ki_low_reg1
		SPLK	#Ki_HI_REG1_,Ki_high_reg1			
		SPLK	#Kd_REG1_,Kd_reg1
		SPLK	#0,pid_e1_reg1
		SPLK	#0,pid_e2_reg1
		SPLK	#0,pid_out1_reg1  

        RET

;===============================================================================
PID_REG1:
;===============================================================================

    	SETC	SXM           		; Sign extension mode
        SETC	OVM              	; Overflow mode
        SPM		0                	; Reset SPM 	   

; Converting from Kp, Ki, Kd to K0, K1 (Note: K2 = Kd)
		LDP		#Kp_reg1
		LACC	Ki_high_reg1,16     ; ACC = Ki              	(Q31)
		ADDS	Ki_low_reg1			; ACC = Ki 					(Q31)		
 		ADD		Kp_reg1,16			; ACC = Kp + Ki				(Q31)
		ADD		Kd_reg1,16			; ACC = Kp + Ki + Kd		(Q31)
		SACH    K0_high_reg1		; K0 = Kp + Ki + Kd			(Q31)
		SACL	K0_low_reg1			; K0 = Kp + Ki + Kd			(Q31)
		
		LACC	Kd_reg1,16			; ACC = Kd					(Q15)
		SFL							; ADD = 2*Kd				(Q15)	
		ADD		Kp_reg1,16			; ACC = 2*Kd+Kp				(Q15)				
		SACH 	K1_reg1				; K1 = 2*Kd+Kp				(Q15)

; e(k) = ref(k)-fb(k) => Q15 = Q15 - Q15 
		LACC    pid_ref_reg1        ; ACC =  pid_ref_reg1               (Q15)
		SUB     pid_fb_reg1         ; ACC = pid_ref_reg1 - pid_fb_reg1  (Q15)
		SACL    pid_e0_reg1         ; e(k) = pid_ref_reg1 - pid_fb_reg1 (Q15)

; tmp1 = -K1*e(k-1)+K2*e(k-2) => Q31 = -Q15*Q15 + Q15*Q15 		
   		LT		Kd_reg1				; TREG = K2   						(Q15)
		MPY		pid_e2_reg1			; PREG = K2*e(k-2)   				(Q30)
		PAC							; ACC = K2*e(k-2)   				(Q30)
		LT		K1_reg1				; TREG = K1   						(Q15)
		MPY		pid_e1_reg1			; PREG = K1*e(k-1)   				(Q30)
		SPAC						; ACC = -K1*e(k-1)+K2*e(k-2)   		(Q30)	
        SACH	tmp1_high_reg1,1	; tmp1 = -K1*e(k-1)+K2*e(k-2)   	(Q31)
        SACL	tmp1_low_reg1,1		; tmp1 = -K1*e(k-1)+K2*e(k-2)   	(Q31)
                
; tmp2 = K0*e(k)   =>  Q31 = Q31*Q15   	   	
; check sign for "error" only   
        
  		LACC	pid_e0_reg1      	; ACC = e(k)	
   		SACL	sign_reg1			; Sign (0=+,1=-) for K0*e(k)

; take absolute for "pid_e2_reg1" only because "K0" is always positive    	   	

   		ABS							; ACC = |e(k)|
   		SACL	abs_e0_reg1			; |e(k)| = ACC low  	   	       
   	   	
; now they're positive.    	   	
		LT		abs_e0_reg1   		; TREG = |e(k)|            
   		MPYU	K0_low_reg1    		; PREG = K0_low*|e(k)| 	   
  		SPH		tmp2_low_reg1  		; tmp2_low = PREG high      
  		MPYU	K0_high_reg1		; PREG = K0_high*|e(k)|
   		PAC							; ACC = K0_high*|e(k)|
   		ADDS 	tmp2_low_reg1 		; ACC = K0_high*|e(k)| + tmp2_low
   		SACH	tmp2_high_reg1,1	; tmp2_high = ACC high   	(Q31)
   		SACL	tmp2_low_reg1,1		; tmp2_low = ACC low     	(Q31)

; check the sign condition
		LACC	sign_reg1			; ACC = sign
		BCND  	DONE_REG1, GT      	; Check sign = positive ?
		LACC	tmp2_high_reg1,16  	; ACC high= tmp2_high
		ADDS	tmp2_low_reg1		; ACC low = tmp2_low
		NEG							; Make the result negative
  		SACH	tmp2_high_reg1		; tmp2_high = ACC high  
  		SACL	tmp2_low_reg1		; tmp2_low = ACC low
DONE_REG1	

; tmp2 + tmp1 = tmp3  => Q31 + Q31 = Q15
		LACC	tmp1_high_reg1,16  	; ACC high = tmp1_high			(Q31)
   		ADDS	tmp1_low_reg1		; ACC low = tmp1_low            (Q31)
		ADDS	tmp2_low_reg1		; ACC = tmp1_low+tmp2_low       (Q31)
		ADDH 	tmp2_high_reg1		; ACC = tmp1_high+tmp2_high     (Q31)
		SACH	tmp3_reg1			; tmp3 = tmp1_high+tmp1_high    (Q15) 

; u(k) = u(k-1) + tmp3  =>  Q15 = Q15 + Q15 
		LACC	pid_out1_reg1,16	; ACC = u(k-1)						(Q15)
		ADD		tmp3_reg1,16		; ACC = u(k-1)+K0*e(k)-K1*e(k-1)+K2*e(k-2)  (Q15)
		SACH	pid_out_reg1	    ; u(k) = u(k-1)+K0*e(k)-K1*e(k-1)+K2*e(k-2)  (Q15)

; If u(k) > u_max, u(k) = u_max. If u(k) < u_min, u(k) = u_min.  
		LACC 	pid_out_reg1		; ACC = u(k)						(Q15)
        SUB		#PID_OUT_MAX_		; ACC = u(k)-u_max					(Q15)
        BCND	SAT_MAX,GT			; Branch if saturated at max
        LACC  	pid_out_reg1		; ACC = u(k)						(Q15)
        SUB		#PID_OUT_MIN_		; ACC = u(k)-u_min					(Q15)
        BCND	SAT_MIN,LT			; Brnch if saturated at min
		B		REG1_END
SAT_MIN
        SPLK	#PID_OUT_MIN_,pid_out_reg1	; u(k) = u_min             (Q15)
   		B		REG1_END
SAT_MAX        
        SPLK    #PID_OUT_MAX_,pid_out_reg1	; u(k) = u_max             (Q15)
REG1_END 
; Updating the errors e(k-1), e(k-2) and output u(k-1)		
		LACC	pid_e1_reg1			; ACC = e(k-1)                 		(Q15)
		SACL	pid_e2_reg1	        ; e(k-2) = e(k-1)                   (Q15)
		LACC	pid_e0_reg1			; ACC = e(k)                 		(Q15)
		SACL	pid_e1_reg1	        ; e(k-1) = e(k)        			 	(Q15)				
        LACC	pid_out_reg1		; ACC = u(k)                 		(Q15)
		SACL	pid_out1_reg1	    ; u(k-1) = u(k)                     (Q15)
	
		CLRC    SXM
		
		RET