📄 foc.asm
字号:
《三相永磁同步伺服电动机磁场定向速度控制程序》
.include "240x.h"
;-----------------------以下定义变量和常数-------------------------------------------------------------
.data
KCURRENT .set 1383H ;电流ipuQ12的转换系数19.5的Q8格式
PWMPRD .set 600 ;周期寄存器值60us/(2*50ns)
VMIN .set 0EC00H ;电压最小极限-1.25 pu,Q12
VMAX .set 1400H ;电压最大极限1.25 pu,Q12
ISMAX .set 1199H ;相电流最大极限4.51A(10%额定值),Q12
ISQREFINIT .set 01000H ;isqref初值4.1A(Q12) (1000H = IBASE)
KENCODER .set 3 ;磁极对数
ENCPULSES .set 4096 ;编码器每转脉冲数(1024*4)
KSPEED .set 0BE7H ;将脉冲数转换成速度系数,11.9的Q8格式
SPEEDSTEP28 .set 28 ;速度采样周期28个中断
.bss T1_PERIODS,1 ;T1周期值的Q5格式
.bss KI,1 ;电流积分系数
.bss KPI,1 ;电流比例系数
.bss KCOR,1 ;电流积分修正系数
.bss KISPEED,1 ;速度积分系数
.bss KPISPEED,1 ;速度比例系数
.bss KCORSPEED,1 ;速度积分修正系数
.bss TMP,1 ;临时变量
.bss IA,1 ;相电流IA
.bss IB,1 ;相电流IB
.bss IC,1 ;相电流IC
.bss SIN,1 ;SIN值Q12
.bss COS,1 ;COS值Q12
.bss DEC_MS,24 ;6个逆阵,Q14格式
.bss CMP_1,1 ;第1基本矢量,Q0格式
.bss CMP_2,1 ;第2基本矢量, Q0格式
.bss CMP_0,1 ;0基本矢量/2, Q0格式
.bss FIRST_TOG,1 ;存放第一次比较匹配的比较器地址
.bss SEC_TOG,1 ;存放第二次比较匹配的比较器地址
.bss TETA_E,1 ;转子电角度[0;1000H],对应[0;360]Q12格式
.bss ISALFA,1 ;ALFA轴电流
.bss ISBETA,1 ;BETA轴电流
.bss VSAL_REF,1 ;ALFA轴参考电压
.bss VSBE_REF,1 ;BETA轴参考电压
.bss ISDREF,1 ;D轴参考电流
.bss ISQREF,1 ;Q轴参考电流
.bss ISD,1 ;D轴电流
.bss ISQ,1 ;Q轴电流
.bss VSDREF,1 ;D轴参考电压
.bss VSQREF,1 ;Q轴参考电压
.bss EPIQ,1 ;Q轴电流调节偏差
.bss EPID,1 ;D轴电流调节偏差
.bss XIQ,1 ;Q轴电流调节器积分累计量
.bss XID,1 ;D轴电流调节器积分累计量
.bss N,1 ;速度
.bss N_REF,1 ;速度参考值
.bss EPISPEED,1 ;速度偏差
.bss XISPEED,1 ;速度调节器积分累计量
.bss P,1 ;SVPWM扇区索引
.bss INITPHASE,1 ;初始化相位标志
.bss TETA_M,1 ;转子机械转角
.bss ISQREFMIN,1 ;Q轴电流最小极限
.bss ISQREFMAX,1 ;Q轴电流最大极限
.bss SECTOR,1 ;SVPWM扇区数
.bss INDEX,1 ;查SIN表索引
.bss UPI,1 ;PI调节器输出
.bss ELPI,1 ;PI调节器极限偏差
.bss ENCODEROLD,1 ;前一个采样周期时编码脉冲数
.bss ENCINCR,1 ;编码脉冲增量
.bss SPEEDTMP,1 ;编码脉冲增量累计值
.bss SPEEDSTEP,1 ;速度采样周期减计数器
.bss WAITTIME,1 ;相位初始化等待循环次数(在该段时间内，转子被调整到一个合适的位置)
;-----------------------以下是程序-----------------------------------------------------------------------------
.text
;-----------------------初始化程序-----------------------------------------------------------------------------
CLRC CNF ; B0
SETC OVM
SPM 0
SETC SXM ;扩展符号
LAR AR0,#DEC_MS ;传送逆阵数据
LAR AR1,#(24-1) ; 24个
LACC #ANGLES_ ; 指向源
LARP AR0
INIT_TBL
TBLR *+,AR1 ;下一个
ADD #1 ;下一个地址
BANZ INIT_TBL,AR0 ;AR1=0结束
LAR AR4,#79H ;用于堆栈保存现场,B2(60H-80H)
LDP #0E0H
SPLK #68H, WDCR ;不用看门狗
SPLK #0284H,SCSR1 ;CLKIN 10MHZ,CLKOUT 20MHZ
LDP #0E1H
SPLK #0900H,ADCTRL1 ;ADC预分频10,1MHZ
SPLK #0001H,MAXCONV ;两个通道
SPLK #0010H,CHSELSEQ1 ;选择ADCIN0和ADCIN1通道
LACC MCRA
OR #0FD8H ;设置PWM1-6,QEP1-2
SACL MCRA
;至此为止，上面的初始化程序和感应电动机的程序是完全一样的
;-----------------------变量初始化-----------------------------------------------------
LDP #0
SPLK #19200,T1_PERIODS ;T1周期值的Q5格式,600*32（Q5格式即右边5位是小数部分，相当于将600（258h）左移动5位，也相当于乘以100000B即2的5次方即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
SACL ISQREFMAX
NEG
SACL ISQREFMIN
LACC #0
SACL ISQREF
SACL ISDREF
SACL N_REF
SACL INDEX
SACL XID
SACL XIQ
SACL XISPEED
SACL UPI
SACL ELPI
SACL INITPHASE
LACC #0A000H
SACL WAITTIME
;-----------------------事件管理器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 ;定时器下溢比较器重载,允许比较 （但CMCONA的12为如果设置为0，表示禁止空间向量PWM，所以是不是应该设置为1啊 也就是说COMCONA应该是9200h)
SPLK #PWMPRD,T1PR ;周期寄存器值600
SPLK #0,T1CNT
SPLK #0840H,T1CON ;连续增减计数方式,预分频为1,允许T1
SPLK #0000H,T2CNT ;编码脉冲计数器
SPLK #0FFFFH,T2PR
SPLK #9870H,T2CON ;定向增减,允许编码接口
;到此为止，事件管理器A初始化程序和感应电机程序是一样的，只是T1PR值不一样
;-----------------------中断初始化--------------------------------------------------------------------
SPLK #0FFFH,EVAIFRA ;清事件管理器A所有中断标志
SPLK #000FH,EVAIFRB
SPLK #000FH,EVAIFRC
SPLK #0200H,EVAIMRA ;开T1下溢中断
SPLK #0000H,EVAIMRB
SPLK #0000H,EVAIMRC
LDP #0H
LACC #0FFH
SACL IFR ;清所有系统中断标志
LACC #0000010B
SACL IMR ;开INT2中断
CLRC INTM ;开总中断
;到此为止,此中断初始化程序和感应电机的程序完全一至
;----------------------初始化结束-------------------------------------------------------------------
;----------------------中断向量定义略-------------------------------------------------------------
;----------------------主程序略----------------------------------------------------------------------
;-----------------------T1下溢中断处理子程序-----------------------------------------------------
_C_INT2
;-----------------------保存现场------------------------------------------------------------------------
MAR *,AR4 ;AR4作为堆栈指针
MAR *-
SST #1,*- ;保存状态寄存器1
SST #0,*- ;保存状态寄存器0
SACH *- ;保存ACC高位
SACL *- ;保存ACC低位
LDP #0E0H
LACC PIVR ;读中断向量
LDP #0E8H
SPLK #0200H,EVAIFRA ;清中断标志位
;到此为止，保存现场程序和感应电机的程序完全一至
;-----------------------电流采样和AD转换------------------------------------------------------
LDP #0E1H
SPLK #2000H,ADCTRL2 ;启动AD转换,IA-ADCIN0,IB-ADCIN1
CONVERSION
BIT ADCTRL2,3 ;将忙状态位复制到TC
BCND CONVERSION,TC ;等待
LACC RESULT0,10 ;转换完毕则将结果寄存器内容载入累加器
LDP #0
SACH IA
LDP #0E1H
LACC RESULT1,10
LDP #0
SACH IB
;到此为止，电流采样和AD转换程序与感应电机的相应程序是一至的
;-----------------------相位初始化-----------------------------------------------------------------
;对于PMSM电机，电机启动时要先进行相位初始化，感应电动机不需要
;转子位置初始化的原理240手册P14-16有详细介绍
LACL INITPHASE ;此时INITPHASE为0
BCND RUN,NEQ ;初始化?已初始过则跳 。第一次执行时还未初始化，继续向下执行，第二次以后执行该中断程序时，已初始化完毕，所以直接跳转。
LACC #0FC00H ;设置TETA = 0FC00H= -90°,Q12
SACL TETA_E
LACC #ISQREFINIT ;Q轴电流等于参考电流
SACL ISQREF
LACC #0 ;对变量和标志清零
SACL ISDREF
SACL TETA_M ;TETA_M为转子机械转角，赋值0
SACL ENCODEROLD ;ENCODEROLD为前一个采样周期的编码脉冲数
SACL N ;转速N为0
SACL SPEEDTMP ;编码脉冲增量累计值
LACC #SPEEDSTEP28 ;SPEEDSTEP=28
SACL SPEEDSTEP
LDP #0E8H
SPLK #0,T2CNT ;编码器计数清零
LDP #0
LACC #WAITTIME ;循环次数A000H,等待(A000H=40960)
SUB #1
SACL WAITTIME
BCND WAIT,NEQ ;初始化相位的等待时间为:40960*60us,在此时间内 转子被初始化一个固定位置。
SPLK #1, INITPHASE ;INITPHASE=1,相位初始化完成
;-----------------------读编码器脉冲数,计算转角增量和转子绝对位置----------------------------------
RUN
LDP #0E8H
LACC T2CNT ;读编码脉冲
;NEG ;编码器反接线时
LDP #0
SACL TMP
SUB ENCODEROLD ;减去前一个周期测的脉冲数
SACL ENCINCR ;得到编码增量
ADD TETA_M ;得到转子绝对位置
BCND ENCMAGZERO,GEQ ;调整TETA_M的范围[0;ENCPULSES-1]
ADD #ENCPULSES ;如果TETA_M是负值调整为正
ENCMAGZERO
SACL TETA_M ;转子的绝对位置TETA_M初值为0，通过累计编码脉冲增量可知其绝对位置(相对其初始位置)
SUB #ENCPULSES ;检测TETA_M是否大于360° (绝对位置TETA_M的最大值，是4096也即编码器一周的脉冲)
BCND ENCMINMAX,LT ;不大于则跳转
SACL TETA_M ;大于则保存差值
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -