📄 foc.asm
字号:
ENCMINMAX
LACC TMP ;TMP的值为T2CNT，即上一次的脉冲计数值
SACL ENCODEROLD ;更新ENCODEROLD
;-----------------------计算绝对电角度-----------------------------------------------------------------------
;此段程序对应于感应电动机中计算转子磁链的那段程序，相比于感应电机，PMSM只需算出转子的绝对位置就可知道转子磁链的位置，因此只需以下几句话
LT TETA_M
MPY #KENCODER ;乘磁极对数KENCODER=3
PAC ;Q12格式
AND #0FFFH ;屏蔽高位,目的是乘以极对数后若超出360范围，对其进行约束
SACL TETA_E ;存放
;-----------------------计算转速---------------------------------------------------------------------------
LACC SPEEDSTEP ;检测是否该采样速度
SUB #1
SACL SPEEDSTEP
BCND NOCALC,GT ;没到采样时刻,退出
LT SPEEDTMP ;到采样时刻,计算编码器增量累计值
MPY #KSPEED ;Q8格式
PAC
SFL
SACH N,7 ;相当于右移8位
LACC #0
SACL SPEEDTMP ;SPEEDTMP清零,SPEEDTMP为脉冲累计值,每隔一个速度采样周期(PWM周期*28)清一次零,每一个电流采样周期(PWM采样周期)将 ENCINCR 累计值存入SPEEDTMP 中
LACC #SPEEDSTEP28 ;SPEEDSTEP=28
SACL SPEEDSTEP ;SPEEDSTEP重新赋初值
;-----------------------转速PI调节,输出IQREF----------------------------------------------------------
LACC N_REF ;转速给定值,外部输入
SUB N
SACL EPISPEED ;转速偏差
LACC XISPEED,12 ;转速调节器积分累计量
LT EPISPEED
MPY KPISPEED ;乘比例系数6800H(6.5的Q12格式)
APAC ;累加
SACH UPI,4 ;相当于右移12位
BIT UPI,0 ;检测调节器输出的正负
BCND UPIMAGZEROS,NTC ;如果正,跳转
LACC ISQREFMIN ;否则是负,检测是否超过电流下限
SUB UPI
BCND NEG_SAT,GT ;超过下限进入饱和区则跳转
LACC UPI ;否则正常调整
B LIMITERS
NEG_SAT
LACC ISQREFMIN ; ACC =下限值
B LIMITERS
UPIMAGZEROS
LACC ISQREFMAX ;检测是否超过电流上限
SUB UPI
BCND POS_SAT,LT ;超过上限进入饱和区则跳转
LACC UPI ;否则正常调整
B LIMITERS
POS_SAT
LACC ISQREFMAX ; ACC =上限值
LIMITERS
SACL ISQREF ;输出IQREF
SUB UPI
SACL ELPI ;求极限偏差
LT ELPI
MPY KCORSPEED ;KCORSPEED= KISPEED/ KPISPEED
PAC
LT EPISPEED
MPY KISPEED ;积分系数
APAC
ADD XISPEED,12
SACH XISPEED,4 ;更新调节器积分累计量
;以上转速PI调节,输出IQREF程序和对应的感应电机程序是一样的，只是定义的变量名称不太一样
;-----------------------计算编码增量累计值------------------------------------------------------------------
NOCALC
LACC SPEEDTMP
ADD ENCINCR
SACL SPEEDTMP
;上面三句与感应电机程序一至
;-----------------------将IA,IB,IC转换成IPUQ12格式---------------------------------------------------------
WAIT
LDP #0
LACC IA
AND #3FFH ;屏蔽高位
SUB #512 ;向下平移,产生正负电流值
SACL TMP
LT TMP
MPY #KCURRENT ;转换系数,Q8格式
PAC
SFL
SACH IA,7 ;IA, Q12格式
LACC IB
AND #3FFH
SUB #512
SACL TMP
LT TMP
MPY #KCURRENT
PAC
SFL
SACH IB,7
LACC IB
ADD IA
NEG
SACL IC ;IC = -(IB+IA)
;"将IA,IB,IC转换成IPUQ12格式"部分与感应电动机程序一致
;-----------------------CLARKE变换--------------------------------------------------------------------------------
LDP #0
LT IA
MPY #5018 ;乘 =5018,Q12格式
PAC
SACH ISALFA,4 ;保存ISALFA
LACC IB,1 ;ACC =2*IB
ADD IA ;ACC =IA+2*IB
SACL TMP ;暂存
LT ISALFA
MPY #2896 ;乘 =2896,Q12格式
PAC
SACH ISBETA ,4 ;保存ISBETA
;CLARKE变换程序与感应电动机完全一至
;-----------------------根据TETA_E查SIN, COS表------------------------------------------------------------
LACC TETA_E ;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 ISBETA
MPY SIN ;Q12格式
LTA ISALFA ;ACC=ISBETA *SIN(TETA),T=ISALFA
MPY COS ;Q12格式
MPYA SIN ;ACC=ISBETA*SIN(TETA)+ISALFA*COS(TETA)，
;P=ISALFA*SIN(TETA)
SACH ISD,4 ;保存ISD
LACC #0 ;累加器清零
LT ISBETA
MPYS COS ;ACC = -ISALFA*SIN(TETA) ,P=ISBETA*COS(TETA)
APAC ;ACC = -ISALFA*SIN(TETA) +ISBETA*COS(TETA)
SACH ISQ,4 ;保存ISQ
;与感应电动机程序一样，只是定义的变量名称不同
;----------------------- Q轴电流PI调节,输出VSQREF ------------------------------------------------------------
LACC ISQREF
SUB ISQ ;ISQ=IT(感应电机中定义)
SACL EPIQ ;Q轴电流偏差
LACC XIQ,12 ;电流调节器积分累计量
LT EPIQ
MPY KPI ;比例系数
APAC
SACH UPI,4
BIT UPI,0 ;检测调节器输出的正负
BCND UPIMAGZEROQ,NTC ;如果正,跳转
LACC #VMIN ;否则是负,检测是否超过电压下限
SUB UPI
BCND NEG_SATQ,GT ;超过下限进入饱和区则跳转
LACC UPI ;否则正常调整
B LIMITERQ
NEG_SATQ
LACC #VMIN ;ACC =下限值
B LIMITERQ
UPIMAGZEROQ
LACC #VMAX ;检测是否超过电压上限
SUB UPI
BCND POS_SATQ,LT ;超过上限进入饱和区则跳转
LACC UPI ;否则正常调整
B LIMITERQ
POS_SATQ
LACC #VMAX ; ACC =上限值
LIMITERQ
SACL VSQREF ;输出VSQREF
SUB UPI
SACL ELPI ;求极限偏差
LT ELPI
MPY KCOR ; KCOR= KI/ KPI
PAC
LT EPIQ
MPY KI ;积分系数
APAC
ADD XIQ,12
SACH XIQ,4 ;更新调节器积分累计量
;对应于感应电机的T轴调节程序
;-----------------------D轴电流PI调节,输出VSDREF-------------------------------------------------------
LACC ISDREF
SUB ISD
SACL EPID ;D轴电流偏差
LACC XID,12 ;电流调节器积分累计量
LT EPID
MPY KPI ;比例系数
APAC
SACH UPI,4
BIT UPI,0 ;检测调节器输出的正负
BCND UPIMAGZEROD,NTC ;如果正,跳转
LACC #VMIN ;否则是负,检测是否超过电压下限
SUB UPI
BCND NEG_SATD,GT ;超过下限进入饱和区则跳转
LACC UPI ;否则正常调整
B LIMITERD
NEG_SATD
LACC #VMIN ;ACC =下限值
B LIMITERD
UPIMAGZEROD
LACC #VMAX ;检测是否超过电压上限
SUB UPI
BCND POS_SATD,LT ;超过上限进入饱和区则跳转
LACC UPI ;否则正常调整
B LIMITERD
POS_SATD
LACC #VMAX ; ACC =上限值
LIMITERD
SACL VSDREF ; 输出VSDREF
SUB UPI
SACL ELPI
LT ELPI
MPY KCOR
PAC
LT EPID
MPY KI
APAC
ADD XID,12
SACH XID,4 ;更新调节器积分累计量
;对应于感应电机程序中的M轴电流调节
;-----------------------PARK反变换--------------------------------------------------------------------------------
LACC #0
LT VSDREF ;T=VSDREF
MPY SIN ;P=VSDREF*SIN(TETA_E)
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -