speed_fr.asm

这个是关于实现同步电机FOC的控制程序源代码

;=======================================================================
; 文件名:	speed_fr.asm			      
;
; 模块名: 	SPEED_FRQ	
;
; 初始化程序名: SPEED_FRQ_INIT
;
; 公司:	达盛科技
;
; 功能:  通过电角度计算转速 	    	
;
;			          |~~~~~~~~~~~~~~~~~~~~|
;	                  |                 Q15|----->o speed_frq 
;theta_elec_fr o----->|Q15    SPEED_FRQ	 Q0|----->o speed_frq_rpm
;	         	      |____________________|
;
; 目标cpu:	x2407
;===========================================================================
; 更改纪录:
;---------------------------------------------------------------------------
; 最后修改时间：2005.08.20	版本号：Ver 1.0	
;===========================================================================
;(使用子程序就把下面的变量声明文件复制到主程序中)
;		.ref	SPEED_FRQ, SPEED_FRQ_INIT		;子程序调用
;		.ref	theta_elec_fr          			;输入
;		.ref	speed_frq, speed_frq_rpm	    ;输出
;===========================================================================
;全局变量定义.
		.def	SPEED_FRQ, SPEED_FRQ_INIT		;子程序调用
		.def	theta_elec_fr          			;输入
		.def	speed_frq, speed_frq_rpm	    ;输出
;===========================================================================
K1_fr_		.set	2560;6400	; 微分常数 K1_fr 设定(Q6) 
K2_fr_		.set	32162		; 滤波常数 K2_fr 设定(Q15)	
K3_fr_		.set	606 	    ; 滤波常数 K3_fr 设定(Q15)

BASE_RPM_FR_ 	 .set 	3000	; 基本 rpm 转速设定 (Q0)	
DIFF_MAX_LIMIT   .set   7333h   ; 转子磁链角度微分上限设定 (Q15)
DIFF_MIN_LIMIT   .set   0CCDh   ; 转子磁链角度微分下限设定 (Q15)
         
		.include	x24x_app.h
            
theta_elec_fr   .usect	"speed_fr",1 	; 转子电角度     (Q15)
speed_frq       .usect	"speed_fr",1 	; 转子电角速度   (Q15)
speed_frq_rpm   .usect	"speed_fr",1    ; 转子 rpm转速   (Q0)
K1_fr			.usect	"speed_fr",1	; 微分常数       (Q6)
K2_fr			.usect	"speed_fr",1	; 低通滤波常数   (Q15)
K3_fr			.usect	"speed_fr",1	; 低通滤波常数   (Q15)
rpm_max_fr      .usect	"speed_fr",1    ; 基本 rpm 转速  (Q0)
theta_old       .usect	"speed_fr",1	; 前一时刻转子磁链角度 (Q15)
min_theta_fr    .usect	"speed_fr",1    ; 转子磁链角度微分下限 (Q15)
max_theta_fr    .usect	"speed_fr",1    ; 转子磁链角度微分上限 (Q15)
tmp1_fr			.usect	"speed_fr",1	; 临时变量

;=========================
SPEED_FRQ_INIT:		;初始化程序
;=========================    
	LDP		#K1_fr    
	SPLK	#K1_fr_,K1_fr	     		; K1 = 1/(fb*T)     (Q6)
  	SPLK	#K2_fr_,K2_fr       		; K2 = Tau/(Tau+T)  (Q15)
  	SPLK	#K3_fr_,K3_fr       		; K3 = T/(Tau+T)    (Q15)
	SPLK	#BASE_RPM_FR_,rpm_max_fr  	; 基本rpm转速初始化 (Q0)	
    SPLK	#DIFF_MAX_LIMIT,max_theta_fr; 转子磁链角度微分上限初始化 (Q15)	
    SPLK	#DIFF_MIN_LIMIT,min_theta_fr; 转子磁链角度微分下限初始化 (Q15)    
    SPLK	#0h,theta_old           	; 转子磁连角度初始化(Q15)
 
    RET
                   
;=========================
SPEED_FRQ:			;控制程序
;=========================    
   		SPM		0 				; 设置乘积移位模式 
		SETC	SXM				; 允许符号扩展
		SETC	OVM				; 设置溢出保护
		LDP		#max_theta_fr
; 微分计算
; 检查角度是否属于有效微分范围 (介于min_theta,max_theta之间)
		LACC	max_theta_fr
		SUB		theta_elec_fr
		BCND	LESS_MAX_FR,GT	; 转到 LESS_MAX ，当 max_theta > theta_elec 
        B		SKIP_DIFF_FR
LESS_MAX_FR
    	LACC	min_theta_fr
    	SUB		theta_elec_fr
    	BCND	MORE_MIN_FR,LT  ; 转到 MORE_MIN ，当 min_theta < theta_elec 
        B		SKIP_DIFF_FR
MORE_MIN_FR	
		; 计算微分值，当 min_theta < theta_elec < max_theta
		LT		K1_fr			; TREG = K1              (Q6)
		MPY		theta_elec_fr   ; PREG = K1*theta(k)     (Q21)
		PAC						; ACC = K1*theta(k)      (Q21)
		MPY		theta_old       ; PREG = K1*theta(k-1)   (Q21)
		SPAC					; ACC = K1*theta(k)-K1*theta(k-1)   (Q21)
		SFL               		; ACC = K1*theta(k)-K1*theta(k-1)   (Q22)
		SFL               		; ACC = K1*theta(k)-K1*theta(k-1)   (Q23)
		SFL               		; ACC = K1*theta(k)-K1*theta(k-1)   (Q24)
		SACH	tmp1_fr,7       ; tmp1_fr = K1*theta(k)-K1*theta(k-1)   (Q15)
		; 低通滤波
		LT		K2_fr			; TREG = K2                	 (Q15)
		MPY		speed_frq       ; PREG = K2*speed_frq(k-1)   (Q30)
        PAC						; ACC =  K2*w_psi_r(k-1)     (Q30)
        LT		K3_fr			; TREG = K3                  (Q15)
        MPY		tmp1_fr         ; PREG = K3*tmp1_fr          (Q30)
        APAC					; ACC = K2*speed_frq(k-1)+K3*tmp1_fr           (Q30)
        SACH	speed_frq,1     ; speed_frq(k) = K2*speed_frq(k-1)+K3*tmp1_fr  (Q15)
SKIP_DIFF_FR

; 更新转子磁链角度
		LACC	theta_elec_fr	; ACC = theta_elec            (Q15)
		SACL	theta_old       ; theta_old = theta_elec      (Q15)
; 将转速值转换成相应的 rpm 值 (Q15 -> Q0)
		LT		rpm_max_fr   	; TREG = rpm_max_fr           (Q0)
		MPY	    speed_frq    	; PREG = rpm_max_fr*speed_frq (Q15)
		PAC						; ACC = rpm_max_fr*speed_frq  (Q15)	
		SACH	speed_frq_rpm,1	; speed_frq_rpm = rpm_max_fr*speed_frq  (Q0)
       
	    RET