pid.asm

编写基于DSP的PID汇编子程序

;临时数据寄存器
	.bss VE0 ,1        	;e(k)
	.bss VE1 ,1       	;e(k-1)
	.bss VE2 ,1       	;e(k-2)
	.bss PIDTEMP1 ,1    ;
	.bss PID1 ,1  
	.bss PIDTEMP2 ,1	;
	.bss PID2 ,1
	.bss PIDTEMP3 ,1	;
	.bss PID3 ,1
	.bss UREF ,1		;Uref=1D Q14
	.bss U ,1			;Uout from piddata.dat Q14 ==>0x8000@data length=0x03e9
	.bss UCTE ,1
	.bss UCT0 ,1               
	.bss UCT1 ,1         
	.bss KP ,1
	.bss KI ,1
	.bss KD ,1
     
DP_USER  .set 100h
                   ;用户寄存器存放页指针	
			.include  "F2407REGS.H" 
			.def  _c_int0
            .text
_c_int0   
SYSINIT:                            ; 系统初始化程序
		SETC   INTM
        setc   SXM
       	CLRC   OVM
		CLRC   CNF	
        LDP    #0E0H
	    SPLK   #0220H,SCSR1			;4*CLKIN=6M,CLKOUT=24M
        SPLK   #0E8h,WDCR 			;DISABLE WD
	    LDP	   #0
		SPLK   #0000h,IMR  			;DSIABLE INT
		SPLK   #0FFFFh,IFR 			;CLEAR INT FLAG 
		
		LDP #100h
		SPLK #0400H,UREF            ;UREF=1D   Q14
		SPLK #1H,KP              ;KP=1D     Q14
		SPLK #1H,KI              ;KI=1D     Q14
		SPLK #1H,KD              ;KD=1D     Q14
		SPLK #0000h,UCT0				;UCT0=0D   Q12
		SPLK #0000h,VE0					;VE1=0
		SPLK #0000h,VE1					;VE2=0
		SPLK #0000h,VE2
		MAR *,AR0
		LAR AR0,#9000H				;THE START ADD OF OUTPUT DATA OF THE SYSTEM

		
PID:     
		LACL UREF
		SUB  *+                     ;UREF-U,Q14
		;
		SACL VE0						;VE=e(k)=UREF-U,Q14
		SUB  VE1					;VE1=e(k-1) 
		SACL PIDTEMP1				;PIDTEMP1=de(K)=e(k)-e(k-1),Q14
		SUB VE1
		ADD VE2						;VE2=e(k-2),Q14
		SACL PIDTEMP3				;PIDTEMP3=de(k)-de(k-1)=e(k)-2e(k-1)+e(k-2),Q14
		LT KP
		MPY PIDTEMP1
		PAC                         ;KP*de(k),Q28
		SACl PID1					;Q12
		LT KI                       ;Q14
		MPY VE0
		PAC                         ;KI*e(k),Q28
		SACl PID2					;Q12
		LT KD                       ;Q14
		MPY PIDTEMP3
		PAC							;KD*[de(k)-de(k-1)],Q28
		SACl PID3                   ;Q12
		
		LACL UCT0					;u(k-1)
		ADD PID1
		ADD PID2
		ADD PID3
		SACL UCT0
		
		LACL VE1
		SACL VE2
		LACL VE0 
		SACL VE1
		
		nop
		B PID
		
        .end