omiga_pid.asm

DSP关于F2407的实例程序

;====================================================================
; Filename	:	Omiga_PID.asm
; Module names:	Omiga_PID
; Initialization routines: Omiga_PID_INIT
; Description:	Omiga的PID调节器
;------------------------------------------------------------
; IST_ref = IST_ref1 + Omiga_PID_K0 * OmigaE0 + Omiga_PID_K1 * OmigaE1 + Omiga_PID_K2 * OmigaE2
;
;				|~~~~~~~~~~~|
;	OmigaR0,1    o------>	|	    |
;	Omiga_ref    o------>	|  PID	    |	----->o  IST_ref
;	IST_ref1     o------>	|	    |
;	OmigaE0,1,2  o------>  |           |
; Omiga_PID_K0,K1,K2 o------>	|___________|
; 	
; Modify: 2004.8.30
;=====================================================================
		.include "f2407.h"	
		.global	Omiga_PID,Omiga_PID_INIT		; function call
		.global	OMIGAE0_FILTER
		
		.global	OmigaR0,Omiga_ref			; Inputs
		.global	Omiga_PID_K0,Omiga_PID_K1,Omiga_PID_K2;Inputs
		.global	OmigaE0,OmigaE1,OmigaE2		; Inputs
		.global	IST_ref1					; Input
		.global	IST_ref,IST_ref_TEMP				; Outputs
		.global	IST_refmax,IST_refmin
		.global	OMIGAE0_FILTER_RESULT
		.global	TEMP	
;=====================================================================
Omiga_PID_INIT:
;=====================================================================
		LDP	#DP_DATA		
		SPLK	#14000,IST_refmax		; 设定最值输出，7.5A
		SPLK	#-14000,IST_refmin		; -7.5A
		
		SPLK	#0,IST_ref1			;给初值
		SPLK	#0,OmigaE0
		SPLK	#0,OmigaE1
		SPLK	#0,OmigaE2
		
		SPLK	#31200,Omiga_PID_K0
		;SPLK	#4065,Omiga_PID_K0			;根据实际来设定，现在先胡乱设。
		SPLK	#-31000,Omiga_PID_K1
		SPLK	#0,Omiga_PID_K2			;Kp=10000,Ki=25,Kd=0

		RET
;=====================================================================
Omiga_PID:
;=====================================================================
		SETC	SXM
		SPM	1			;符号数相乘
;------------------------------------------------------------								
		LDP	#DP_DATA
;		SPLK	#330,Omiga_ref
;		SPLK	#270,OmigaR0
		
		LACC	IST_ref
		SACL	IST_ref1			;IST_ref->IST_ref1
		LACC	OmigaE1
		SACL	OmigaE2		;OmigaE1->OmigaE2
		LACC	OmigaE0
		SACL	OmigaE1		;OmigaE0->OmigaE1
; calculate IST_ref
; IST_ref = IST_ref1 + Omiga_PID_K0 * OmigaE0 + Omiga_PID_K1 * OmigaE1 + Omiga_PID_K2 * OmigaE2	
		LACC	Omiga_ref
		SUB	OmigaR0
		;Omiga_PID_K0,1,2系数用16位表示不够，要增加位数。相当于OmigaE0,1,2放大。
		SACL	OmigaE0,4	;新的OmigaE0,按最大OmigaE0为1200计算（75Hz），左移4位不会溢出。
		
	;	SPLK	#600,OmigaE0
		
		CALL	OMIGAE0_FILTER
		LACC	OMIGAE0_FILTER_RESULT
		SACL	OmigaE0
		
		LACC	IST_ref,16
		ADD	IST_ref_TEMP	;ACC为上一次PID运算结果。
		LT	OmigaE0
		MPY	Omiga_PID_K0
		LTA	OmigaE1
		MPY	Omiga_PID_K1
		LTA	OmigaE2
		MPY	Omiga_PID_K2
		APAC
		;数据处理
		SACH	IST_ref			;存PID计算结果	
		SACL	IST_ref_TEMP		;存PID计算结果低16位。
;------------------------------------------------------------
; 判断 IST_ref 是否饱和
		LACC	IST_ref
		SUB	IST_refmin
		BCND	Omiga_PID_MIN,GEQ
		LACC	IST_refmin
		B	Omiga_PID_RESULT
Omiga_PID_MIN:		
		LACC	IST_ref
		SUB	IST_refmax
		BCND	Omiga_PID_MAX,LEQ
		LACC	IST_refmax
		B	Omiga_PID_RESULT
Omiga_PID_MAX:
		LACC	IST_ref
		B	Omiga_PID_RESULT
Omiga_PID_RESULT:		
		SACL	IST_ref			; 最终结果
;		SPLK	#4096,IST_ref
;------------------------------------------------------------
		SPM	0
		RET