?? pid.asm
字號:
;=====================================================================================
; File name: PID.ASM
;
; Originator: Digital Control Systems Group
; Texas Instruments
;
; Description:
; Performs PID and feed forward control
;
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 9-15-2000 Release Rev 1.0
;-------------------------------------------------------------------------------------
; C prototype : int pid(struct pid_data pointer *);
;===========================================================================
;
; History:
; __________________________________________________________________________
; Date of Mod | DESCRIPTION
; ------------|-----------------------------------------------------------
; |
; |
; |
; |
;===========================================================================
; The PID and Feed-forward controller operates on a structure defined as
; follows:
; [un_H,un_L,en,en-1,en-2,K0,K1,K2,Kx,Kf,rn,sat_upper,sat_lower,output]
; This is fourteen elements in all.
; If any func *s are added they should be added in at the end, or else the pid_ff
; called with a * to the first data element.
;
.def _pid_ff
_pid_ff: POPD *+
SAR AR0,*+
SAR AR1,*
LARK AR0,1
LAR AR0,*0+,AR0
;===============================================================================
SETC OVM
SPM 1
clrc SXM ; ARP=AR0, AR0->FR0.
;-------------------------------------------------------------------------------
SBRK #3 ; Point to ARG1 (in this pointer to
; source struct.
; AR0=(FR0)-3
; ARP=AR0, AR0->FR0-3.
;-------------------------------------------------------------------------------
LAR AR3,*,AR3 ; point AR3 to struct.
; ARP=AR3, AR0->FR0-3, AR3->un_H.
;-------------------------------------------------------------------------------
ADRK #7 ; move AR3 to point to K2
; ARP=AR3, AR0->FR0-3, AR3->K2.
;-------------------------------------------------------------------------------
MAR *,AR0 ; ARP=AR0 to get the pointer again.
; ARP=AR0, AR0->FR0-3, AR3->K2.
;-------------------------------------------------------------------------------
LAR AR2,*,AR2 ; Get the ptr to struct pid in AR2.
; ARP=AR2, AR0->FR0-3, AR2->un_H,AR3->K2
;-------------------------------------------------------------------------------
LACC *+,16 ; get un_H in ACCH
; ARP=AR2, AR0->FR0-3, AR2->un_L,AR3->K2
;-------------------------------------------------------------------------------
ADD *+ ; get un_L in ACCL
; ARP=AR2, AR0->FR0-3, AR2->en,AR3->K2
;-------------------------------------------------------------------------------
ADRK #2 ; point AR2 to en-2.
; ARP=AR2, AR0->FR0-3, AR2->en-2,AR3->K2
;-------------------------------------------------------------------------------
LT *-,AR3 ; get en-2 in TREG
; ARP=AR3, AR2->en-1,AR3->K2, AR0->FR0-3
;-------------------------------------------------------------------------------
MPY *-,AR2 ; get product (en-2)*K2
; ARP=AR2, AR2->en-1,AR3->K1, AR0->FR0-3
;-------------------------------------------------------------------------------
LTD *-,AR3 ; get en-1 and point to en
; also en-1 is copied to en-2.
; **ALSO **
; add product to un_H:un_L.
; ARP=AR3, AR2->en ,AR3->K1, AR0->FR0-3
;-------------------------------------------------------------------------------
MPY *-,AR2 ; multiply by K1 and move AR3 to K0
; ARP=AR2, AR2->en,AR3->K0, AR0->FR0-3
;-------------------------------------------------------------------------------
LTD *-,AR3 ; load TREG with en, copy en to en-1
; ARP=AR3 pointing to K0
; AR2 now points to un_L
; **ALSO **
; add product to un_H:un_L.
; ARP=AR3, AR2->un_L,AR3->K0, AR0->FR0-3
;-------------------------------------------------------------------------------
MPY *,AR2 ; get product K0.en
; ARP=AR2, AR2->un_L,AR3->K0, AR0->FR0-3
APAC ; add to acc
;-------------------------------------------------------------------------------
; Acc now has un=un-1 + K0.en + K1 en-1 + K2 en-2.
; AR3 points to K0, AR2 points to un_L
; ARP=AR3.
;-------------------------------------------------------------------------------
SACL *- ; store lower un-sat'ed sum in un_L
; ARP=AR2, AR2->un_H,AR3->K0, AR0->FR0-3
;-------------------------------------------------------------------------------
SACH * ; store upper un-sat'ed sum in un_H
; ARP=AR2, AR2->un_H,AR3->K0, AR0->FR0-3
;-------------------------------------------------------------------------------
; AR2 points to un_H
; B _PID_DIAG_EXIT
_PID_SAT_CHK:
LACC *,16,AR3 ; get upper part in ACC_L
ADRK #6 ; now AR3 points to upper_limit.
SUB *,16
; do un - upper limit, this should NOT be ve.
; if so it would exceed limits and must be clamped.
; if less than 0 or equal to zero, branch to skip clamping.
BCND _PID_PLUS_OK,LEQ
_PID_PLUS_CLAMP:
LACL *+,AR2 ; get upper limit.
; inc AR3 to point to lower limit.
;
SACL *+ ; store in un_H
LACK #0 ;
SACL *- ; AR2 points once again to un_H
B _PID_CLAMPING_DONE
_PID_PLUS_OK:
_PID_NEG_CHK:
MAR *+,AR2 ; incoming ARP=AR3 pointing to upper_limit
; inc AR3 to point to lower limit.
; AR2 points to un_H
LACC *,16,AR3 ; get un_H
SUB *,16
; do un_h -lower limit, this should not be negative.
; so if it went lower than the limit then must be clamped to lower limit.
; if greater than or equal to zero skip clamp.
BCND _PID_NEG_OK,GEQ
_PID_NEG_CLAMP:
LACL *,AR2 ; get lower limit.
SACL *+ ; store lower limit to un_H
LACK #0 ;
SACL *-,AR3 ; store zero for un_L
; ARP=AR3
; AR2 still points to un_H
_PID_NEG_OK:
_PID_CLAMPING_DONE:
; At this point the state is :
; un_H:un_L clamped !!
; ARP=AR3
; AR2->un_H
; AR3->lower limit
SBRK #3 ; point AR3 to Kf.
MAR *,AR2 ; ARP=AR2->un_H
LACC *+,16 ; get un_H in ACCH
ADD * ; get un_L in ACCL.
; AR2 now points to un_L
ADRK #9 ; AR2 now points to rn
LT *,AR3 ; get rn in TREG.
MPY *-,AR2 ; multiply by Kf.
; dec AR3 to point to Kx.
APAC ; get PID+FF.
ADRK #3 ; point AR2 to output.
SACH *,AR3 ; store u_H in output.
LAR AR3,*,AR2 ; get Kx in AR3
; ARP=AR2.
LACK #0 ; get zero in acc.
_PID_MULT_LOOP:
ADD *,16,AR3 ; add the output to the acc
; make current AR=AR3 for BANZ loop.
BANZ _PID_MULT_LOOP,*-,AR2
; here ARP=AR2
SACH * ; store multiplied output in 'output'
_PID_DIAG_EXIT:
LACL *,AR1 ; Return 'output' in accumulator.
; ARP=AR1 in prep for return
CLRC OVM
SPM 0
;===========================================================================
SBRK 2
LAR AR0,*-
PSHD *
RET
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -