smopos.asm

无刷直流电机的无传感器控制TI程序

************************************************************************
* File name:	smopos.asm
* Module Name:	Smopos
*
* Initialization Routine: Smopos_init
*
* Originator:	Digital Control Systems Group
*		Texas Instruments
*
* Description:	This module implements a sliding mode current observer
*		and estimates the back EMF and rotor position for
*		sensorless permanent-magnet synchronous motor control
*
*
*                         ------------------
*      speedref--------->|  Sliding Mode    |-------->thetae
*     vsalfa, vsbeta---->| Rotor Position   |-------->isalfae*, isbetae*,...
*     isalfa, isbeta---->|    Observer      |-------->zalfa, zbeta
*                         ------------------
*                                  |
*                    fsmopos, gsmopos, kslide, cmptable.tab
*
* Note:		Variables are all Q15 unless otherwise noted
* *:			Deubg only outputs
************************************************************************
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 9-15-2000	Release	Rev 1.0	

**************************************************************
* Select Debug Mode
**************************************************************
debug_mode	.set 1			; set to 1 for debug mode

**************************************************************
* Global Function and Variable References and Definitions
**************************************************************
		.ref	_atan_div	; atan(sin_comp, cos_comp) function call

		.def	Smopos_init	; initialization routine
		.def	Smopos	; routine name
		.def	speedref	; input, reference speed
		.def	vsalfa	; input, alfa-axis voltage
		.def	vsbeta	; input, beta-axis voltage
		.def	isalfa	; input, alfa-axis current
		.def	isbeta	; input, beta-axis current
		.def	zalfa		; output, alfa-axis sliding control
		.def	zbeta		; output, beta-axis sliding control
		.def	thetae	; output, estimated rotor angle

		.if	debug_mode  
		.def	isalfae	; debug output, estimated alfa-axis current
		.def	isbetae	; debug output, estimated beta-axis current
		.def	esalfa	; debug output, estimated alfa-axis back-emf
		.def	esbeta	; debug output, estimated beta-axis back-emf
		.def	isalfaerr	; debug output, current estimation error
		.def	isbetaerr	; debug output, current estimation error
		.endif

**************************************************************
* Variables
**************************************************************
speedref	.usect "smopos",1	; speed reference
vsalfa	.usect "smopos",1	; alfa-axis voltage
vsbeta	.usect "smopos",1	; beta-axis voltage
isalfa	.usect "smopos",1	; alfa-axis current
isbeta	.usect "smopos",1	; beta-axis current
isalfae	.usect "smopos",1	; estimated alfa-axis current
isbetae	.usect "smopos",1	; estimated beta-axis current

		.if	debug_mode
isalfaerr	.usect "smopos",1	; alfa-axis current estimate error
isbetaerr	.usect "smopos",1	; beta-axis current estimate error
		.endif

zalfa		.usect "smopos",1	; alfa-axis sliding control
zbeta		.usect "smopos",1	; beta-axis sliding control
esalfalo	.usect "smopos",1	; alfa-axis back EMF low word
esalfa	.usect "smopos",1	; alfa-axis back EMF
esbetalo	.usect "smopos",1	; beta-axis back EMF low word
esbeta	.usect "smopos",1	; beta-axis back EMF
eserrlo	.usect "smopos",1	; sliding control filter error low word
eserrhi	.usect "smopos",1	; sliding control filter error high
thetau	.usect "smopos",1	; estimated rotor angle
thetae	.usect "smopos",1	; compensated rotor angle
delta		.usect "smopos",1	; rotor angle compensation
fsmopos	.usect "smopos",1	; motor dependent plant matrix
gsmopos	.usect "smopos",1	; motor dependent control gain
kslide	.usect "smopos",1	; sliding control gain
kslf		.usect "smopos",1	; sliding control filter gain
smoptemp	.usect "smopos",1	; scratch pad

**************************************************************
* Default Program Patameters (File: smopos.xls)
**************************************************************
fsmopos_		.set	32097		; Q15, Current Observer f
gsmopos_		.set	19677		; Q15, Current Observer gain
kslide_		.set	1419		; Q15, sliding control gain
kslf_			.set	5147		; Q15, sliding control filter g, f0 = 250Hz

		.text
**************************************************************
* Initialization routine
**************************************************************
Smopos_init
		ldp	#fsmopos		;
		splk	#fsmopos_,fsmopos	;
		splk	#gsmopos_,gsmopos	;
		splk	#kslide_,kslide	;
		splk	#kslf_,kslf		;
		splk	#0,isalfae		;
		splk	#0,isbetae		;
		splk	#0,esalfa		;
		splk	#0,esalfalo		;
		splk	#0,esbeta		;
		splk	#0,esbetalo		;
		splk	#0,zalfa		;
		splk	#0,zbeta		;
		ret				;

**************************************************************
* Main program
**************************************************************
Smopos
		setc	SXM		; sign extension on
		clrc	OVM		; overflow mode off
		spm	#1		; product leftshift by 1 bit for Q15 math

**************************************************************
* Sliding Mode Current Observer
* isalfae=fsmopos * isalfae + gsmopos * (vsalfa - esalfa - zalfa)
* isbetae=fsmopos * isbetae + gsmopos * (vsbeta - esbeta - zbeta)
**************************************************************
Smoi
		ldp	#vsalfa		;
		lacc	vsalfa		;
		sub	esalfa		;
		sub	zalfa			;
		sacl	smoptemp		;
		lt	smoptemp		;
		mpy	gsmopos		;
		pac				;
		lt	isalfae		;
		mpy	fsmopos		;
		apac				;
		sach	isalfae		;

		lacc	vsbeta		;
		sub	esbeta		;
		sub	zbeta			;
		sacl	smoptemp		;
		lt	smoptemp		;
		mpy	gsmopos		;
		pac				;
		lt	isbetae		;
		mpy	fsmopos		;
		apac				;
		sach	isbetae		;
* END Sliding Mode Current Observer


***********************************************
* Sliding Control Calculator
* zalfa = kslide * SIGN ( isalfae - isalfa )
* zbeta = kslide * SIGN ( isbetae - isbeta )
***********************************************
Slcntrl
		lacc	isalfae		;
		sub	isalfa		;
		.if	debug_mode		;
		sacl	isalfaerr		;
		.endif
		bcnd	Ealfaz,EQ		;
		bcnd	Ealfagt,GT		;
		lacc	kslide		;
		neg				;
		b	Stzalfa		;
Ealfaz	lacl	#0			;
		b	Stzalfa		;
Ealfagt	lacc	kslide		;
Stzalfa	sacl	zalfa			;

		lacc	isbetae		;
		sub	isbeta		;
		.if	debug_mode		;
		sacl	isbetaerr		;
		.endif
		bcnd	Ebetaz,EQ		;
		bcnd	Ebetagt,GT		;
		lacc	kslide		;
		neg				;
		b	Stzbeta		;
Ebetaz	lacl	#0			;
		b	Stzbeta		;
Ebetagt	lacc	kslide		;
Stzbeta	sacl	zbeta			;
* END Sliding Control Calculator


******************************************************
* Sliding Control Filter - back EMF Calculator
* esalfa = esalfa + kslf * ( zalfa - esalfa )
* esbeta = esbeta + kslf * ( zbeta - esbeta )
******************************************************
Slcntrlf2
		lacc	zalfa,16	;
		subs	esalfalo	;
		sub	esalfa,16	;
		sacl	eserrlo	;
		sach	eserrhi	;
		lt	eserrlo	; Note: kslf>0
		mpyu	kslf		;
		pac			;
		sach	smoptemp	;
		lt	eserrhi	;
		mpy	kslf		;
		pac			;
		add	smoptemp	;
		adds	esalfalo	;
		add	esalfa,16	;
		sacl	esalfalo	;
		sach	esalfa	;

		lacc	zbeta,16	;
		subs	esbetalo	;
		sub	esbeta,16	;
		sacl	eserrlo	;
		sach	eserrhi	;
		lt	eserrlo	;
		mpyu	kslf		;
		pac			;
		sach	smoptemp	;
		lt	eserrhi	;
		mpy	kslf		;
		pac			;
		add	smoptemp	;
		adds	esbetalo	;
		add	esbeta,16	;
		sacl	esbetalo	;
		sach	esbeta	;
*** END Sliding Control Filter


******************************************************
* Rotor Angle Calculator
* thetau = atan(esalfa, -esbeta)
* Note: (esalfa, esbeta)' = (3/2) Ke Omega (-sin(theta), cos(theta))'
******************************************************
Rangle_cal
		lacc	esbeta	;
		sacl	*+		; push variable 2
		lacc	esalfa	;
		neg			;
		sacl	*+		; push variable 1
		call	_atan_div	; range: -pi to pi => (-1.0 - 1.0)
		sbrk	#2		; pop 2 stack levels
		sfr			; devide by 2 => (-1/2 - 1/2)
		and	#07fffH	; modulo 2*pi
		sacl	thetau	;
*** END Rotor Angle Calculator

		sacl	thetae	; Skip angle compensation for now
		b	Smo_rt

******************************************************
* Rotor Angle Compensator
* thetae =+ delta_table(speed reference)
* thetae =+ delta_table(speed reference)
* 2^cmpablel is delta lookup table length
* cmptable_ is delta lookup table start address
******************************************************
;Anglecomp
;		lacc	speedref,16-cmptablel_
;		sach	smoptemp	; scale to index
;		lacc	smoptemp	;
;		add	#cmptable_	;
;		tblr	delta		; read out delta
;
;		lacc	thetau	; add delta to theta
;		add	delta		;
;		and	#07fffH	; modulo 2*pi (or 07fffH)
;		sacl	thetae	;
*** END Rotor Angle Compensation

Smo_rt	spm	#0		; C requirement
		ret
*** END Main Program

**************************************************************
* Include rotor angle compensation table
**************************************************************
;		.include cmptable.tab	; Rotor position compensation table