qep_drv.asm

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

;===========================================================================
; 文件名:	qep_drv.asm
; 模块名:	QEP_THETA_DRV
;
; 初始化程序名: QEP_THETA_DRV_INIT
;	
; 同步程序: QEP_INDEX_ISR_DRV
;
; 公司:	达盛科技
;
; 功能:	通过编码器脉冲信号测算转子位置和旋转向。 
;		QEP_THETA_DRV: 通过编码器脉冲信号计算转子位置
;		QEP_INDEX_ISR_DRV: 利用编码器零标记信号同步T2计数器
;
;				         |~~~~~~~~~~~~~~~~~~~|
; QEP A/B(EV H/W)o------>|			         |----->o  theta_elec
; polepairs      o------>|   QEP_THETA_DRV   |----->o  theta_mech
; cal_angle      o------>|  			     |----->o  dir_QEP
; mech_scale     o------>|  			     |
;			  	         |-------------------|
; QEP Index (EV H/W) o-->| QEP_INDEX_ISR_DRV |----->o  index_sync_flg
;				         |                   |----->o  QEP_cnt_idx
;				         |___________________|
;
; 目标cpu:	x2407
;=====================================================================================
; 修改纪录:
;-------------------------------------------------------------------------------------
; 最后更新时间：2005.08.23	版本号：Ver 1.0	
;===========================================================================

;---------------------------------------------------------------------------
; 全局变量声明
;---------------------------------------------------------------------------
;(要调用模块，就将下面声明语句复制到主程序代码中相应位置)
;		.ref	QEP_THETA_DRV,QEP_THETA_DRV_INIT; 子程序调用
;		.ref	polepairs,cal_angle,mech_scale	; 输入
;		.ref	theta_elec,theta_mech,dir_QEP	; 输出
;
;		.ref	QEP_INDEX_ISR_DRV				; 子程序调用
;		.ref	index_sync_flg,QEP_cnt_idx		; 输出
;---------------------------------------------------------------------------
; 标号定义
;---------------------------------------------------------------------------
		.def	QEP_THETA_DRV,QEP_THETA_DRV_INIT; 子程序调用
		.def	polepairs,cal_angle,mech_scale	; 输入
		.def	theta_elec,theta_mech,dir_QEP	; 输出

		.def	QEP_INDEX_ISR_DRV				; 子程序调用
		.def	index_sync_flg,QEP_cnt_idx		; 输出

;-------------------------------------------
; 相关外围变量定义
;-------------------------------------------
		.include	"x24x_app.h"

;---------------------------------------
; 变量定义
;---------------------------------------
theta_elec	.usect "qep_drv",1
theta_mech	.usect "qep_drv",1
dir_QEP		.usect "qep_drv",1
polepairs	.usect "qep_drv",1
QEP_cnt_idx	.usect "qep_drv",1
cal_angle	.usect "qep_drv",1
index_sync_flg	.usect "qep_drv",1
mech_scale	.usect "qep_drv",1
theta_raw	.usect "qep_drv",1

;---------------------------------------------------------------
; 参数设置
;---------------------------------------------------------------
POLEPAIRS_	.set	5		; 极对数
CAL_ANGLE_	.set	0		; 编码器偏角
MECH_SCALE_	.set	16776	; 转换系数 0.9999/Total_count (Q26) 
							;(total_count = 4*编码器线数 )
;----------------------------------------------------
; 初始化程序
;----------------------------------------------------
QEP_THETA_DRV_INIT:
		ldp	#polepairs
		SPLK	#POLEPAIRS_,polepairs
		splk	#CAL_ANGLE_,cal_angle
		SPLK	#MECH_SCALE_,mech_scale
		ldp	#OCRA>>7
		lacl	OCRA
		or	#00111000b
		sacl	OCRA
		ldp	#T2PER>>7
		SPLK	#0FFFFh,T2PER				; 兼容 '240/24x/240x'
		SPLK	#1001100001110000b,T2CON	; 定向增减计数,不分频, src=QEP
		SPLK	#1111000000000100b,CAPCON	; Tmr2, CAP3 pos edge, enbl
				;|||||||||||||||| 	
                ;5432109876543210
		RET

;---------------------------------------------------------
; 角位置计算程序
;---------------------------------------------------------
QEP_THETA_DRV
		ldp	#GPTCON>>7
		lacc	GPTCON,2
		and	#1,16			; 获取计数方向信号
		ldp	#dir_QEP		;
		sach	dir_QEP		; 保存方向信号
		ldp	#T2CNT>>7		; 读取脉冲计数值
		lacc	T2CNT		;
		sub  #4000			; 如果需要反相则做反相处理
		abs					; 反相处理
		ldp	#cal_angle		;
		add	cal_angle		; 加偏角
		sacl	theta_raw	;
		LT	theta_raw		;
		MPY	mech_scale		; Q0*Q26=Q26(32bit)
		PAC					;
		and	#7FFFh,11		; 模(360 in Q26)
		SACH	theta_mech,5; 保存为 Q(26-16+5)=Q15
		LT	theta_mech		;
		MPY	polepairs		; Q15*Q0=Q15(32bit)
		PAC
		and	#7FFFh			; 模(360 in Q15)
		SACL	theta_elec	; 保存为 Q15
		RET

;---------------------------------------------------------------------------
; 同步程序
;---------------------------------------------------------------------------
QEP_INDEX_ISR_DRV:
		ldp	#T2CNT>>7
		bldd	T2CNT,#QEP_cnt_idx	; 读取 QEP 计数值
		SPLK	#0,T2CNT			; 计数器清零
		ldp	#index_sync_flg
		SPLK	#0Fh,index_sync_flg	; 设置零位校正标志.
		RET