📄 smopos.asm
字号:
;=====================================================================================
; File name: SMOPOS.ASM
;
; Originator: Digital Control Systems Group
; Texas Instruments
;
; Description: Implementating a sliding mode current observer and estimating
; the back EMF and rotor position for sensorless permanent-
; magnet synchronous motor control
;
; Note that thetae = (0,7FFFh) => (0,360 degree)
;
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 03-01-2001 Release Rev 1.00
;================================================================================
; Applicability: F240,F241,C242,F243,F24xx. (Peripheral Independent).
;================================================================================
; Routine Name: smopos_calc Type: C Callable
;
; C prototype : void smopos_calc(struct SMOPOS *p);
;
; The struct object is defined in the header file "smopos.h" as follows:
;
; typedef struct { int vsalfa; /* Input: Stationary alfa-axis stator voltage (Q15) */
; int esalfa; /* Variable: Stationary alfa-axis back EMF (Q15) */
; int zalfa; /* Output: Stationary alfa-axis sliding control (Q15) */
; int gsmopos; /* Parameter: Motor dependent control gain (Q15) */
; int isalfae; /* Variable: Estimated stationary alfa-axis stator current (Q15) */
; int fsmopos; /* Parameter: Motor dependent plant matrix (Q15) */
; int vsbeta; /* Input: Stationary beta-axis stator voltage (Q15) */
; int esbeta; /* Variable: Stationary beta-axis back EMF (Q15) */
; int zbeta; /* Output: Stationary beta-axis sliding control (Q15) */
; int isbetae; /* Variable: Estimated stationary beta-axis stator current (Q15) */
; int isalfa; /* Input: Stationary alfa-axis stator current (Q15) */
; int isalfaerr; /* Variable: Stationary alfa-axis current error (Q15) */
; int kslide; /* Parameter: Sliding control gain (Q15) */
; int isbeta; /* Input: Stationary beta-axis stator current (Q15) */
; int isbetaerr; /* Variable: Stationary beta-axis current error (Q15) */
; int esalfalo; /* Variable: Stationary alfa-axis back EMF - low word */
; int kslf; /* Parameter: Sliding control filter gain (Q15) */
; int esbetalo; /* Variable: Stationary beta-axis back EMF - low word */
; int thetau; /* Varialbe: Estimated rotor angle (Q15) */
; int thetae; /* Output: Compensated rotor angle (Q15) */
; int comp_ang_flg; /* Input: Compensated rotor angle flag, 1=comp., 0=no comp. */
; int speedref; /* Input: Reference speed (Q15) */
; int (*calc)(); /* Pointer to calculation function */
; } SMOPOS;
;
; Frame Usage Details:
; step | a | b | c | d | e
;____________|_____________|______________|______________|_____________ |___________
; AR0 | smoptemp(1) | eserrlo(3) | ptr_smo(4) | smoptemp(5) | delta(5)
; AR1 | eserrhi(3) | es_dq(4) | | |
; AR2 | smoptemp(3) | esbeta_p(4) | | |
; AR3 | esalfa_p(4) | | | |
;
;================================================================================
.def _smopos_calc
;================================================================================
.ref ATANTAB_45 ; ArcTan look-up table (256 entries)
;================================================================================
; Include rotor angle compensation table
;----------------------------------------------------------------------------------
.include cmptable.tab ; Rotor position compensation table
;================================================================================
__smopos_calc_framesize .set 0004h
;================================================================================
_smopos_calc:
; Assume now ARP=AR1
POPD *+ ; Keep return address
SAR AR0,*+ ; Keep old frame pointer (FP)
SAR AR1,* ; Keep old stack pointer (SP)
LARK AR0,__smopos_calc_framesize ; Load AR0 with frame size
LAR AR0,*0+,AR0 ; AR0->FP0 (new FP), ARP=AR0
;================================================================================
SBRK #3 ; ARP=AR0, AR0->FR0-3 (1st argument)
;----------------------------------------------------------------------------------
LAR AR2,* ; ARP=AR0, AR0->vsalfa, AR2->vsalfa
;----------------------------------------------------------------------------------
ADRK #3 ; ARP=AR0, AR0->FR0, AR2->vsalfa
;----------------------------------------------------------------------------------
MAR *,AR2 ; ARP=AR2, AR0->FR0, AR2->vsalfa
;----------------------------------------------------------------------------------
SETC SXM ; Turn sign extension mode on
; ARP=AR2, AR0->FR0, AR2->vsalfa
;----------------------------------------------------------------------------------
CLRC OVM ; Set overflow mode off
; ARP=AR2, AR0->FR0, AR2->vsalfa
;----------------------------------------------------------------------------------
SPM 1 ; Set product mode left shifted 1 bit
; ARP=AR2, AR0->FR0, AR2->vsalfa
;----------------------------------------------------------------------------------
; (1) Sliding mode current observer
; isalfae = fsmopos*isalfae + gsmopos*(vsalfa-esalfa-zalfa)
; isbetae = fsmopos*isbetae + gsmopos*(vsbeta-esbeta-zbeta)
;----------------------------------------------------------------------------------
SMOI ; ARP=AR2, AR0->FR0, AR2->vsalfa
;----------------------------------------------------------------------------------
LACC *+ ; ACC = vsalfa (Q15)
; ARP=AR2, AR0->FR0, AR2->esalfa
;----------------------------------------------------------------------------------
SUB *+ ; ACC = vsalfa - esalfa (Q15)
; ARP=AR2, AR0->FR0, AR2->zalfa
;----------------------------------------------------------------------------------
SUB *+,AR0 ; ACC = vsalfa - esalfa - zalfa (Q15)
; ARP=AR2, AR0->FR0, AR2->gsmoppos, ARP=AR0
;----------------------------------------------------------------------------------
SACL * ; FR0 = smoptemp = vsalfa - esalfa - zalfa (Q15)
; ARP=AR0, AR0->FR0, AR2->gsmoppos
;----------------------------------------------------------------------------------
LT *,AR2 ; TREG = smoptemp = vsalfa - esalfa - zalfa (Q15)
; ARP=AR0, AR0->FR0, AR2->gsmoppos, ARP=AR2
;----------------------------------------------------------------------------------
MPY *+ ; PREG = gsmopos*(vsalfa-esalfa-zalfa) (Q30)
; ARP=AR2, AR0->FR0, AR2->isalfae
;----------------------------------------------------------------------------------
PAC ; ACC = gsmopos*(vsalfa-esalfa-zalfa) (Q31)
; ARP=AR2, AR0->FR0, AR2->isalfae
;----------------------------------------------------------------------------------
LT *+ ; TREG = isalfae (Q15)
; ARP=AR2, AR0->FR0, AR2->fsmopos
;----------------------------------------------------------------------------------
MPY *- ; PREG = isalfae*fsmopos (Q30)
; ARP=AR2, AR0->FR0, AR2->isalfae
;----------------------------------------------------------------------------------
APAC ; ACC = isalfae*fsmopos+gsmopos*(vsalfa-esalfa-zalfa) (Q31)
; ARP=AR2, AR0->FR0, AR2->isalfae
;----------------------------------------------------------------------------------
SACH * ; isalfae = isalfae*fsmopos+gsmopos*(vsalfa-esalfa-zalfa) (Q15)
; ARP=AR2, AR0->FR0, AR2->isalfae
;----------------------------------------------------------------------------------
ADRK #2 ; ARP=AR2, AR0->FR0, AR2->vsbeta
;----------------------------------------------------------------------------------
LACC *+ ; ACC = vsbeta (Q15)
; ARP=AR2, AR0->FR0, AR2->esbeta
;----------------------------------------------------------------------------------
SUB *+ ; ACC = vsbeta - esbeta (Q15)
; ARP=AR2, AR0->FR0, AR2->zbeta
;----------------------------------------------------------------------------------
SUB *,AR0 ; ACC = vsbeta - esbeta - zbeta (Q15)
; ARP=AR2, AR0->FR0, AR2->zbeta, ARP=AR0
;----------------------------------------------------------------------------------
SACL * ; FR0 = smoptemp = vsbeta - esbeta - zbeta (Q15)
; ARP=AR0, AR0->FR0, AR2->zbeta
;----------------------------------------------------------------------------------
LT *,AR2 ; TREG = smoptemp = vsbeta - esbeta - zbeta (Q15)
; ARP=AR0, AR0->FR0, AR2->zbeta, ARP=AR2
;----------------------------------------------------------------------------------
SBRK #5 ; ARP=AR2, AR0->FR0, AR2->gsmopos
;----------------------------------------------------------------------------------
MPY * ; PREG = gsmopos*(vsbeta-esbeta-zbeta) (Q30)
; ARP=AR2, AR0->FR0, AR2->gsmopos
;----------------------------------------------------------------------------------
PAC ; ACC = gsmopos*(vsbeta-esbeta-zbeta) (Q31)
; ARP=AR2, AR0->FR0, AR2->gsmopos
;----------------------------------------------------------------------------------
ADRK #6 ; ARP=AR2, AR0->FR0, AR2->isbetae
;----------------------------------------------------------------------------------
LT * ; TREG = isbetae (Q15)
; ARP=AR2, AR0->FR0, AR2->isbetae
;----------------------------------------------------------------------------------
SBRK #4 ; ARP=AR2, AR0->FR0, AR2->fsmopos
;----------------------------------------------------------------------------------
MPY * ; PREG = isbetae*fsmopos (Q30)
; ARP=AR2, AR0->FR0, AR2->fsmopos
;----------------------------------------------------------------------------------
APAC ; ACC = isbetae*fsmopos+gsmopos*(vsbeta-esbeta-zbeta) (Q31)
; ARP=AR2, AR0->FR0, AR2->fsmopos
;----------------------------------------------------------------------------------
ADRK #4 ; ARP=AR2, AR0->FR0, AR2->isbetae
;----------------------------------------------------------------------------------
SACH * ; isbetae = isbetae*fsmopos+gsmopos*(vsbeta-esbeta-zbeta) (Q31)
; ARP=AR2, AR0->FR0, AR2->isbetae
;----------------------------------------------------------------------------------
; End: Sliding mode current observer
;----------------------------------------------------------------------------------
; (2) Sliding control calculator
; zalfa = kslide*SIGN(isalfae-isalfa)
; zbeta = kslide*SIGN(isbetae-isbeta)
;----------------------------------------------------------------------------------
SLCNTRL ; ARP=AR2, AR0->FR0, AR2->isbetae
;----------------------------------------------------------------------------------
SBRK #5 ; ARP=AR2, AR0->FR0, AR2->isalfae
;----------------------------------------------------------------------------------
LACC * ; ACC = isalfae (Q15)
; ARP=AR2, AR0->FR0, AR2->isalfae
;----------------------------------------------------------------------------------
ADRK #6 ; ARP=AR2, AR0->FR0, AR2->isalfa
;----------------------------------------------------------------------------------
SUB *+ ; ACC = isalfae - isalfa (Q15)
; ARP=AR2, AR0->FR0, AR2->isalfaerr
;----------------------------------------------------------------------------------
SACL *+ ; isalfaerr = isalfae - isalfa (Q15)
; ARP=AR2, AR0->FR0, AR2->kslide
;----------------------------------------------------------------------------------
BCND EALFAZ,EQ ; Branch to EALFAZ if (isalfae-isalfa)=0
; ARP=AR2, AR0->FR0, AR2->kslide
;----------------------------------------------------------------------------------
BCND EALFAGT,GT ; Branch to EALFAGT if (isalfae-isalfa)>0
; ARP=AR2, AR0->FR0, AR2->kslide
;----------------------------------------------------------------------------------
LACC * ; ACC = kslide (Q15)
; ARP=AR2, AR0->FR0, AR2->kslide
;----------------------------------------------------------------------------------
NEG ; ACC = -kslide (Q15)
; ARP=AR2, AR0->FR0, AR2->kslide
;----------------------------------------------------------------------------------
B STZALFA ; ARP=AR2, AR0->FR0, AR2->kslide
;----------------------------------------------------------------------------------
EALFAZ ; ARP=AR2, AR0->FR0, AR2->kslide
;----------------------------------------------------------------------------------
LACL #0 ; ACC = 0
; ARP=AR2, AR0->FR0, AR2->kslide
;----------------------------------------------------------------------------------
B STZALFA ; ARP=AR2, AR0->FR0, AR2->kslide
;----------------------------------------------------------------------------------
EALFAGT ; ARP=AR2, AR0->FR0, AR2->kslide
;----------------------------------------------------------------------------------
LACC * ; ACC = kslide (Q15)
; ARP=AR2, AR0->FR0, AR2->kslide
;----------------------------------------------------------------------------------
STZALFA ; ARP=AR2, AR0->FR0, AR2->kslide
;----------------------------------------------------------------------------------
SBRK #10 ; ARP=AR2, AR0->FR0, AR2->zalfa
;----------------------------------------------------------------------------------
SACL * ; zalfa = kslide*SIGN(isalfae-isalfa) (Q15)
; ARP=AR2, AR0->FR0, AR2->zalfa
;----------------------------------------------------------------------------------
ADRK #7 ; ARP=AR2, AR0->FR0, AR2->isbetae
;----------------------------------------------------------------------------------
LACC * ; ACC = isbetae (Q15)
; ARP=AR2, AR0->FR0, AR2->isbetae
;----------------------------------------------------------------------------------
ADRK #4 ; ARP=AR2, AR0->FR0, AR2->isbeta
;----------------------------------------------------------------------------------
SUB *+ ; ACC = isbetae - isbeta (Q15)
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -