pid_i.asm

这是实现三相异步电动机FOC的DSP开发源代码

;============================================================
; 文件名:	pid_i.asm
;
; 模块名:	pid_reg_id, pid_reg_iq
;
; 初始化程序名: pid_reg_id_init, pid_reg_iq_init
;
; 公司:	达盛科技
;
; 功能描述:带积分饱和矫正的d轴 和 q 轴PI电流调节器
;		    
;		        |~~~~~~~~~~~~|
; i_fdb o------>|	         |
; i_ref o------>|  pid_reg   |----->o  u_out
; u_int o------>|	         |
; Kp            |            |
; Ki    o------>|____________|
; Kc
;
; 目标板cpu: 'c2xx 
;====================================================================
; 更改记录:
;--------------------------------------------------------------------
; 最后更新时间:2005.6.15	版本号:	Ver 1.0
;====================================================================

*************************************************************
* D轴 PI 电流调节器
*************************************************************
;------------------------------------------------------------
; 变量声明
;------------------------------------------------------------
;		.ref	pid_reg_id,pid_reg_id_init		; 函数调用
;		.ref	id_fdb,id_ref,Kp_d,Ki_d,Kc_d	; 输入参量
;		.ref	ud_int				; 输入
;		.ref	ud_out				; 输出
;------------------------------------------------------------
;变量声明
;------------------------------------------------------------
		.def	pid_reg_id,pid_reg_id_init		; 函数调用
		.def	id_fdb,id_ref,Kp_d,Ki_d,Kc_d	; 输入
		.def	ud_int				; 输入
		.def	ud_out				; 输出
		.def	id_error,uprsat_d
;------------------------------------------------------------
; 变量定义
;------------------------------------------------------------
id_fdb		.usect "pid",1		; 励磁电流反馈
id_ref		.usect "pid",1		; 励磁电流参考
ud_out		.usect "pid",1		; 控制电压输出

ud_int		.usect "pid",1		; 误差积分
uintlo_d	.usect "pid",1

Kp_d		.usect "pid",1		; 比利增益
Ki_d		.usect "pid",1		; 积分增益
Kc_d		.usect "pid",1		; 积分校正增益

id_error	.usect "pid",1		; 励磁电流误差
uprsat_d	.usect "pid",1		; 防积分饱和控制电压
saterr_d	.usect "pid",1		; 饱和误差
tmp_d		.usect "pid",1		; 临时变量

;------------------------------------------------------------
; 常量设置       .set【和.equ相似:把常量值赋予符号】
;------------------------------------------------------------
Kp_d_		.set	2400			; Q11, 比例增益
Ki_d_		.set	500;1271;2000;;	; Q25, 积分增益
Kc_d_		.set	32767			; Q14, 饱和校正增益

Umax_d_		.set	07000h		; 最大输出电压
Umin_d_		.set	08FFFh		; 最小输出电压

;------------------------------------------------------------
; 初始化
;------------------------------------------------------------
pid_reg_id_init
     ldp	#Kp_d				;
	 SPLK	#Kp_d_,Kp_d         ;
     SPLK   #Ki_d_,Ki_d        	;
     SPLK   #Kc_d_,Kc_d       	;
	 SPLK	#0,ud_int			; 积分时间初始化
     SPLK	#0,uintlo_d
	 RET

;------------------------------------------------------------
; 控制程序
;------------------------------------------------------------
pid_reg_id
	setc	SXM           	; 允许符号扩展
	setc	OVM				; 设置溢出标志
    ldp	#id_ref		
    LACC	id_ref,16		; 利用ACCH作为溢出保护寄存器
    SUB   id_fdb,16			;
    SACH  id_error			;  id_ref - id_fdb，Q15

	lacl	uintlo_d		;
	add		ud_int,16		; 32位，Q30
    spm		2				; 乘积除 4 累加
	LT		id_error		;
    mpy		Kp_d	        ; Q15*Q11 -> 32位，Q26
	apac					; 32位 Q30 uint + id_error*Kp_d 
    SACH	uprsat_d		; uprsat_d 存储为 Q14 
    sacl	tmp_d			;
    adds	tmp_d			;
	add	uprsat_d,16			; Q30 -> Q31 用于溢出保护
	sach	tmp_d			; 以 Q15 格式保存到 tmp_d 
	bit tmp_d,0
	bcnd U_gmind,ntc	
	lacc	tmp_d			;
	sub	#Umin_d_			;
    bcnd	U_lmaxd,GEQ		; 如果 tmp_d>=U_min 则继续
    lacc	#Umin_d_		; 否则认为饱和
    B	Nextd            
U_gmind
	lacc	tmp_d			;		
	sub	#Umax_d_	        ;
    BCND	U_lmaxd,LEQ     ; 如果 tmp_d<=U_max 则继续
    lacc	#Umax_d_		; 否则认为饱和
	b	Nextd

U_lmaxd
	lacc	tmp_d			;
Nextd                            	
	sacl	ud_out			;

Int_termd
	lacc	ud_out,15		; 利用 ACC 高位做溢出保护
	SUB	uprsat_d,16			;
	sach	saterr_d		; saterr_d = ud_out-uprsat_d 存储为 Q14 

	lt	id_error			;
	mpy	Ki_d				; Q15*Q25 -> Q40
	pac						; Q40 -> Q44
	sach	tmp_d			; 
	lacc	tmp_d			; Q44 -> Q28 (r/s 16 位)
		
	LT	saterr_d           	;
    MPY	Kc_d            	; saterr_d * Kc_d，Q14*Q14 -> Q28
	APAC                    ; Ki_d*id_error + Kc_d*saterr_d，Q28 

	norm	*				;
	norm	*				; Q28 -> Q30 (带溢出保护)

	ADDS	uintlo_d
	ADD	ud_int,16        	; uint + saterr_d*Kc_d + id_error*Ki_d
    SACH  ud_int
	SACL	uintlo_d        ; 保存为 32位，Q30
	RET

***D轴 PI 电流调节器结束

		
*************************************************************
* Q轴 PI 电流调节器
*************************************************************
;------------------------------------------------------------
; 变量声明
;------------------------------------------------------------
;	.ref	pid_reg_iq,pid_reg_iq_init	; 子程序调用
;	.ref	iq_fdb,iq_ref,Kp_q,Ki_q,Kc_q; 输入
;	.ref	uq_int						; 输入
;	.ref	uq_out						; 输出

;------------------------------------------------------------
; 变量声明
;------------------------------------------------------------
	.def	pid_reg_iq,pid_reg_iq_init	; 子程序调用
	.def	iq_fdb,iq_ref,Kp_q,Ki_q,Kc_q; 输入
	.def	uq_int						; 输入
	.def	uq_out						; 输出

;------------------------------------------------------------
; 变量定义
;------------------------------------------------------------
iq_fdb		.usect "pid",1		; 电流反馈
iq_ref		.usect "pid",1		; 电流参考
uq_out		.usect "pid",1		; 控制电压输出
                                 
uq_int		.usect "pid",1		; 积分误差
uintlo_q	.usect "pid",1

Kp_q		.usect "pid",1		; 比例增益
Ki_q		.usect "pid",1		; 积分增益
Kc_q		.usect "pid",1		; 积分饱和校正增益

iq_error	.usect "pid",1		; 电流误差
uprsat_q	.usect "pid",1		; 饱和前控制电压
saterr_q	.usect "pid",1		; 饱和误差
tmp_q		.usect "pid",1		; 临时变量

;------------------------------------------------------------
; 参数设置
;------------------------------------------------------------
Kp_q_		.set	300			; 比例增益,Q11
Ki_q_		.set	100			; 积分增益,Q25 
Kc_q_		.set	24576		; 饱和校正增益,Q14 

Umax_q_		.set	06000h		; 最大电压
Umin_q_		.set	09FFFh		; 最小电压

;------------------------------------------------------------
; 初始化
;------------------------------------------------------------
pid_reg_iq_init
	ldp	#Kp_q				;
	SPLK	#Kp_q_,Kp_q     ;
    SPLK  #Ki_q_,Ki_q       ;
    SPLK  #Kc_q_,Kc_q       ;
	SPLK	#0,uq_int		; 积分时间初始化
    SPLK	#0,uintlo_q
    SPLK	#3000H,Umax_q_
	RET

;------------------------------------------------------------
; 控制程序
;------------------------------------------------------------
pid_reg_iq
	setc	SXM           	; 允许符号扩展
	setc	OVM				; 设置溢出保护

    ldp	#iq_ref				;
    LACC	iq_ref,16		; 利用 ACC 高位做溢出保护
    SUB   iq_fdb,16			;
    SACH  iq_error			; iq_ref - iq_fdb，Q15 

	lacl	uintlo_q		;
	add	uq_int,16			; 32位，Q30
    spm	2					; 乘积除 4 后累加
	LT	iq_error			;
    mpy	Kp_q	        	; Q15*Q11 -> 32位 Q26
	apac					; 32位 Q30 uint + iq_error*Kp_q
    SACH	uprsat_q		; 存储为 Q14 uprsat_q
    sacl	tmp_q			;
    adds	tmp_q			;
	add	uprsat_q,16			; Q30 -> Q31 带溢出保护
	sach	tmp_q			; 以Q15格式保存到 tmp_q
	bit tmp_q,0
	bcnd U_gminq,ntc	
	lacc	tmp_q			;
	sub	#Umin_q_			;
    bcnd	U_lmaxq,GEQ		; 如果 tmp_q>=U_min则继续
    lacc	#Umin_q_		; 否则认为饱和
    B	Nextq            
U_gminq
	lacc	tmp_q			;		
	sub	#Umax_q_	        ;
    BCND	U_lmaxq,LEQ     ; 如果 tmp_q<=U_max则继续
    lacc	#Umax_q_		; 否则认为饱和
    b	Nextq

U_lmaxq
	lacc	tmp_q			;
Nextq                            	
	sacl	uq_out			;

Int_termq
	lacc	uq_out,15		; 利用 ACC 高位做溢出保护
	SUB	uprsat_q,16			;
    sach	saterr_q		; uq_out-uprsat_q = saterr_q 保存为 Q14 

	lt	iq_error			;
	mpy	Ki_q				; Q15*Q26 -> Q40
	pac						; Q40 -> Q44
	sach	tmp_q			;
	lacc	tmp_q			; Q44 -> Q28 (r/s 16 位)
		
	LT	saterr_q           	;
    MPY	Kc_q            	; saterr_q * Kc_q，Q14*Q14 -> Q28 
	APAC                    ; Ki_q*iq_error + Kc_q*saterr_q，Q28 

	norm	*				;
	norm	*				; Q28 -> Q30 (带溢出保护)

	ADDS	uintlo_q
	ADD	uq_int,16        	; uint + saterr_q*Kc_q + iq_error*Ki_q
    SACH 	uq_int
	SACL	uintlo_q        ; 保存为 32位 Q30

	RET

***Q轴 PI 电流调节器结束