永磁同步程序.docx
《永磁同步程序.docx》由会员分享，可在线阅读，更多相关《永磁同步程序.docx（23页珍藏版）》请在冰豆网上搜索。
永磁同步程序
《三相永磁同步伺服电动机磁场定向速度控制程序》
.INCLUDE"240X.H"
；-----------------------以下定义变量和常数-------------------------------------------------------------
.DATA
KCURRENT.SET1383H;电流ipuQ12的转换系数19.5的Q8格式
PWMPRD.SET600;周期寄存器值60us/（2*50ns）
VMIN.SET0EC00H;电压最小极限-1.25pu,Q12
VMAX.SET1400H;电压最大极限1.25pu,Q12
ISMAX.SET1199H;相电流最大极限4.51A（10%额定值）,Q12
ISQREFINIT.SET01000H;isqref初值4.1A（Q12）（1000H=IBASE）
KENCODER.SET3;磁极对数
ENCPULSES.SET4096;编码器每转脉冲数（1024*4）
KSPEED.SET0BE7H;将脉冲数转换成速度系数,11.9的Q8格式
SPEEDSTEP.SET28;速度采样周期28个中断
.BSST1_PERIODS,1;T1周期值的Q5格式
.BSSKI,1;电流积分系数
.BSSKPI,1;电流比例系数
.BSSKCOR,1;电流积分修正系数
.BSSKISPEED,1;速度积分系数
.BSSKPISPEED,1;速度比例系数
.BSSKCORSPEED,1;速度积分修正系数
.BSSTMP,1;临时变量
.BSSIA,1;相电流IA
.BSSIB,1;相电流IB
.BSSIC,1;相电流IC
.BSSSIN,1;SIN值Q12
.BSSCOS,1;COS值Q12
.BSSDEC_MS,24;6个逆阵,Q14格式
.BSSCMP_1,1;第1基本矢量,Q0格式
.BSSCMP_2,1;第2基本矢量,Q0格式
.BSSCMP_0,1;0基本矢量/2,Q0格式
.BSSFIRST_TOG,1;存放第一次比较匹配的比较器地址
.BSSSEC_TOG,1;存放第二次比较匹配的比较器地址
.BSSTETA_E,1;转子电角度[0;1000H],对应[0;360]Q12格式
.BSSISALFA,1;ALFA轴电流
.BSSISBETA,1;BETA轴电流
.BSSVSAL_REF,1;ALFA轴参考电压
.BSSVSBE_REF,1;BETA轴参考电压
.BSSISDREF,1;D轴参考电流
.BSSISQREF,1;Q轴参考电流
.BSSISD,1;D轴电流
.BSSISQ,1;Q轴电流
.BSSVSDREF,1;D轴参考电压
.BSSVSQREF,1;Q轴参考电压
.BSSEPIQ,1;Q轴电流调节偏差
.BSSEPID,1;D轴电流调节偏差
.BSSXIQ,1;Q轴电流调节器积分累计量
.BSSXID,1;D轴电流调节器积分累计量
.BSSN,1;速度
.BSSN_REF,1;速度参考值
.BSSEPISPEED,1;速度偏差
.BSSXISPEED,1;速度调节器积分累计量
.BSSP,1;SVPWM扇区索引
.BSSINITPHASE,1;初始化相位标志
.BSSTETA_M,1;转子机械转角
.BSSISQREFMIN,1;Q轴电流最小极限
.BSSISQREFMAX,1;Q轴电流最大极限
.BSSSECTOR,1;SVPWM扇区数
.BSSINDEX,1;查SIN表索引
.BSSUPI,1;PI调节器输出
.BSSELPI,1;PI调节器极限偏差
.BSSENCODEROLD,1;前一个采样周期时编码脉冲数
.BSSENCINCR,1;编码脉冲增量
.BSSSPEEDTMP,1;编码脉冲增量累计值
.BSSSPEEDSTEP,1;速度采样周期减计数器
.BSSWAITTIME,1;相位初始化等待循环次数
;-----------------------以下是程序-----------------------------------------------------------------------------
.TEXT
;-----------------------初始化程序-----------------------------------------------------------------------------
CLRCCNF;B0
SETCOVM
SPM0
SETCSXM;扩展符号
LARAR0,#DEC_MS;传送逆阵数据
LARAR1,#（24-1）;24个
LACC#ANGLES_;指向源
LARPAR0
INIT_TBL
TBLR*+,AR1;下一个
ADD#1;下一个地址
BANZINIT_TBL,AR0;AR1=0结束
LARAR4,#79H;用于堆栈保存现场,B2（60H-80H）
LDP#0E0H
SPLK#68H,WDCR;不用看门狗
SPLK#0284H,SCSR1;CLKIN10MHZ,CLKOUT20MHZ
LDP#0E1H
SPLK#0900H,ADCTRL1;ADC预分频10,1MHZ
SPLK#0001H,MAXCONV;两个通道
SPLK#0010H,CHSELSEQ1;选择ADCIN0和ADCIN1通道
LACCMCRA
OR#0FD8H;设置PWM1-6,QEP1-2
SACLMCRA
;-----------------------变量初始化-----------------------------------------------------
LDP#0
SPLK#19200,T1_PERIODS;T1周期值的Q5格式,600*32
SPLK#7AH,KI;积分系数0.03的Q12格式
SPLK#999H,KPI;比例系数0.60的Q12格式
SPLK#0CCH,KCOR;KCOR=KI/KPI=0.05,Q12格式
SPLK#7AH,KISPEED;速度积分系数0.03的Q12格式
SPLK#6800H,KPISPEED;速度比例系数6.5的Q12格式
SPLK#12H,KCORSPEED;KISPEED/KPISPEED=0.0046,Q12
LACC#ISMAX
SACLISQREFMAX
NEG
SACLISQREFMIN
LACC#0
SACLISQREF
SACLISDREF
SACLN_REF
SACLINDEX
SACLXID
SACLXIQ
SACLXISPEED
SACLUPI
SACLELPI
SACLINITPHASE
LACC#0A000H
SACLWAITTIME
;-----------------------事件管理器A初始化-------------------------------------------------------------------
LDP#0E8H
SPLK#0666H,ACTRA;引脚PWM1,3,5高有效,2,4,6低有效
SPLK#300,CMPR1;占空比初值为0
SPLK#300,CMPR2
SPLK#300,CMPR3
SPLK#01F4H,DBTCONA;死区时间1.6us
SPLK#8200H,COMCONA;定时器下溢比较器重载,允许比较
SPLK#PWMPRD,T1PR;周期寄存器值600
SPLK#0,T1CNT
SPLK#0840H,T1CON;连续增减计数方式,预分频为1,允许T1
SPLK#0000H,T2CNT;编码脉冲计数器
SPLK#0FFFFH,T2PR
SPLK#9870H,T2CON;定向增减,允许编码接口
;-----------------------中断初始化--------------------------------------------------------------------
SPLK#0FFFH,EVAIFRA;清事件管理器A所有中断标志
SPLK#000FH,EVAIFRB
SPLK#000FH,EVAIFRC
SPLK#0200H,EVAIMRA;开T1下溢中断
SPLK#0000H,EVAIMRB
SPLK#0000H,EVAIMRC
LDP#0H
LACC#0FFH
SACLIFR;清所有系统中断标志
LACC#0000010B
SACLIMR;开INT2中断
CLRCINTM;开总中断
;----------------------初始化结束-------------------------------------------------------------------
;-----------------------T1下溢中断处理子程序-----------------------------------------------------
_C_INT2
;-----------------------保存现场------------------------------------------------------------------------
MAR*,AR4;AR4作为堆栈指针
MAR*-
SST#1,*-;保存状态寄存器1
SST#0,*-;保存状态寄存器0
SACH*-;保存ACC高位
SACL*-;保存ACC低位
LDP#0E0H
LACCPIVR;读中断向量
LDP#0E8H
SPLK#0200H,EVAIFRA;清中断标志位
;-----------------------电流采样和AD转换------------------------------------------------------
LDP#0E1H
SPLK#2000H,ADCTRL2;启动AD转换,IA-ADCIN0,IB-ADCIN1
CONVERSION
BITADCTRL2,3;将忙状态位复制到TC
BCNDCONVERSION,TC;等待
LACCRESULT0,10
LDP#0
SACHIA
LDP#0E1H
LACCRESULT1,10
LDP#0
SACHIB
;-----------------------相位初始化-----------------------------------------------------------------
LACLINITPHASE
BCNDRUN,NEQ;初始化?
已初始过则跳转
LACC#0FC00H;设置TETA=0FC00H=-90°,Q12
SACLTETA_E
LACC#ISQREFINIT;Q轴电流等于参考电流
SACLISQREF
LACC#0;对变量和标志清零
SACLISDREF
SACLTETA_M
SACLENCODEROLD
SACLN
SACLSPEEDTMP
LACC#SPEEDSTEP;SPEEDSTEP=28
SACLSPEEDSTEP
LDP#0E8H
SPLK#0,T2CNT;编码器计数清零
LDP#0
LACC#WAITTIME;循环次数A000H,等待
SUB#1
SACLWAITTIME
BCNDWAIT,NEQ
SPLK#1,INITPHASE;INITPHASE=1,相位初始化完成
;-----------------------读编码器脉冲数,计算转角增量和转子绝对位置----------------------------------
RUN
LDP#0E8H
LACCT2CNT;读编码脉冲
;NEG;编码器反接线时
LDP#0
SACLTMP
SUBENCODEROLD;减去前一个周期测的脉冲数
SACLENCINCR;得到编码增量
ADDTETA_M;得到转子绝对位置
BCNDENCMAGZERO,GEQ;调整TETA_M的范围[0;ENCPULSES-1]
ADD#ENCPULSES;如果TETA_M是负值调整为正
ENCMAGZERO
SACLTETA_M
SUB#ENCPULSES;检测TETA_M是否大于360°
BCNDENCMINMAX,LT;不大于则跳转
SACLTETA_M;大于则保存差值
ENCMINMAX
LACCTMP
SACLENCODEROLD;更新ENCODEROLD
;-----------------------计算绝对电角度-----------------------------------------------------------------------
LTTETA_M
MPYU#KENCODER;乘磁极对数KENCODER
PAC;Q12格式
AND#0FFFH;屏蔽高位
SACLTETA_E;存放
;-----------------------计算转速---------------------------------------------------------------------------
LACCSPEEDSTEP;检测是否该采样速度
SUB#1
SACLSPEEDSTEP
BCNDNOCALC,GT;没到采样时刻,退出
LTSPEEDTMP;到采样时刻,计算编码器增量累计值
MPY#KSPEED;Q8格式
PAC
SFL
SACHN,7;相当于右移8位
LACC#0
SACLSPEEDTMP;SPEEDTMP清零
LACC#SPEEDSTEP;SPEEDSTEP=28
SACLSPEEDSTEP;SPEEDSTEP重新赋初值
;-----------------------转速PI调节,输出IQREF----------------------------------------------------------
LACCN_REF;转速给定值,外部输入
SUBN
SACLEPISPEED;转速偏差
LACCXISPEED,12;转速调节器积分累计量
LTEPISPEED
MPYKPISPEED;乘比例系数6800H（6.5的Q12格式）
APAC;累加
SACHUPI,4;相当于右移12位
BITUPI,0;检测调节器输出的正负
BCNDUPIMAGZEROS,NTC;如果正,跳转
LACCISQREFMIN;否则是负,检测是否超过电流下限
SUBUPI
BCNDNEG_SAT,GT;超过下限进入饱和区则跳转
LACCUPI;否则正常调整
BLIMITERS
NEG_SAT
LACCISQREFMIN;ACC=下限值
BLIMITERS
UPIMAGZEROS
LACCISQREFMAX;检测是否超过电流上限
SUBUPI
BCNDPOS_SAT,LT;超过上限进入饱和区则跳转
LACCUPI;否则正常调整
BLIMITERS
POS_SAT
LACCISQREFMAX;ACC=上限值
LIMITERS
SACLISQREF;输出IQREF
SUBUPI
SACLELPI;求极限偏差
LTELPI
MPYKCORSPEED;KCORSPEED=KISPEED/KPISPEED
PAC
LTEPISPEED
MPYKISPEED;积分系数
APAC
ADDXISPEED,12
SACHXISPEED,4;更新调节器积分累计量
;-----------------------计算编码增量累计值------------------------------------------------------------------
NOCALC
LACCSPEEDTMP
ADDENCINCR
SACLSPEEDTMP
;-----------------------将IA,IB,IC转换成IPUQ12格式---------------------------------------------------------
WAIT
LDP#0
LACCIA
AND#3FFH;屏蔽高位
SUB#512;向下平移,产生正负电流值
SACLTMP
LTTMP
MPY#KCURRENT;转换系数,Q8格式
PAC
SFL
SACHIA,7;IA,Q12格式
LACCIB
AND#3FFH
SUB#512
SACLTMP
LTTMP
MPY#KCURRENT
PAC
SFL
SACHIB,7
LACCIB
ADDIA
NEG
SACLIC;IC=-（IB+IA）
;-----------------------CLARKE变换--------------------------------------------------------------------------------
LDP#0
LTIA
MPY#5018;乘
=5018,Q12格式
PAC
SACHISALFA,4;保存ISALFA
LACCIB,1;ACC=2*IB
ADDIA;ACC=IA+2*IB
SACLTMP;暂存
LTISALFA
MPY#2896;乘
=2896,Q12格式
PAC
SACHISBETA,4;保存ISBETA
;-----------------------根据TETA_E查SIN,COS表------------------------------------------------------------
LACCTETA_E;TETA_E范围[0;1000H],[0;360]的Q12格式
RPT#3;右移4位,范围变为[0;255],
SFR
AND#0FFH;屏蔽高位
SACLINDEX;生成查表指针
ADD#SINTAB;加上表的首地址
TBLRSIN;保存SIN值
LACLINDEX;COS（TETA）=SIN（TETA+90°）
ADD#040H;90°=40H
AND#0FFH
ADD#SINTAB
TBLRCOS;保存COS值
;-----------------------PARK变换------------------------------------------------------------------------------------
LACC#0;累加器清零
LTISBETA
MPYSIN;Q12格式
LTAISALFA;ACC=ISBETA*SIN（TETA）,T=ISALFA
MPYCOS;Q12格式
MPYASIN;ACC=ISBETA*SIN（TETA）+ISALFA*COS（TETA），
;P=ISALFA*SIN（TETA）
SACHISD,4;保存ISD
LACC#0;累加器清零
LTISBETA
MPYSCOS;ACC=-ISALFA*SIN（TETA）,P=ISBETA*COS（TETA）
APAC;ACC=-ISALFA*SIN（TETA）+ISBETA*COS（TETA）
SACHISQ,4;保存ISQ
;-----------------------Q轴电流PI调节,输出VSQREF------------------------------------------------------------
LACCISQREF
SUBISQ
SACLEPIQ;Q轴电流偏差
LACCXIQ,12;电流调节器积分累计量
LTEPIQ
MPYKPI;比例系数
APAC
SACHUPI,4
BITUPI,0;检测调节器输出的正负
BCNDUPIMAGZEROQ,NTC;如果正,跳转
LACC#VMIN;否则是负,检测是否超过电压下限
SUBUPI
BCNDNEG_SATQ,GT;超过下限进入饱和区则跳转
LACCUPI;否则正常调整
BLIMITERQ
NEG_SATQ
LACC#VMIN;ACC=下限值
BLIMITERQ
UPIMAGZEROQ
LACC#VMAX;检测是否超过电压上限
SUBUPI
BCNDPOS_SATQ,LT;超过上限进入饱和区则跳转
LACCUPI;否则正常调整
BLIMITERQ
POS_SATQ
LACC#VMAX;ACC=上限值
LIMITERQ
SACLVSQREF;输出VSQREF
SUBUPI
SACLELPI;求极限偏差
LTELPI
MPYKCOR;KCOR=KI/KPI
PAC
LTEPIQ
MPYKI;积分系数
APAC
ADDXIQ,12
SACHXIQ,4;更新调节器积分累计量
;-----------------------D轴电流PI调节,输出VSDREF-------------------------------------------------------
LACCISDREF
SUBISD
SACLEPID;D轴电流偏差
LACCXID,12;电流调节器积分累计量
LTEPID
M
升级会员 