📄 foc.asm
字号:
BIT UPI,0 ;检测调节器输出的正负
BCND UPIMAGZEROS,NTC ;如果正,跳转
LACC ITREFMIN ;否则是负,检测是否超过电流下限
SUB UPI
BCND NEG_SAT,GT ;超过下限进入饱和区则跳转
LACC UPI ;否则正常调整
B LIMITERS
NEG_SAT
LACC ITREFMIN ;ACC =下限值
B LIMITERS
UPIMAGZEROS
LACC ITREFMAX ;检测是否超过电流上限
SUB UPI
BCND POS_SAT,LT ;超过上限进入饱和区则跳转
LACC UPI ;否则正常调整
B LIMITERS
POS_SAT
LACC ITREFMAX ;ACC =上限值
LIMITERS
SACL ITREF ;输出ITREF
SUB UPI
SACL ELPI ;求极限偏差
LT ELPI
MPY KCN ;积分修正系数,Q12
PAC
LT EPISPEED
MPY KIN ;积分系数,Q12
APAC
ADD XISPEED,12
SACH XISPEED,4 ;更新调节器积分累计量
;-----------------------计算编码增量累计值------------------------------------------------------------------
NOCALC
LACC SPEEDTMP
ADD ENCINCR
SACL SPEEDTMP
;-----------------------将IA,IB,IC转换成IPUQ12格式---------------------------------------------------------
LDP #6
LACC IA
AND #3FFH ;屏蔽高位
SUB #511 ;向下平移,产生正负电流值
SACL TMP
LT TMP
MPY KCURRENT ;转换系数,Q8格式
PAC
SFL
SACH IA,7 ;IA, Q12格式
LACC IB
AND #3FFH
SUB #501
SACL TMP
LT TMP
MPY KCURRENT
PAC
SFL
SACH IB,7
LACC IB
ADD IA
NEG
SACL IC ;IC = -(IB+IA)
;------------------------------------状态检测-------------------------------------------------------------------
CALL StateDetect
;-------------------------------------数模输出---------------------------------------------------------------------
OUT N_REF ,DAC0
OUT N , DAC1
OUT IT ,DAC2
OUT N ,DAC_UPDATA
DebugData ; 调试部分
LDP #6
MAR * ,AR1
LACL N_REF
SACL *+
mar *,AR2
lacl N
SACL *+
mar *,AR3
lacl N
sacl *+
LAR AR0 ,#(BUF_START2+SIZE)
CMPR 1
BCND EXIT ,TC
LAR AR1 ,#(BUF_START+SIZE)
LAR AR2 ,#(BUF_START1+SIZE)
LAR AR3 ,#(BUF_START2+SIZE)
EXIT
;-----------------------CLARKE变换--------------------------------------------------------------------------------
LDP #6
LT IA
MPY A1 ;乘sqrt(3/2) =5017,Q12格式
PAC
SACH IALFA, 4 ;保存IALFA q12
LACC IB,1 ;ACC =2*IB
ADD IA ;ACC =IA+2*IB
SACL TMP ;暂存
LT TMP
MPY A2 ;乘sqrt(2)/2 =2896,Q12格式
PAC
SACH IBETA ,4 ;保存IBETA q12
;-----------------------根据TETA_E查SIN, COS表------------------------------------------------------------
LACC TETA_E1 ;TETA_E范围[0;1000H],[0;360]的Q12格式
RPT #3 ;右移4位,范围变为[0;255],
SFR
AND #0FFH ;屏蔽高位
SACL INDEX ;生成查表指针
ADD #SINTAB ;加上表的首地址
TBLR SIN ;保存SIN值
LACL INDEX ;COS(TETA)=SIN(TETA+90°)
ADD #040H ;90°= 40H
AND #0FFH
ADD #SINTAB
TBLR COS ;保存COS值
;-------------------------------------- PARK变换-------------------------------------------------------------
LACC #0 ;累加器清零
LT IBETA
MPY SIN ;Q12格式
LTA IALFA ;ACC=IBETA *SIN(TETA),T=IALFA
MPY COS ;Q12格式
MPYA SIN ;ACC=IBETA*SIN(TETA)+IALFA*COS(TETA)，
;P=IALFA*SIN(TETA)
SACH IM,1 ;保存IM
LACC #0 ;累加器清零
LT IBETA
MPYS COS ;ACC = -IALFA*SIN(TETA) ,P=IBETA*COS(TETA)
APAC ;ACC = -IALFA*SIN(TETA) +IBETA*COS(TETA)
SACH IT,1 ;保存IT
;--------------------------------------转子磁链位置的计算-----------------------------------
LACC IM ;Q12
SUB IDK ;Q12
SACL TMP
LT TMP
MPY KR ;Q15
PAC
SACH TMP,1
LACC TMP
ADD IDK
SACL IDK ;IDK=IDK+KR*(IM-IDK), Q12格式
BCND IDKNOTZERO,NEQ ;如果IDK≠0跳转
LACC #0
SACL TMP ;如果IDK=0,则TMP=IT/IDK=0
B ITPOS
IDKNOTZERO
SACL TMP1 ;暂存IDK,Q12
LACC IT
ABS ;取绝对值
SACL TMP ;暂存IT
LACC TMP ,12 ;右移12位成Q24格式
RPT #15
SUBC TMP1 ;除法
SACL TMP ;TMP=IT/IDK,Q12格式
LACC IT ;根据IT的正负调整商的符号
BCND ITPOS,GT ;IT＞0跳转
LACC TMP ;否则求补
NEG
SACL TMP
ITPOS
LT TMP
MPY KT ;Q12格式
PAC
SACH TMP,4 ;TMP=TMP*KT,Q12格式
LACC TMP
ADD N
;SFR
SACL FS ;FS=N+KT*(IT/IDK),Q12格式
LACC FS
ABS
SACL TMP
LT TMP
MPY K ;Q0
PAC ;计算TETA_E=TETA_E+K*FS
;=TETA_E+TETAINCR
;(0-360)->(0-65535)
SACH TETAINCR,4 ;Q0格式
BIT FS,0 ;根据FS的正负调整
BCND FS_NEG,TC ;为负则跳转
LACL TETAINCR ;否则为正
ADDS TETA_E
SACL TETA_E
B FS_POS
FS_NEG
LACL TETA_E
SUBS TETAINCR
SACL TETA_E
FS_POS
LACL TETA_E
RPT #3
SFR
SACL TETA_E1
NEXT
;-----------------------最小电流控制环节----------------------------- ;07.10.17改
LDP #225
LACL PADATDIR
LDP #6
SACL KEYDATA1
BIT KEYDATA1 ,BIT5
BCND NTX ,TC
LACC IMREF
SUB #366
BCND NTX, LEQ
LACC IM
SUB IT
SACL I0
LACC I0
BCND NTX, LEQ
lt IT
mpy IM
pac
sach I0 ,4
lt I0 ;Q2-14
mpy #ksqrt ;Q0
pac ;q12
add #1,11 ;加到最末位，用于四舍五入
sach I1,3 ; 应该左移四位，这理移三位是应为以2为基准，乘积出来要除以2这个基准值
lacc I1
add #sqrttab
sacl I0
tblr IMREF ;Q4-12
;-----------------------T轴电流PI调节,输出VTREF-----------------------------
NTX
SPLK #200, ITREF
LACC ITREF
SUB IT
SACL EPIT ;T轴电流偏差
LACC XIT,12 ;电流调节器积分累计量
LT EPIT
MPY KP ;比例系数,Q12
APAC
SACH UPI,4
BIT UPI,0 ;检测调节器输出的正负
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -