;******************************************************************************************************
;  。 該模塊采用匯編編寫而成，目的是優化PID的速度，利用DSP核的長累加器和單指令周期MAC指令進行PID參數運算
;  。 該節點可以加入工程，供主程序調用。由于全部采用匯編，使得該節點可以用于任何其他經典PID運算的場合。
;  。 本節點使用匯編程序（有符號小數），允許累加器飽和操作，以獲得更大累加容量
;  。 主要使用了硬件乘加指令（MAC）進行積分操作，在累加器內部進行操作，減少截斷誤差
;*****************************************************************************************************

.include "p30f3010.inc"
	.global _SpeedCalculation
	.global _SpeedControl

;********************************************************************
; 本函數計算BLDC電機的速度（小數格式）。小數除法格式為：
;
;                 最大周期
;   實際速度 =  ------------
;                 實際周期
;
;********************************************************************

_SpeedCalculation:
	MOV __MINPERIOD, W8
	MOV _Period, W9
	REPEAT #17
	DIVF W8, W9
	MOV W0, _Speed
	RETURN

;********************************************************************
;
;                                             ----         比例
;                                            |    |        輸出
;                             ---------------| Kp |-----------------
;                            |               |    |                 |
;                            |                ----                  |
;                            |                                     --- 
;給定          ---           |           --------------    積分   | + | 總輸出    -------
;     --------| + |  誤差    |          |      Ki      |   輸出   |   |          |       |
;             |   |----------|----------| ------------ |----------|+  |----------| 電機  |--
;        -----| - |          |          |  1 - Z^(-1)  |          |   |          |       |  |
;       |      ---           |           --------------           | + |           -------   |
;       |                    |                                     ---                      |
;       | 速度反饋           |         -------------------  微分    |                       |
;       |                    |        |                   | 輸出    |                       |
;       |                     --------| Kd * (1 - Z^(-1)) |---------                        |
;       |                             |                   |                                 |
;       |                              -------------------                                  |
;       |                                                                                   |
;       |                                                                                   |
;        -----------------------------------------------------------------------------------
;
;   ControlOutput(K) = ControlOutput(K-1) 
;                    + ControlDifference(K) * (Kp + Ki + Kd)
;                    + ControlDifference(K-1) * (-Ki - 2*Kd)
;                    + ControlDifference(K-2) * Kd
;
;   使用 PIDCoefficients:
;        			   PIDCoefficients[0] = Kp + Ki + Kd
;   	 			   PIDCoefficients[1] = -(Kp + 2*Kd)
;  	     			   PIDCoefficients[2] = Kd
;   令:
;   ControlOutput -> ControlOutput(K) 和 ControlOutput(K-1)
;   ControlDifference[0] -> ControlDifference(K)
;   ControlDifference[1] -> ControlDifference(K-1)
;   ControlDifference[2] -> ControlDifference(K-2)
;
;   ControlOutput = ControlOutput
;                 + ControlDifference[0] * PIDCoefficients[0]
;                 + ControlDifference[1] * PIDCoefficients[1]
;                 + ControlDifference[2] * PIDCoefficients[2]
;
;********************************************************************

_SpeedControl:
	BSET CORCON, #SATA 			               ; 允許累加器 Acc A 可以飽和控制
	                                           ; 初始化指針
	MOV #_ControlDifference, W8
	MOV #_PIDCoefficients, W10
	MOV #_RefSpeed, W1
	MOV #_Speed, W2
	MOV #_ControlOutput, W0         	
                                               ; 允許飽和的情況下計算最近一次誤差, 不檢查運算上限
	LAC [W1], A
	LAC [W2], B
	SUB A
	SAC A, [W8]
	                               		       ; 準備 MAC 操作
	MOVSAC A, [W8]+=2, W4, [W10]+=2, W5
	LAC [W0], A                                ; 將上一次輸出裝載到 Acc  
	                                           ; 執行 MAC 操作
	REPEAT #2	                               ; 使用硬件Repeat指令重復 3 次
	MAC W4*W5, A, [W8]+=2, W4, [W10]+=2, W5
	                                           ; 結果放置于 ControlOutput (帶飽和) 
	SAC A, [W0]
	BCLR CORCON, #SATA                         ; 禁止 Acc A 飽和
	MOV #_ControlDifference, W8
	MOV [W8+2], W2 	                           ; 令 ControlDifference[2] = ControlDifference[1]
	MOV W2, [W8+4]
	MOV [W8], W2	                           ; 令 ControlDifference[1] = ControlDifference[0]
	MOV W2, [W8+2]
	RETURN;


 .end