📄 motor velocity control dsp program.txt
字号:
LACC TMP
SACL ENCODEROLD ;更新ENCODEROLD
;-----------------------计算绝对电角度-----------------------------------------------------------------------
LT TETA_M
MPYU #KENCODER ;乘磁极对数KENCODER
PAC ;Q12格式
AND #0FFFH ;屏蔽高位
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清零
LACC #SPEEDSTEP ;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 ;更新调节器积分累计量
;-----------------------计算编码增量累计值------------------------------------------------------------------
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)
;-----------------------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
;-----------------------根据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
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 ;更新调节器积分累计量
;-----------------------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 ;更新调节器积分累计量
;-----------------------PARK反变换--------------------------------------------------------------------------------
LACC #0
LT VSDREF ;T=VSDREF
MPY SIN ;P=VSDREF*SIN(TETA_E)
LTA VSQREF ;ACC=P; T=VSQREF
MPY COS ;P=VSQREF*COS(TETA_E)
MPYA SIN ;ACC=P; P=VSQREF*SIN(TETA_E)
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -