📄 foc.asm
字号:
;----------------------矢量控制--------------
.include "F2407REGS.h" ; 寄存器地址
.include "vectors.h"
.sect "table" ;07.10.17 改
sqrttab .include sqrt1.tab ;07.10.17 改
.global _c_int0 ;全局化标号
.global _C_INT2
.global _PDP_ISR
;-----------------------以下定义变量和常数-------------------------------------------------------------
PWMCNT_LED_ .set 01000H
PWM_T .set 4000 ; 5k 4000
VMIN_ .set 0F475H
VMAX_ .set 2955 ;1.25pu
KIN_ .set 2500
KPN_ .set 9000
KCN_ .set 1138
KI_ .set 300
KP_ .set 2000
KC_ .set 614
K_N_REF .set 2796 ;q8
KCURRENT_ .set 3072 ;q8
MAXCURRENT .set 6000 ; 10A
IMAX_ .set 3176 ;1.1pu 5A
SIZE .set 1000
BUF_START .set 8500H
BUF_START1 .set 8900H
BUF_START2 .set 8D00H
A1_ .set 5017
A2_ .set 2896
A3_ .set 7095
IMREF_ .set 1450;1250 ;2.5A 根号1.5倍 1.22 4.33A
;N_REF_ .set 1000 ;4096
KSPEED_ .set 5120 ;将脉冲数转换成速度系数Q20格式1/204.8
;速度基值1500rpm,速度采样周期2ms
SPEEDSTEP20 .set 10 ;速度采样周期10个中断 2ms
ksqrt .set 500
.bss T1_PERIODS,1 ;T1周期值的Q5格式
.bss KCURRENT,1 ;电流ipuQ12的转换系数,Q8格式
.bss KI,1 ;电流积分系数,Q12格式
.bss KP,1 ;电流比例系数,Q12格式
.bss KC,1 ;电流积分修正系数,Q12格式
.bss KIN,1 ;速度积分系数,Q12格式
.bss KPN,1 ;速度比例系数,Q12格式
.bss KCN,1 ;速度积分修正系数,Q12
.bss VMIN,1 ;电压最小极限-1.25 pu,Q12
.bss VMAX,1 ;电压最大极限1.25 pu,Q12
.bss IMAX,1 ;相电流最大极限,Q12
.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 IALFA,1 ;ALFA轴电流
.bss IBETA,1 ;BETA轴电流
.bss VALF_REF,1 ;ALFA轴参考电压
.bss VBET_REF,1 ;BETA轴参考电压
.bss IMREF,1 ;M轴参考电流
.bss ITREF,1 ;T轴参考电流
.bss IM,1 ;M轴电流
.bss IT,1 ;T轴电流
.bss VMREF,1 ;M轴参考电压
.bss VTREF,1 ;T轴参考电压
.bss EPIT,1 ;T轴电流调节偏差
.bss EPIM,1 ;M轴电流调节偏差
.bss XIT,1 ;T轴电流调节器积分累计量
.bss XIM,1 ;M轴电流调节器积分累计量
.bss N,1 ;速度
.bss N_REF,1 ;速度参考值
.bss EPISPEED,1 ;速度偏差
.bss XISPEED,1 ;速度调节器积分累计量
.bss P,1 ;SVPWM扇区索引
.bss ITREFMIN,1 ;T轴电流最小极限
.bss ITREFMAX,1 ;T轴电流最大极限
.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 KR,1 ;常数
.bss KT,1 ;常数
.bss K,1 ;转换常数
.bss IDK,1 ;转子励磁电流,Q12
.bss FS,1 ;转子磁链角频率与额定角频率之比
.bss TETAINCR,1 ;TETA转角增量
.bss TMP1,1 ;临时变量
.bss ACCH,1 ;ACC高字保存单元
.bss ACCL,1 ;ACC低字保存单元
.bss AR0_SAVE,1 ;AR0保存单元
.bss P_HI,1 ;P寄存器高字保存单元
.bss P_LO,1 ;P寄存器低字保存单元
.bss T_SAVE,1
.bss PWMCNT_LED,1
.bss CTRL ,1
.bss TEMPER ,1
.bss UDC ,1
.bss KEYDATA,1
.bss A1 ,1
.bss A2,1
.bss A3,1
.bss KEYDATA1 ,1
.bss TETA_E1 ,1
.bss KSPEED ,1
.bss PWMCNT_N_REF ,1
.bss Speed_set ,1
.bss N_REF_STEP ,1
.bss N_REF_former ,1
.bss I0,1 ;07.10.17改
.bss I1,1 ;07.10.17改
;-------------------------------------- CONTEXT段,定义保护现场数据区--------------------------
ST0_SAVE .usect ".context",1 ;状态寄存器ST0保存单元
ST1_SAVE .usect ".context",1 ;状态寄存器ST1保存单元
;-----------------------以下是程序-----------------------------------------------------------------------------
.text
;--------------------------------------系统初始化程序----------------------------------------------------
_c_int0
SETC INTM ; Disable interrupt
SETC SXM ; Supress sign extension
SETC OVM ; Results overflow normally in Accumulator
CLRC CNF ; B0 is configured as data space
CLRC C ; clear carry bit
LDP #224
SPLK #0000001010000100B,SCSR1 ;CLKIN 20M,CLKOUT 40M
SPLK #68H,WDCR ;不用看门狗
;--------------------------------------I/O 初始化程序----------------------------------------------------
LDP #225
LACC MCRA
OR #0FC0H ;设置PWM1-6引脚基本功能
SACL MCRA
LACC MCRC
AND #0FDFFH
SACL MCRC
LACC PFDATDIR
AND #0FDFDH
SACL PFDATDIR
;--------------------------------------中断初始化程序----------------------------------------------------
LDP #0
SPLK #0FFH,IFR ; 清所有系统中断标志
SPLK #00000011B,IMR ; 开INT2中断
LDP #232
SPLK #0FFFH,EVAIFRA ; 清事件管理器A所有中断标志
SPLK #0FH,EVAIFRB
SPLK #0FH,EVAIFRC
SPLK #0201H,EVAIMRA ; 开T1下溢中断
SPLK #0,EVAIMRB ; 屏蔽所有中断
SPLK #0,EVAIMRC ; 屏蔽所有中断
;--------------------------------------初始化事件管理器A程序-----------------------------------------
LDP #0E8H
SPLK #PWM_T ,T1PR ; T1周期值=200 us/25ns/2=4000
SPLK #PWM_T ,CMPR1 ; 占空比初值 0%
SPLK #PWM_T ,CMPR2
SPLK #PWM_T ,CMPR3
SPLK #0000100110011001B,ACTRA ; 引脚PWM1,3,5低有效,2,4,6高有效
;SPLK #0000011001100110B,ACTRA ; 引脚PWM1,3,5高有效,2,4,6低有效
SPLK #06F4H,DBTCONA ; 死区时间6*32*25ns=4.8us
SPLK #1000001000000000B,COMCONA ;允许比较,不用硬件空间矢量
SPLK #0,T1CNT
SPLK #1000100000000010B,T1CON ;连续增减计数方式,预分频=1
; splk #1110000001010101b,GPTCONA
;测速初始化
splk #0110001011110000b,CAPCONA
SPLK #0,T2CNT ;编码脉冲计数器
SPLK #0FFFFH,T2PR
splk #1001100101110000b,T2CON ;timer2 for QEP
;splk #1110001011110000b,CAPCONA
;SPLK #9870H,T2CON ;定向增减,允许编码接口
;--------------------------------------初始化AD采样程序-----------------------------------------
LDP #225
SPLK #4000H,ADCTRL1
NOP
SPLK #3000H,ADCTRL1
SPLK #4000H,ADCTRL2
SPLK #4,MAXCONV ;/*For SEQ mode (i.e, cascaded mode), MAX CONV1_3-0 are used.*/
SPLK #05431H,CHSELSEQ1
SPLK #0007H,CHSELSEQ2 ;Result0 - ADC模块的ADCIN1 - Udc
;Result1 - ADC模块的ADCIN3 - CT-U(Iw)
;Result2 - ADC模块的ADCIN4 - CT-V(Iu)
;Result3 - ADC模块的ADCIN5 - Regulator
;-----------------------变量初始化-----------------------------------------------------
LDP #6
SPLK #4000,T1_PERIODS ;T1周期值的Q0格式,
SPLK #KI_,KI ;给电流积分系数赋值,Q12
SPLK #KP_,KP ;给电流比例系数赋值,Q12
SPLK #KC_,KC ;给电流积分修正系数赋值,Q12
SPLK #KIN_,KIN ;给速度积分系数赋值,Q12
SPLK #KPN_,KPN ;给速度比例系数赋值,Q12
SPLK #KCN_,KCN ;给速度积分修正系数赋值,Q12
SPLK #VMIN_,VMIN ;给电压最小极限赋值-1.25 pu,Q12
SPLK #VMAX_, VMAX ;给电压最大极限赋值1.25 pu,Q12
SPLK #IMAX_,IMAX ;给相电流最大极限赋值,Q12
SPLK #KCURRENT_ ,KCURRENT ;电流ipuQ12的转换系数的Q8格式
SPLK #KSPEED_ , KSPEED
SPLK #58,KR ;(Lr=0.3702,Rr=3.271,T=0.0002);(Lr=0.2677,Rr=2.7472,T=0.0002)Q15格式 67
SPLK #115,KT ; (Lr=0.3702,Rr=3.271) ; (Lr=0.2677,Rr=2.7472) Q12格式 134
SPLK #656,K ;转换系数,采样周期200微秒,Q0
SPLK #0,IDK
SPLK #0,FS
SPLK #0,TETAINCR
SPLK #0,TMP
SPLK #0,TMP1
SPLK #500, ksqrt ;07.10.17改
LACC #1000
SACL ITREFMAX
NEG
SACL ITREFMIN
LACC #0
SACL ITREF
SACL IMREF
SACL N_REF
SACL INDEX
SACL XIM
SACL XIT
SACL XISPEED
SACL UPI
SACL ELPI
SACL ENCODEROLD
SACL N
SACL SPEEDTMP
LACC #SPEEDSTEP20 ;SPEEDSTEP=20
SACL SPEEDSTEP
SPLK #0H ,PWMCNT_LED
splk #0, TETA_E
splk #KCURRENT_, KCURRENT
splk #A1_ ,A1
splk #A2_ ,A2
splk #A3_ ,A3
splk #IMREF_, IMREF ; 4.33A
; splk #N_REF_, N_REF
LAR AR0,#DEC_MS ;传送逆阵数据
LAR AR1,#(24-1) ; 24个
LACC #ANGLES_ ; 指向源
LARP AR0
INIT_TBL
TBLR *+,AR1 ;下一个
ADD #1 ;下一个地址
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -