📄 1240.asm
字号:
SST #1,ST1_SAVE ;保存ST1
;LACC #0 ;TIANJIAN
SACH ACCH ;保存ACC
SACL ACCL
SPH P_HI ;保存P
SPL P_LO
MPY #1 ;P<=T
SPL T_SAVE ;保存T
SAR AR0,AR0_SAVE ;保存AR0
LDP #224
LACL PIVR ;读偏移地址
XOR #29H ;T1下溢中断
BCND T1UF_ISR,EQ ;是下溢中断跳T1UF_ISR
B GISR_RET
GISR_RET
LDP #4 ;否则恢复现场
LAR AR0,AR0_SAVE ;恢复AR0
LT P_LO ;恢复P
MPY #1
LPH P_HI
LT T_SAVE ;恢复T
LACC ACCH,16
ADDS ACCL ;恢复ACC
LDP #0 ;指向B2
LST #1,ST1_SAVE ;恢复ST1
LST #0,ST0_SAVE ;恢复ST0
LDP #0
SPLK #0000000000000010B,IMR
CLRC INTM ;开中断
RET ;返回
T1UF_ISR LDP #232
SPLK #0200H,EVAIFRA
LDP #0
SPLK #0002h,IFR
LDP #6 ;计算转角增量
LT OMEGA ;调制波角频率送到TREG ;Q5
MPY T_SAMPLE ;(TREG)*[(T_SAMPLE)采样周期,Q24格式]送PREG ;Q5*Q24
PAC
SFR ;SXM=0所以右移一位成Q12格式
ADD THETA_H,16 ;;（THETA_H参考电压相位角高字,Q12格式） 左移16位高位填0与ACC相加结果存ACC
ADDS THETA_L ;计算绝对位置;参考电压相位角低字,Q12格式
SACH THETA_H ;保存
SACL THETA_L
BCND CHK_UPLIM,GEQ ;如果大于0检查上限
ADD THETA_360,16 ;否则与2π比较，Q12
SACH THETA_H ;Q12
B RND_THETA
CHK_UPLIM
SUB THETA_360,16 ;2π比较，Q12
BCND REST_THETA,LEQ ;如果在此范围内恢复THETA_H
SACH THETA_H
B RND_THETA
REST_THETA
ADD THETA_360,16
RND_THETA
ADD #1,15 ;圆整并保留高字
SACH THETA_R ;保存
LACC #1 ;假设THETA-H在第一象限
SACL SS ;SIN符号=1
SACL SC ;COS符号=1
LACC THETA_R
SACL THETA_M ;存入THETA—M
SUB THETA_90
BCND E_Q,LEQ ;在第一象限则跳转
SPLK #-1,SC ;COS符号=-1
LACC THETA_180
SUB THETA_R ;180-θ
SACL THETA_M ;存入THETA_M
BCND E_Q,GEQ ;在第二象限则跳转
SPLK #-1,SS ;SIN符号=-1
LACC THETA_R
SUB THETA_180 ;THETA-180
SACL THETA_M ;存入THETA_M
LACC THETA_270
SUB THETA_R
BCND E_Q,GEQ ;在第3象限则跳转
SPLK #1,SC ;COS符号=1
LACC THETA_360
SUB THETA_R
SACL THETA_M ;在第4象限
E_Q
LT THETA_M ;Q12，计算查表索引
MPY THETA_L ;Q12*Q9
PAC
SACH SIN_INDX ;Q5
LACC SIN_INDX,11 ;左移11位
SACH SIN_INDX ;相当于右移5位变成Q0格式，即整数
LACC SIN_ENTRY ;查SIN表
ADD SIN_INDX
TBLR SIN_THETA
LACC SIN_END
SUB SIN_INDX
TBLR COS_THETA
LT SS ;查COS表
MPY SIN_THETA ;修改符号，Q14
PAC
SACL SIN_THETA ;右移16位保存，Q14
LT SC
MPY COS_THETA ;修改符号，Q14
PAC
SACL COS_THETA ;右移16位保存，Q14
LT SET_V ;开始计算UA，UB
MPY SIN_THETA ;Q14*Q14
PAC
SACH UA,1 ;UA，Q13
MPY SIN_THETA ;Q14*Q14
PAC
SACH UB,1 ;UB,Q13格式
LT THETA_R ;Q12格式，确定扇区
MPY THETA_S ;Q12*Q15
PAC
SACH SECTOR
LACC SECTOR,5
SACH SECTOR ;相当于右移5位变成Q0格式，即整数
LACC #DEC_MS ;逆阵数据首地址
ADD SECTOR,2
SACL TEMP ;产生地址指针
LAR AR0,TEMP ;产生逆阵表
LT UA ;Q13格式，计算UA*M（1，1）+UB*M（1，2）
MPY *+ ;M(1,1)UA,Q13*Q14
PAC ;Q11
LT UB ;Q11
MPY *+ ;M(1,2)UB,Q13*Q14
APAC ;0.5*C1,Q11
BCND CMP1BIG0,GEQ ;IF>=0,THEN GOON
LACC #0 ;否则为0
CMP1BIG0
SACH TEMP ;0.5*C1,Q11
LT TEMP ;Q11
MPY T1_PERIODS ;Q11*Q5
PAC ;Q0
SACH CMP_1 ;0.5*C1*TP,Q0
LACC CMP_1,15
SACH CMP_1
LT UA ;计算UA*M(2,1)+UB*M(2,2)
MPY *+ ;M(2,1)UA,Q13*Q14
PAC ;Q11
LT UB ;Q13
MPY *+ ;M(2,2)UB,Q13*Q14
APAC ;0.5*C2,Q11+Q11
BCND CMP2BIG0,GEQ ;IF>=0,THEN GOON
LACC #0 ;否则为0
CMP2BIG0
SACH TEMP ;0.5*C2,Q11
LT TEMP ;Q11
MPY T1_PERIODS ;Q11*Q5
PAC ;Q0
SACH CMP_2 ;0.5*C2*TP,Q0
LACC CMP_2,15
SACH CMP_2
LACC #500 ;T1周期值
SUB CMP_1
SUB CMP_2 ;Q0
BCND CMP0BIG0,GEQ ;IF>=0,THEN GOON
LACC #0 ;否则为0
CMP0BIG0
SACL CMP_0
LACC CMP_0,15 ;相当右移一位，除2
SACH CMP_0 ;0.25*C0*TO
LACC #FIRST_ ;指向第一次比较匹配的比较寄存器地址表入口地址
ADD SECTOR
TBLR FIRST_TOG ;查到第一次比较匹配的比较寄存器地址
LAR AR0,FIRST_TOG ;只向该地址
LACC CMP_0
SACL * ;CMP_0送入该比较器
LACC #SECOND_ ;指向第二次比较匹配比较寄存器地址表入口地址
ADD SECTOR
TBLR SEC_TOG ;查到第一次比较匹配的比较寄存器地址
LAR AR0,SEC_TOG ;只向该地址
LACC CMP_0
ADD CMP_1 ;CMP_0+CMP_1
SACL * ;送入该比较器
;---------------------------T1下溢中断处理程序------------------------------------------------------------
; LACC #CMPR3
; SUB FIRST_TOG
; ADD #CMPR2
; SACL TEMP ;计算第一次比较匹配的比较寄存器地址
; LAR AR0,TEMP ;只向该地址
; LACC CMP_0
; ADD CMP_1
; ADD CMP_2 ;CMP_0+CMP_1+CMP@
; SACL * ;送入该比较器
; B GISR_RET ;返回
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
LACC #THI_ ;指向第二次比较匹配比较寄存器地址表入口地址
ADD SECTOR
TBLR THI_TOG ;查到第一次比较匹配的比较寄存器地址
LAR AR0,THI_TOG ;只向该地址
LACC T1PR
SUB CMP_0 ;CMP_0+CMP_1
SUB CMP_1
SACL *
LDP #232
SPLK #0FFFFh,EVAIFRA ;清事件管理器中断标记
LDP #0
SPLK #0FFFFH,IFR ;清一级中断标记
CLRC INTM ;复位看门狗
B GISR_RET
.DATA
ANGLES_ .WORD 0H ;3.14159/2,Q12格式
.WORD 03244H ;3.14159,Q12
.WORD 04B66H ;3*3.14159/2,Q12
.WORD 06488H ;2*3.14159,Q12
.WORD 20066 ;矩阵A的逆阵数据，每个逆阵有4个数据，Q14
.WORD -11585 ;按参考电压所在的扇区索引
.WORD 0
.WORD 23170
.WORD -20066
.WORD 11585
.WORD 20066
.WORD 11585
.WORD 0
.WORD 23170
.WORD -20066
.WORD -11585
.WORD 0
.WORD -23170
.WORD -20066
.WORD 11585
.WORD -20066
.WORD -11585
.WORD 20066
.WORD -11585
.WORD 20066
.WORD 11585
.WORD 0
.WORD -23170
FIRST_ .WORD CMPR1 ;第一次比较匹配的比较寄存器地址
.WORD CMPR2 ;按参考电压所在的扇区索引
.WORD CMPR2
.WORD CMPR3
.WORD CMPR3
.WORD CMPR1
SECOND_
.WORD CMPR2
.WORD CMPR1
.WORD CMPR3
.WORD CMPR2
.WORD CMPR1
.WORD CMPR3
THI_
.WORD CMPR3
.WORD CMPR3
.WORD CMPR1
.WORD CMPR1
.WORD CMPR2
.WORD CMPR2
SIN_ENTRY_
.WORD 0
.WORD 286,572,857,1143,1428 ;0--90度SIN值表，Q14
;.WORD 0,0,0,0,0
.WORD 1713,1997,2280,2563,2845
.WORD 3126,3406,3686,3964,4240
.WORD 4516,1790,5063,5334,5604
.WORD 5872,6138,6402,6664,6924
.WORD 7182,7438,7692,7943,8192
.WORD 8438,8682,8923,9162,9397
.WORD 9630,9860,10087,10311,10531
.WORD 10749,10963,11174,11381,11585
.WORD 11786,11982,12176,12365,12551
.WORD 12733,12911,13085,13255,13421
.WORD 13583,13741,13894,14044,14189
.WORD 14330,14466,14598,14726,14849
.WORD 14968,15082,15191,15296,15396
.WORD 15491,15582,15668,15749,15826
.WORD 15897,15964,16026,16083,16135
.WORD 16182,16225,16262,16294,16322
.WORD 16344,16362,16374,16382,16384
.END
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -