kesair_pid.asm

DSP关于F2407的实例程序

;====================================================================
; Filename	:	KesaiR_PID.asm
; Module names:	KesaiR_PID
; Initialization routines: KesaiR_PID_INIT
; Description:	KesaiR的PID调节器
;------------------------------------------------------------
; USM0 = USM1 + KesaiR_PID_K0 * KesaiRE0 + KesaiR_PID_K1 * KesaiRE1 + KesaiR_PID_K2 * KesaiRE2
;
;				|~~~~~~~~~~~|
;	KesaiR0,1    o------>	|	    |
;	KesaiR_ref    o------>	|  PID	    |	----->o  USM0
;	USM1 	     o------>	|	    |
;	KesaiRE0,1,2  o------>  |           |
; KesaiR_PID_K0,K1,K2 o------>	|___________|
; 	
; Modify: 2004.6.17
;=====================================================================
		.include "f2407.h"	
		.global	KesaiR_PID,KesaiR_PID_INIT		; function call
		
		.global	KesaiR0,KesaiR_ref			; Inputs
		.global	KesaiR_PID_K0,KesaiR_PID_K1,KesaiR_PID_K2;Inputs
		.global	KesaiRE0,KesaiRE1,KesaiRE2		; Inputs
		.global	USM1,USM_TEMP				; Input
		.global	USM0					; Outputs
		.global	USMmax,USMmin
;=====================================================================
KesaiR_PID_INIT:
;=====================================================================
		LDP	#DP_DATA		
		SPLK	#14000,USMmax		;设定最值输出
		SPLK	#-14000,USMmin		
		
		SPLK	#0,USM1			;给初值
		SPLK	#0,KesaiRE0
		SPLK	#0,KesaiRE1
		SPLK	#0,KesaiRE2
		
		SPLK	#13780,KesaiR_PID_K0
		;SPLK	#8780,KesaiR_PID_K0			;根据实际来设定，现在先胡乱设。
		SPLK	#-13700,KesaiR_PID_K1
		SPLK	#0,KesaiR_PID_K2

		RET
;=====================================================================
KesaiR_PID:
;=====================================================================
		SETC	SXM
		SPM	1			;符号数相乘
;------------------------------------------------------------								
		LDP	#DP_DATA
		LACC	USM0
		SACL	USM1			;USM0->USM1
		LACC	KesaiRE1
		SACL	KesaiRE2		;KesaiRE1->KesaiRE2
		LACC	KesaiRE0
		SACL	KesaiRE1		;KesaiRE0->KesaiRE1
;		SPLK	#2100,KesaiR0
; calculate USM0
; USM0 = USM1 + KesaiR_PID_K0 * KesaiRE0 + KesaiR_PID_K1 * KesaiRE1 + KesaiR_PID_K2 * KesaiRE2	
		LACC	KesaiR_ref
		SUB	KesaiR0
		;数据处理
		SACL	KesaiRE0		;新的KesaiRE0

		LACC	USM1,16
		ADD	USM_TEMP		;ACC为上一次的计算结果
		LT	KesaiRE0
		MPY	KesaiR_PID_K0
		LTA	KesaiRE1
		MPY	KesaiR_PID_K1
		LTA	KesaiRE2
		MPY	KesaiR_PID_K2
		APAC
		;数据处理
		SACH	USM0			;存PID计算结果
		SACL	USM_TEMP		;存PID计算结果的低16位。	
;------------------------------------------------------------
; 判断 USM0 是否饱和
		LACC	USM0
		SUB	USMmin
		BCND	KesaiR_PID_MIN,GEQ
		LACC	USMmin
		B	KesaiR_PID_RESULT
KesaiR_PID_MIN:		
		LACC	USM0
		SUB	USMmax
		BCND	KesaiR_PID_MAX,LEQ
		LACC	USMmax
		B	KesaiR_PID_RESULT
KesaiR_PID_MAX:
		LACC	USM0
		B	KesaiR_PID_RESULT
KesaiR_PID_RESULT:		
		SACL	USM0			; 最终结果
;------------------------------------------------------------
		SPM	0
		RET