📄 smopos.asm
字号:
; ARP=AR2, AR0->FR0, AR2->isbetaerr
;----------------------------------------------------------------------------------
SACL * ; isbetaerr = isbetae - isbeta (Q15)
; ARP=AR2, AR0->FR0, AR2->isbetaerr
;----------------------------------------------------------------------------------
SBRK #2 ; ARP=AR2, AR0->FR0, AR2->kslide
;----------------------------------------------------------------------------------
BCND EBETAZ,EQ ; Branch to EBETAZ if (isbetae-isbeta)=0
; ARP=AR2, AR0->FR0, AR2->kslide
;----------------------------------------------------------------------------------
BCND EBETAGT,GT ; Branch to EBETAGT if (isbetae-isbeta)>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 STZBETA ; ARP=AR2, AR0->FR0, AR2->kslide
;----------------------------------------------------------------------------------
EBETAZ ; ARP=AR2, AR0->FR0, AR2->kslide
;----------------------------------------------------------------------------------
LACL #0 ; ACC = 0
; ARP=AR2, AR0->FR0, AR2->kslide
;----------------------------------------------------------------------------------
B STZBETA ; ARP=AR2, AR0->FR0, AR2->kslide
;----------------------------------------------------------------------------------
EBETAGT ; ARP=AR2, AR0->FR0, AR2->kslide
;----------------------------------------------------------------------------------
LACC * ; ACC = kslide (Q15)
; ARP=AR2, AR0->FR0, AR2->kslide
;----------------------------------------------------------------------------------
STZBETA ; ARP=AR2, AR0->FR0, AR2->kslide
;----------------------------------------------------------------------------------
SBRK #4 ; ARP=AR2, AR0->FR0, AR2->zbeta
;----------------------------------------------------------------------------------
SACL * ; zbeta = kslide*SIGN(isbetae-isbeta) (Q15)
; ARP=AR2, AR0->FR0, AR2->zbeta
;----------------------------------------------------------------------------------
; End: Sliding control calculator
;----------------------------------------------------------------------------------
; (3) Sliding control filter - back EMF calculator
; esalfa = esalfa + kslf*(zalfa-esalfa)
; esbeta = esbeta + kslf*(zbeta-esbeta)
;----------------------------------------------------------------------------------
SLCNTRLF2 ; ARP=AR2, AR0->FR0, AR2->zbeta
;----------------------------------------------------------------------------------
SBRK #6 ; ARP=AR2, AR0->FR0, AR2->zalfa
;----------------------------------------------------------------------------------
LACC *,16 ; ACC = zalfa (Q31)
; ARP=AR2, AR0->FR0, AR2->zalfa
;----------------------------------------------------------------------------------
ADRK #13 ; ARP=AR2, AR0->FR0, AR2->esalfalo
;----------------------------------------------------------------------------------
SUBS * ; ACC = zalfa - esalfa (Q31)
; ARP=AR2, AR0->FR0, AR2->esalfalo
;----------------------------------------------------------------------------------
SBRK #14 ; ARP=AR2, AR0->FR0, AR2->esalfa
;----------------------------------------------------------------------------------
SUB *,16,AR0 ; ACC = zalfa - esalfa (Q31)
; ARP=AR2, AR0->FR0, AR2->esalfa, ARP=AR0
;----------------------------------------------------------------------------------
SACL *+ ; FR0 = eserrlo = zalfa - esalfa (Q31)
; ARP=AR0, AR0->FR1, AR2->esalfa
;----------------------------------------------------------------------------------
SACH *- ; FR1 = eserrhi = zalfa - esalfa (Q31)
; ARP=AR0, AR0->FR0, AR2->esalfa
;----------------------------------------------------------------------------------
LT *,AR2 ; TREG = eserrlo
; ARP=AR0, AR0->FR0, AR2->esalfa, ARP=AR2
;----------------------------------------------------------------------------------
ADRK #15 ; ARP=AR2, AR0->FR0, AR2->kslf (>0)
;----------------------------------------------------------------------------------
MPYU *,AR0 ; PREG = kslf* eserrlo
; ARP=AR2, AR0->FR0, AR2->kslf (>0), ARP=AR0
;----------------------------------------------------------------------------------
PAC ; ACC = kslf* eserrlo
; ARP=AR0, AR0->FR0, AR2->kslf (>0)
;----------------------------------------------------------------------------------
ADRK #2 ; ARP=AR0, AR0->FR2, AR2->kslf (>0)
;----------------------------------------------------------------------------------
SACH *- ; FR2 = smoptemp = kslf*eserrlo
; ARP=AR0, AR0->FR1, AR2->kslf (>0)
;----------------------------------------------------------------------------------
LT *+,AR2 ; TREG = eserrhi
; ARP=AR0, AR0->FR2, AR2->kslf (>0), ARP=AR2
;----------------------------------------------------------------------------------
MPY *-,AR0 ; PREG = kslf*eserrhi
; ARP=AR2, AR0->FR2, AR2->esalfalo, ARP=AR0
;----------------------------------------------------------------------------------
PAC ; ACC = kslf*eserrhi
; ARP=AR0, AR0->FR2, AR2->esalfalo
;----------------------------------------------------------------------------------
ADD *-,AR2 ; ACC = smoptemp + kslf*eserrhi
; ARP=AR0, AR0->FR1, AR2->esalfalo, ARP=AR2
;----------------------------------------------------------------------------------
ADDS * ; ACC = esalfa+smoptemp+kslf*eserrhi (Q31)
; ARP=AR2, AR0->FR1, AR2->esalfalo
;----------------------------------------------------------------------------------
SBRK #14 ; ARP=AR2, AR0->FR1, AR2->esalfa
;----------------------------------------------------------------------------------
ADD *,16 ; ACC = esalfa+smoptemp+kslf*eserrhi (Q31)
; ARP=AR2, AR0->FR1, AR2->esalfa
;----------------------------------------------------------------------------------
SACH * ; esalfa = esalfa+smoptemp+kslf*eserrhi (Q31)
; ARP=AR2, AR0->FR1, AR2->esalfa
;----------------------------------------------------------------------------------
ADRK #14 ; ARP=AR2, AR0->FR1, AR2->esalfalo
;----------------------------------------------------------------------------------
SACL * ; esalfa = esalfa+smoptemp+kslf*eserrhi (Q31)
; ARP=AR2, AR0->FR1, AR2->esalfalo
;----------------------------------------------------------------------------------
SBRK #7 ; ARP=AR2, AR0->FR1, AR2->zbeta
;----------------------------------------------------------------------------------
LACC *,16 ; ACC = zbeta (Q31)
; ARP=AR2, AR0->FR1, AR2->zbeta
;----------------------------------------------------------------------------------
ADRK #9 ; ARP=AR2, AR0->FR1, AR2->esbetalo
;----------------------------------------------------------------------------------
SUBS * ; ACC = zbeta - esbeta (Q31)
; ARP=AR2, AR0->FR1, AR2->esbetalo
;----------------------------------------------------------------------------------
SBRK #10 ; ARP=AR2, AR0->FR1, AR2->esbeta
;----------------------------------------------------------------------------------
SUB *,16,AR0 ; ACC = zbeta - esbeta (Q31)
; ARP=AR2, AR0->FR1, AR2->esbeta, ARP=AR0
;----------------------------------------------------------------------------------
SBRK #1 ; ARP=AR0, AR0->FR0, AR2->esbeta
;----------------------------------------------------------------------------------
SACL *+ ; FR0 = eserrlo = zbeta - esbeta (Q31)
; ARP=AR0, AR0->FR1, AR2->esbeta
;----------------------------------------------------------------------------------
SACH *- ; FR1 = eserrhi = zbeta - esbeta (Q31)
; ARP=AR0, AR0->FR0, AR2->esbeta
;----------------------------------------------------------------------------------
LT *,AR2 ; TREG = eserrlo
; ARP=AR0, AR0->FR0, AR2->esbeta, ARP=AR2
;----------------------------------------------------------------------------------
ADRK #9 ; ARP=AR2, AR0->FR0, AR2->kslf (>0)
;----------------------------------------------------------------------------------
MPYU *,AR0 ; PREG = kslf* eserrlo
; ARP=AR2, AR0->FR0, AR2->kslf (>0), ARP=AR0
;----------------------------------------------------------------------------------
PAC ; ACC = kslf* eserrlo
; ARP=AR0, AR0->FR0, AR2->kslf (>0)
;----------------------------------------------------------------------------------
ADRK #2 ; ARP=AR0, AR0->FR2, AR2->kslf (>0)
;----------------------------------------------------------------------------------
SACH *- ; FR2 = smoptemp = kslf*eserrlo
; ARP=AR0, AR0->FR1, AR2->kslf (>0)
;----------------------------------------------------------------------------------
LT *+,AR2 ; TREG = eserrhi
; ARP=AR0, AR0->FR2, AR2->kslf (>0), ARP=AR2
;----------------------------------------------------------------------------------
MPY *+,AR0 ; PREG = kslf*eserrhi
; ARP=AR2, AR0->FR2, AR2->esbetalo, ARP=AR0
;----------------------------------------------------------------------------------
PAC ; ACC = kslf*eserrhi
; ARP=AR0, AR0->FR2, AR2->esbetalo
;----------------------------------------------------------------------------------
ADD *,AR2 ; ACC = smoptemp + kslf*eserrhi
; ARP=AR0, AR0->FR2, AR2->esbetalo, ARP=AR2
;----------------------------------------------------------------------------------
ADDS * ; ACC = esbeta+smoptemp+kslf*eserrhi (Q31)
; ARP=AR2, AR0->FR2, AR2->esbetalo
;----------------------------------------------------------------------------------
SBRK #10 ; ARP=AR2, AR0->FR2, AR2->esbeta
;----------------------------------------------------------------------------------
ADD *,16 ; ACC = esbeta+smoptemp+kslf*eserrhi (Q31)
; ARP=AR2, AR0->FR2, AR2->esbeta
;----------------------------------------------------------------------------------
SACH * ; esbeta = esbeta+smoptemp+kslf*eserrhi (Q31)
; ARP=AR2, AR0->FR2, AR2->esbeta
;----------------------------------------------------------------------------------
ADRK #10 ; ARP=AR2, AR0->FR2, AR2->esbetalo
;----------------------------------------------------------------------------------
SACL * ; esbeta = esbeta+smoptemp+kslf*eserrhi (Q31)
; ARP=AR2, AR0->FR2, AR2->esbetalo
;----------------------------------------------------------------------------------
; End: Sliding control filter
;----------------------------------------------------------------------------------
; (4) Rotor angle calculator
; thetau = atan(-esalfa,esbeta)
; Note: (esalfa,esbeta)' = (3/2) Ke Omega (-sin(theta),cos(theta))'
;----------------------------------------------------------------------------------
RANGLE_CAL ; ARP=AR2, AR0->FR2, AR2->esbetalo
;----------------------------------------------------------------------------------
SBRK #10 ; ARP=AR2, AR0->FR2, AR2->esbeta
;----------------------------------------------------------------------------------
; Checking |esalfa|/|esbeta| > 1 ?
LACC *,AR0 ; ACC = esbeta (Q15)
; ARP=AR2, AR0->FR2, AR2->esbeta, ARP=AR0
;----------------------------------------------------------------------------------
ABS ; ACC = |esbeta| (Q15)
; ARP=AR0, AR0->FR2, AR2->esbeta
;----------------------------------------------------------------------------------
SACL *+,AR2 ; FR2 = esbeta_p = |esbeta| (Q15)
; ARP=AR0, AR0->FR3, AR2->esbeta, ARP=AR2
;----------------------------------------------------------------------------------
SBRK #6 ; ARP=AR2, AR0->FR2, AR2->esalfa
;----------------------------------------------------------------------------------
LACC *,AR0 ; ACC = esalfa (Q15)
; ARP=AR2, AR0->FR3, AR2->esalfa, ARP=AR0
;----------------------------------------------------------------------------------
ABS ; ACC = |esalfa| (Q15)
; ARP=AR0, AR0->FR3, AR2->esalfa
;----------------------------------------------------------------------------------
SACL *- ; FR3 = esalfa_p = |esalfa| (Q15)
; ARP=AR0, AR0->FR2, AR2->esalfa
;----------------------------------------------------------------------------------
SUB * ; ACC = |esalfa|-|esbeta| (Q15)
; ARP=AR0, AR0->FR2, AR2->esalfa
;----------------------------------------------------------------------------------
BCND SMO_LARGE_QD,GT ; Branch to LARGE_QD if |esalfa|>|esbeta|
; ARP=AR0, AR0->FR2, AR2->esalfa
;----------------------------------------------------------------------------------
BCND SMO_EQ_QD,EQ ; Branch to EQ_QD if |esalfa|=|esbeta|
; ARP=AR0, AR0->FR2, AR2->esalfa
;----------------------------------------------------------------------------------
SMO_SMALL_QD ; Here, |esalfa|/|esbeta| is less than 1.
; ARP=AR0, AR0->FR2, AR2->esalfa
;----------------------------------------------------------------------------------
ADRK #1 ; ARP=AR0, AR0->FR3, AR2->esalfa
;----------------------------------------------------------------------------------
LACC *-,15 ; ACC = FR3 = |esalfa| left shifted by 15 (es_qd=Q15)
; ARP=AR0, AR0->FR2, AR2->esalfa
;----------------------------------------------------------------------------------
RPT #15 ; Repeat SUBC 16 times
; ARP=AR0, AR0->FR2, AR2->esalfa
;----------------------------------------------------------------------------------
SUBC * ; Dividing |esalfa|/|esbeta|
; ARP=AR0, AR0->FR2, AR2->esalfa
;----------------------------------------------------------------------------------
SBRK #1 ; ARP=AR0, AR0->FR1, AR2->esalfa
;----------------------------------------------------------------------------------
SACL * ; FR1 = es_qd = |esalfa|/|esbeta| (Q15)
; ARP=AR0, AR0->FR1, AR2->esalfa
;----------------------------------------------------------------------------------
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -