📄 foc.asm
字号:
BCND UPIMAGZEROT,NTC ;如果正,跳转
LACC VMIN ;否则是负,检测是否超过电压下限
SUB UPI
BCND NEG_SATT,GT ;超过下限进入饱和区则跳转
LACC UPI ;否则正常调整
B LIMITERT
NEG_SATT
LACC VMIN ;ACC =下限值
B LIMITERT
UPIMAGZEROT
LACC VMAX ;检测是否超过电压上限
SUB UPI
BCND POS_SATT,LT ;超过上限进入饱和区则跳转
LACC UPI ;否则正常调整
B LIMITERT
POS_SATT
LACC VMAX ;ACC =上限值
LIMITERT
SACL VTREF ;输出VTREF
SUB UPI
SACL ELPI ;求极限偏差
LT ELPI
MPY KC ;积分修正系数,Q12
PAC
LT EPIT
MPY KI ;积分系数,Q12
APAC
ADD XIT,12
SACH XIT,4 ;更新调节器积分累计量
;-----------------------M轴电流PI调节,输出VMREF-------------------------------------------------------
LACC IMREF
SUB IM
SACL EPIM ;M轴电流偏差
LACC XIM,12 ;电流调节器积分累计量
LT EPIM
MPY KP ;比例系数,Q12
APAC
SACH UPI,4
BIT UPI,0 ;检测调节器输出的正负
BCND UPIMAGZEROM,NTC ;如果正,跳转
LACC VMIN ;否则是负,检测是否超过电压下限
SUB UPI
BCND NEG_SATM,GT ;超过下限进入饱和区则跳转
LACC UPI ;否则正常调整
B LIMITERM
NEG_SATM
LACC VMIN ;ACC =下限值
B LIMITERM
UPIMAGZEROM
LACC VMAX ;检测是否超过电压上限
SUB UPI
BCND POS_SATM,LT ;超过上限进入饱和区则跳转
LACC UPI ;否则正常调整
B LIMITERM
POS_SATM
LACC VMAX ;ACC =上限值
LIMITERM
SACL VMREF ;输出VMREF
SUB UPI
SACL ELPI
LT ELPI
MPY KC ;积分修正系数,Q12
PAC
LT EPIM
MPY KI ;积分系数,Q12
APAC
ADD XIM,12
SACH XIM,4 ;更新调节器积分累计量
;-----------------------PARK反变换--------------------------------------------------------------------------------
LACC #0
LT VMREF ;T=VMREF
MPY SIN ;P=VMREF*SIN(TETA_E)
LTA VTREF ;ACC=P; T=VTREF
MPY COS ;P=VTREF*COS(TETA_E)
MPYA SIN ;ACC=P; P=VTREF*SIN(TETA_E)
SACH VBET_REF,1 ;保存VBET_REF
LACC #0
LT VMREF ;T=VMREF
MPYS COS ;ACC= -VTREF*SIN(TETA_E),
;P=VMREF*COS(TETA_E)
APAC ;ACC= -VTREF*SIN(TETA_E)
;+VMREF*COS(TETA_E)
SACH VALF_REF,1 ;保存VALF_REF
;----------------------- SVPWM-----------------------------------------------------------------------------------
;-----------------------计算扇区数SECTOR-------------------------------------------------------
LACC #0 ;P清零
SACL P
LACC VBET_REF ;B0
BCND B0_NEG, LEQ ;B0≤0跳转
LACC #1
SACL P ;否则P=1
B0_NEG
LT VALF_REF ;计算B1
MPY A3 ;乘 的Q12格式
PAC
SACH TMP,4
LACC TMP
SUB VBET_REF
SFR ;除2
BCND B1_NEG,LEQ ;B1≤0跳转
LACC P
ADD #2
SACL P ;否则P+2
B1_NEG
LACC TMP ;计算B2
ADD VBET_REF
SFR ;除2
NEG ;求补
BCND B2_NEG,LEQ ;B2≤0跳转
LACC P
ADD #4
SACL P ;否则P+4
B2_NEG
LACC #PSECTOR ;指向表头
ADD P
SUB #1
TBLR SECTOR ;得到扇区数
;-----------------------根据逆阵计算T1,T2---------------------------------------------------------------
LACC #DEC_MS ;逆阵数据首地址
ADD SECTOR,2
SACL TMP ;产生地址指针
LAR AR0,TMP ;指向逆阵表
MAR *,AR0
LT VALF_REF ;Q12格式.
;计算VALF_REF *M(1,1)+VBET_REF*M(1,2)
MPY *+ ;M(1,1)* VALF_REF, Q11
PAC ;Q10格式
LT VBET_REF ;Q12
MPY *+ ;M(1,2)* VBET_REF, Q11
APAC ;0.5*C1,Q10
BCND CMP1BIG0,GEQ ;如果大于0继续
LACC #0 ;否则0
CMP1BIG0
SACH TMP ;0.5*C1,Q10格式
LT TMP ;Q10格式
MPY T1_PERIODS ;Q0
PAC ;Q10格式
SACH CMP_1 ,6 ;0.5*C1*TP,Q0格式
LT VALF_REF ;计算VALF_REF*M(2,1)+VBET_REF*M(2,2)
MPY *+ ;M(2,1) VALF_REF
PAC ;Q10格式
LT VBET_REF ;Q12格式
MPY *+ ;M(2,2) VBET_REF
APAC ;0.5*C2,Q10
BCND CMP2BIG0,GEQ ;如果大于0继续
LACC #0 ;否则0
CMP2BIG0
SACH TMP ;0.5*C2,Q10格式
LT TMP ;Q10格式
MPY T1_PERIODS ;Q0
PAC ;Q10格式
SACH CMP_2 ,6 ;0.5*C2*TP,Q0格式
LACC #PWM_T ;T1周期值
SUB CMP_1
SUB CMP_2 ;Q0格式
BCND CMP0BIG0,GEQ ;如果大于0继续
LACC #0 ;否则0
CMP0BIG0
SACL CMP_0
LACC CMP_0,15 ;相当于右移1位,除2
SACH CMP_0 ;0.25*C0*TP
LACC #FIRST_ ;指向第一次比较匹配的比较寄存器地址表入口地址
ADD SECTOR
TBLR FIRST_TOG ;查到第一次比较匹配的比较器地址
LAR AR0,FIRST_TOG ;指向该地址
LACC CMP_0
SACL * ;CMP_0送入该比较器
LACC #SECOND_ ;指向第二次比较匹配的比较寄存器地址表入口地址
ADD SECTOR
TBLR SEC_TOG ;查到第二次比较匹配的比较器地址
LAR AR0,SEC_TOG ;指向该地址
LACC CMP_0
ADD CMP_1 ;CMP_0+CMP_1
SACL * ;送入该比较器
LACC #CMPR3
SUB FIRST_TOG
ADD #CMPR2
SUB SEC_TOG
ADD #CMPR1
SACL TMP ;计算第三次比较匹配的比较器地址
LAR AR0,TMP ;指向该地址
LACC CMP_0
ADD CMP_1
ADD CMP_2 ; CMP_0+CMP_1+CMP_2
SACL * ;送入该比较器
B REST
;---------------------------------------状态检测－－－－－－－－－－－－－
StateDetect
;----------------------------检测直流电压－－－－－－－－－－－－－－－
; LDP #6
;LACL UDC
;SUB #615
;ABS
;SUB #329
;BGZ NORMAL
;LED_D7_OFF
;B NEXT3
;NORMAL LED_D7_ON
NEXT3
LDP #6
LACC IA ; q12
ABS
SUB #MAXCURRENT
BGZ GUOLIU
LACC IB ;q12
ABS
SUB #MAXCURRENT
BGZ GUOLIU1
LACC IC ; q12
ABS
SUB #MAXCURRENT
BGZ GUOLIU2
RET
GUOLIU SETC INTM
STOP_PWM
LED_D9_ON
B GUOLIU
GUOLIU1 SETC INTM
STOP_PWM
LED_D9_ON
B GUOLIU1
GUOLIU2 SETC INTM
STOP_PWM
LED_D9_ON
B GUOLIU2
;-----------------------数据区------------------------------------------------------------------------------
.data
PSECTOR
.word 1 ;扇区数
.word 5
.word 0
.word 3
.word 2
.word 4
SINTAB .word 0 ; 0 0 0.0000
.word 804 ; 1 1.41 0.0245
.word 1608 ; 2 2.81 0.0491
.word 2410 ; 3 4.22 0.0736
.word 3212 ; 4 5.63 0.0980
.word 4011 ; 5 7.03 0.1224
.word 4808 ; 6 8.44 0.1467
.word 5602 ; 7 9.84 0.1710
.word 6393 ; 8 11.25 0.1951
.word 7179 ; 9 12.66 0.2191
.word 7962 ; 10 14.06 0.2430
.word 8739 ; 11 15.47 0.2667
.word 9512 ; 12 16.88 0.2903
.word 10278 ; 13 18.28 0.3137
.word 11039 ; 14 19.69 0.3369
.word 11793 ; 15 21.09 0.3599
.word 12539 ; 16 22.50 0.3827
.word 13279 ; 17 23.91 0.4052
.word 14010 ; 18 25.31 0.4276
.word 14732 ; 19 26.72 0.4496
.word 15446 ; 20 28.13 0.4714
.word 16151 ; 21 29.53 0.4929
.word 16846 ; 22 30.94 0.5141
.word 17530 ; 23 32.34 0.5350
.word 18204 ; 24 33.75 0.5556
.word 18868 ; 25 35.16 0.5758
.word 19519 ; 26 36.56 0.5957
.word 20159 ; 27 37.97 0.6152
.word 20787 ; 28 39.38 0.6344
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -