pid_reg1.asm

变频器程序

       	                ; ARP=AR0, AR0->FR2, AR2->Kp_reg1
;----------------------------------------------------------------------------------
       	SBRK	#2		; ARP=AR0, AR0->FR0, AR2->Kp_reg1
;----------------------------------------------------------------------------------
      	LACC	*		; ACC = sign_reg1 (pid_e0_reg1)
      					; ARP=AR0, AR0->FR0, AR2->Kp_reg1
;----------------------------------------------------------------------------------
        ADRK	#3		; ARP=AR0, AR0->FR3, AR2->Kp_reg1 
;----------------------------------------------------------------------------------
       	BCND	DONE_REG1,GT ; Check sign = positive ?
       					; ARP=AR0, AR0->FR3, AR2->Kp_reg1
;----------------------------------------------------------------------------------
      	LACC	*-,16	; ACC high = tmp2_high_reg1
      					; ARP=AR0, AR0->FR2, AR2->Kp_reg1
;----------------------------------------------------------------------------------
      	ADDS	*		; ACC = tmp2_high_reg1 + tmp2_low_reg1  
      					; ARP=AR0, AR0->FR2, AR2->Kp_reg1
;----------------------------------------------------------------------------------
       	NEG				; Make the result negative
       					; ARP=AR0, AR0->FR2, AR2->Kp_reg1
;----------------------------------------------------------------------------------
      	SACL	*+		; tmp2_low_reg1 = ACC high	(Q31) 	     
      					; ARP=AR0, AR0->FR3, AR2->Kp_reg1
;----------------------------------------------------------------------------------
      	SACH	*		; tmp2_high_reg1 = ACC low  (Q31)
      					; ARP=AR0, AR0->FR3, AR2->Kp_reg1
;----------------------------------------------------------------------------------
DONE_REG1
;----------------------------------------------------------------------------------
        MAR		*+,AR2	; ARP=AR0, AR0->FR4, AR2->Kp_reg1, ARP=AR2 	 	
;----------------------------------------------------------------------------------
		ADRK	#3		; ARP=AR2, AR0->FR4, AR2->K2_reg1 (Kd_reg1)
;----------------------------------------------------------------------------------
        LT		* 		; TREG = K2_reg1	(Q15)
        				; ARP=AR2, AR0->FR4, AR2->K2_reg1
;----------------------------------------------------------------------------------
        ADRK	#4		; ARP=AR2, AR0->FR4, AR2->pid_e2_reg1
;----------------------------------------------------------------------------------
        MPY		*-		; PREG = K2_reg1*pid_e2_reg1 (Q30)
                        ; ARP=AR2, AR0->FR4, AR2->pid_e1_reg1
;----------------------------------------------------------------------------------
		PAC				; ACC = K2_reg1*pid_e2_reg1 (Q30)
                        ; ARP=AR2, AR0->FR4, AR2->pid_e1_reg1
;----------------------------------------------------------------------------------
     	LT		*,AR0	; TREG = pid_e1_reg1   (Q15)
     					; ARP=AR2, AR0->FR4, AR2->pid_e1_reg1, ARP=AR0  
;----------------------------------------------------------------------------------
		MPY		*-		; PREG = K1_reg1*pid_e1_reg1 (Q30)
						; ARP=AR0, AR0->FR3, AR2->pid_e1_reg1
;----------------------------------------------------------------------------------
    	SPAC			; ACC = -K1_reg1*pid_e1_reg1 + K2_reg1*pid_e2_reg1 (Q30)
    					; ARP=AR0, AR0->FR3, AR2->pid_e1_reg1
;----------------------------------------------------------------------------------
		SFL				; ACC = -K1_reg1*pid_e1_reg1 + K2_reg1*pid_e2_reg1 (Q31)
						; ARP=AR0, AR0->FR3, AR2->pid_e1_reg1
;----------------------------------------------------------------------------------
       	ADDH	*-		; ACC high = ACC high + tmp2_high_reg1  (Q31)
       					; ARP=AR0, AR0->FR2, AR2->pid_e1_reg1
;----------------------------------------------------------------------------------
       	ADDS	*-,AR2	; ACC low = ACC low + tmp2_low_reg1  (Q31)
       					; ARP=AR0, AR0->FR1, AR2->pid_e1_reg1, ARP=AR2  
;----------------------------------------------------------------------------------
		ADRK	#2		; ARP=AR2, AR0->FR1, AR2->pid_out_reg1   
;----------------------------------------------------------------------------------
       	ADD		*,16	; ACC = ACC + pid_out_reg1	(Q15)
       					; ARP=AR2, AR0->FR1, AR2->pid_out_reg1
;----------------------------------------------------------------------------------
      	SACH	*		; pid_out_reg1 = ACC + pid_out_reg1	(Q15)
      					; ARP=AR2, AR0->FR1, AR2->pid_out_reg1
;----------------------------------------------------------------------------------
       	LACC	*		; ACC = pid_out_reg1  (Q15)
       	  	            ; ARP=AR2, AR0->FR1, AR2->pid_out_reg1
;----------------------------------------------------------------------------------
        SBRK	#4		; ARP=AR2, AR0->FR1, AR2->pid_out_max
;----------------------------------------------------------------------------------
        SUB		*		; ACC = pid_out_reg1 - pid_out_max  (Q15)
        				; ARP=AR2, AR0->FR1, AR2->pid_out_max	
;----------------------------------------------------------------------------------
       	BCND	SAT_MAX,GT ; Branch if saturated at max	
       					; ARP=AR2, AR0->FR1, AR2->pid_out_max
;----------------------------------------------------------------------------------
		ADRK 	#4		; ARP=AR2, AR0->FR1, AR2->pid_out_reg1 
;----------------------------------------------------------------------------------
		LACC	*		; ACC = pid_out_reg1  (Q15)
						; ARP=AR2, AR0->FR1, AR2->pid_out_reg1
;----------------------------------------------------------------------------------
		SBRK	#3		; ARP=AR2, AR0->FR1, AR2->pid_out_min
;----------------------------------------------------------------------------------
		SUB		*		; ACC = pid_out_reg1 - pid_out_min  (Q15)
        				; ARP=AR2, AR0->FR1, AR2->pid_out_min	
;----------------------------------------------------------------------------------
       	BCND	SAT_MIN,LT ; Branch if saturated at min	
       					; ARP=AR2, AR0->FR1, AR2->pid_out_min
;----------------------------------------------------------------------------------
		ADRK	#3     	; ARP=AR2, AR0->FR1, AR2->pid_out_reg1 
;----------------------------------------------------------------------------------
      	B		REG1_END ; ARP=AR2, AR0->FR1, AR2->pid_out_reg1
;----------------------------------------------------------------------------------
SAT_MIN		
;----------------------------------------------------------------------------------
      	LACC	*		; ACC = pid_out_min (Q15)
      					; ARP=AR2, AR0->FR1, AR2->pid_out_min
;----------------------------------------------------------------------------------
        ADRK 	#3		; ARP=AR2, AR0->FR1, AR2->pid_out_reg1 
;----------------------------------------------------------------------------------
       	SACL	*		; pid_out_reg1 = pid_out_min (Q15)
                        ; ARP=AR2, AR0->FR1, AR2->pid_out_reg1
;----------------------------------------------------------------------------------
      	B		REG1_END ; ARP=AR2, AR0->FR1, AR2->pid_out_reg1 
;----------------------------------------------------------------------------------
SAT_MAX
;----------------------------------------------------------------------------------
      	LACC	*		; ACC = pid_out_max (Q15)
      					; ARP=AR2, AR0->FR1, AR2->pid_out_max
;----------------------------------------------------------------------------------
        ADRK 	#4		; ARP=AR2, AR0->FR1, AR2->pid_out_reg1 
;----------------------------------------------------------------------------------
       	SACL	*		; pid_out_reg1 = pid_out_max (Q15)
                        ; ARP=AR2, AR0->FR1, AR2->pid_out_reg1
;----------------------------------------------------------------------------------
REG1_END     	
;----------------------------------------------------------------------------------
      	SBRK	#2		; ARP=AR2, AR0->FR1, AR2->pid_e1_reg1  
;----------------------------------------------------------------------------------
		LACC	*+		; ACC = pid_e1_reg1  (Q15)
						; ARP=AR2, AR0->FR1, AR2->pid_e2_reg1  
;----------------------------------------------------------------------------------
       	SACL	*-,AR0	; pid_e2_reg1 = pid_e1_reg1 (Q15)
       					; ARP=AR2, AR0->FR1, AR2->pid_e1_reg1, ARP=AR0
;----------------------------------------------------------------------------------
		SBRK	#1		; ARP=AR0, AR0->FR0, AR2->pid_e1_reg1
;----------------------------------------------------------------------------------
       	LACC	*,AR2	; ACC = FR0 = pid_e0_reg1 (sign_reg1) (Q15)
       					; ARP=AR0, AR0->FR0, AR2->pid_e1_reg1, ARP=AR2
;----------------------------------------------------------------------------------
		SACL	*,AR1	; pid_e1_reg1 = pid_e0_reg1 (Q15)
						; ARP=AR2, AR0->FR0, AR2->pid_e1_reg1, ARP=AR1
;----------------------------------------------------------------------------------
_pid_reg1_calc_exit:
       	;MAR     *,AR1   ; can be removed if this condition is met on
       	;                ; every path to this code. (i.e., ARP=AR1 here)

        CLRC	OVM
        CLRC	SXM

    	SBRK 	#(__pid_reg1_calc_framesize+1)
        LAR  	AR0,*-
        PSHD	*
        
        RET