;====================================================================
; Filename	:	IST_PID.asm
; Module names:	IST_PID
; Initialization routines: IST_PID_INIT
; Description:	轉矩電流調節器
;------------------------------------------------------------
; UST0 = UST1 + IST_PID_K0 * ISTE0 + IST_PID_K1 * ISTE1 + IST_PID_K2 * ISTE2
;
;				|~~~~~~~~~~~|
;	IST0,1    o------>	|	    |
;	IST_ref    o------>	|  PID	    |	----->o  UST0
;	UST1 	     o------>	|	    |
;	ISTE0,1,2  o------> 	|           |
; IST_PID_K0,K1,K2 o------>	|___________|
; 	
; Modify: 2004.6.17
;=====================================================================
		.include "f2407.h"	
		.global	IST_PID,IST_PID_INIT		; function call
		
		.global	IST0,IST_ref			; Inputs
		.global	IST_PID_K0,IST_PID_K1,IST_PID_K2;Inputs
		.global	ISTE0,ISTE1,ISTE2		; Inputs
		.global	UST1,UST_TEMP			; Input
		.global	UST0					; Outputs
		.global	USTmax,USTmin
;=====================================================================
IST_PID_INIT:
;=====================================================================
		LDP	#DP_DATA		
		SPLK	#28000,USTmax		;設最值輸出，注意不要超過調制度0.866,28000差不多就是最大了。
		SPLK	#-28000,USTmin		
		
		SPLK	#0,UST1			;給初值
		SPLK	#0,ISTE0
		SPLK	#0,ISTE1
		SPLK	#0,ISTE2
		
		SPLK	#18030,IST_PID_K0
		;SPLK	#7775,IST_PID_K0		;根據實際來設定，現在先胡亂設。
		SPLK	#-18000,IST_PID_K1		;比例積分調節。
		SPLK	#0,IST_PID_K2			;先設為0，微分增益。

		RET
;=====================================================================
IST_PID:
;=====================================================================
		SETC	SXM
		SPM	1			;符號數相乘
;------------------------------------------------------------								
		LDP	#DP_DATA
;		SPLK	#500,ISTE0
		LACC	UST0
		SACL	UST1			;UST0->UST1
		LACC	ISTE1
		SACL	ISTE2		;ISTE1->ISTE2
		LACC	ISTE0
		SACL	ISTE1		;ISTE0->ISTE1
; calculate UST0
; UST0 = UST1 + IST_PID_K0 * ISTE0 + IST_PID_K1 * ISTE1 + IST_PID_K2 * ISTE2	
		LACC	IST_ref
		SUB	IST0
		;數據處理
		SACL	ISTE0		;新的ISTE0

		LACC	UST1,16		
		ADD	UST_TEMP	;ACC為上一次的計算結果
		LT	ISTE0
		MPY	IST_PID_K0
		LTA	ISTE1
		MPY	IST_PID_K1
		LTA	ISTE2
		MPY	IST_PID_K2
		APAC
		;數據處理
		SACH	UST0			;存PID計算結果	
		SACL	UST_TEMP		;存PID計算結果的低16位。	
;------------------------------------------------------------
; 判斷 UST0 是否飽和
		LACC	UST0
		SUB	USTmin
		BCND	IST_PID_MIN,GEQ
		LACC	USTmin
		B	IST_PID_RESULT
IST_PID_MIN:		
		LACC	UST0
		SUB	USTmax
		BCND	IST_PID_MAX,LEQ
		LACC	USTmax
		B	IST_PID_RESULT
IST_PID_MAX:
		LACC	UST0
		B	IST_PID_RESULT
IST_PID_RESULT:		
		SACL	UST0			; 最終結果
;------------------------------------------------------------
		SPM	0
		RET