ex7p6pid.asm

用于DSK5416的程序

;___________________________________________________________________________
;
; Program Name: ex7p6PID.asm
;
; Description:  This is an example to show how to implement a PID controller.
;               It implements the following equation 
;
;               u(n) = u(n-1) + K0.e(n) + K1.e(n-1) + K2.e(n-2)
;
;               where 
;               K0, K1, and K2 are controller coefficients (q15 numbers).
;               e(n), e(n-1), and e(n-2) are error signal samples (integers).
;
;               The error samples are the stored values and the computed
;               control values are also stored in a buffer.
;
;               The program can be modified for a realtime control system
;               using an interrupt invoked at the sampling interval, reading
;               the next incoming error sample from an input port, and
;               applying the computed control through an output port.
;               
;
; Author:       Avtar Singh, SJSU
;
;___________________________________________________________________________



                .mmregs                 ; memory-mapped registers
                .def _c_int00

ErrSamples      .bss e, 200, 1          ; Allocate space for e(n)s
ContSamples     .bss u, 200, 1          ; Allocate space for u(n)s
SampleCnt       .set 200                ; Sample count

                .data
; Control and error signals (sequential locations)
un:             .word 0                 ; computed control u(n) as integer
en:             .word 1                 ; error samples e(n) as integer
enm1:           .word 2                 ; error samples e(n-1) as integer
enm2:           .word 1                 ; error samples e(n-2) as integer

                .sect "coeff"
; PID Controller coefficients (sequential locations)
K0:             .word 2000h             ; 1/4 in q15
K1:             .word 0400h             ; 1/32 in q15
K2:             .word 0040h             ; 1/512 in q15


                .text
_c_int00:
                STM #ContSamples,AR6    ; Clear control sample buffer
                RPT #SampleCnt				
                ST #0, *AR6+					

                STM #ErrSamples, AR5    ; AR5 points to InSamples buffer start
                STM #ContSamples,AR6    ; AR6 points to OutSample buffer start
                STM #SampleCnt, AR4     ; AR4 = Number of samples to filter
loop:
                LD *AR5+,B              ; B = next error sample
                CALL PID                ; Call PID Control Routine
                STH B,*AR6+             ; Store computed control
                BANZ loop,*AR4-         ; Repeat till all samples done
                nop
                nop
                nop
	
	
;-------------------------------------------------------------------------------
; PID Controller Subroutine
; Enter with B = e(n) as integer
; Exit with B = u(n) as integer
; Uses A, AR2 and AR3
;-------------------------------------------------------------------------------
	
PID:             
                SSBX SXM                ; Select sign extension mode   
                STM #enm2, AR2          ; AR2 points to current e(n-2)
                STM #K2, AR3            ; AR3 points to current K2
                LD #0, A                ; A = 0
                MAC *AR2-, *AR3-, A     ; A = K2.e(n-2)
                DELAY *AR2              ; e(n-1) -> e(n-2)
                MAC *AR2-, *AR3-, A     ; A = K1.e(n-1) + K2.e(n-2)
                DELAY *AR2              ; e(n) -> e(n-1)
                STL B, *AR2             ; new e(n)
                MAC *AR2-, *AR3, A      ; A = K0.e(n) + K1.e(n-1) + K2.e(n-2)
                ADD *AR2, 15, A         ; A = u(n-1)+K0.e(n)+K1.e(n-1)+K2.e(n-2)
                ADD #1, 14, A           ; Round the result                
                STH A, 1, *AR2          ; new u(n)
                LD *AR2, B              ; B = new control
                RET                     ; Return
                nop
                nop
                nop
 
                .end