pid.s

dsPIC30f3010的PID控制代码

;******************************************************************************************************
;  。 该模块采用汇编编写而成，目的是优化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