📄 main.asm
字号:
SST #ST0,ST0_SAVE ; 保存现场ST0
SST #ST1,ST1_SAVE ; 保存ST1
LDP #6
SACH ACCH
SACL ACCL ; 保存ACC
SPH P_HI
SPL P_LO ; 保存 P
MPY #1 ; P<=T
SPL T_SAVE ; 保存 T
SAR AR0,AR0_SAVE ; 保存AR0
CLRC SXM
LDP #224
LACC PIVR ;读偏移地址
SUB #029H ; T1 下溢中断?
BCND T1UF_ISR,EQ ;是下溢中断跳T1UF_ISR
REST:
LDP #6 ;否则恢复现场
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 #ST1,ST1_SAVE ; 恢复ST1
LST #ST0,ST0_SAVE ; 恢复ST0
CLRC INTM ; 开中断
RET ; 返回
T1UF_ISR:
LDP #232
SPLK #0FFFH,EVAIFRA ; 清中断标志
LDP #6 ; 计算转角增量
LT OMEGA ; Q5
MPY T_SAMPLE ;Q5*Q24
PAC ;积存ACC, Q13
SFR ; 右移一位成Q12格式
ADD THETA_H,16 ; Q12
ADDS THETA_L ;计算绝对位置
SACH THETA_H ;保存
SACL THETA_L
BCND CHK_UPLIM,GEQ ;如果大于0检查上限
ADD THETA_360,16 ;否则与2π比较,Q12
SACH THETA_H ; 保存
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 ; 恢复THETA_H
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 ;在第三象限则跳转
SPLK #1,SC ; COS符号=1
LACC THETA_360
SUB THETA_R
SACL THETA_M ; 在第四象限
E_Q:
LT THETA_M ; Q12.计算查表索引
MPYU THETA_I ; Q12*Q9
PAC ; Q5
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 COS_THETA ; Q14*Q14
PAC ;存入ACC, Q12
SACH UA ; UA ,Q12格式
MPY SIN_THETA ; Q14*Q14
PAC ; 存入ACC, Q12
SACH UB ; UB, Q12格式
LT THETA_R ; Q12格式.确定扇区
MPY THETA_S ; Q12*Q15
PAC
SACH SECTOR
LACC SECTOR,5
SACH SECTOR ; 相当于右移11位变成Q0格式(整数)
LACC #DEC_MS ;逆阵数据首地址
ADD SECTOR,2
SACL TEMP ;产生地址指针
LAR AR0,TEMP ; 指向逆阵表
LT UA ; Q12格式.计算UA*M(1,1)+UB*M(1,2)
MPY *+ ; M(1,1) UA, Q12*Q14
PAC ; Q10格式
LT UB ; Q12
MPY *+ ; M(1,2) UB, Q12*Q14
APAC ; 0.5*C1, Q10
BCND CMP1BIG0,GEQ ; 如果大于0继续
LACC #0 ; 否则0
CMP1BIG0:
SACH TEMP ; 0.5*C1,Q10格式
LT TEMP ; Q10格式
MPY T1_PERIODS ; Q10*Q5
PAC ; Q15格式
ADD #1,15 ; 防止C1=0
SACH CMP_1,1 ; 0.5*C1*TP,Q0格式
LT UA ;计算UA*M(2,1)+UB*M(2,2)
MPY *+ ; M(2,1) UA,Q12*Q14
PAC ; Q10格式
LT UB ; Q12格式
MPY *+ ; M(2,2) UB: Q12*Q14
APAC ; 0.5*C2,Q10
BCND CMP2BIG0,GEQ ; 如果大于0继续
LACC #0 ;否则0
CMP2BIG0:
SACH TEMP ; 0.5*C2,Q10格式
LT TEMP ; Q10格式
MPY T1_PERIODS ; Q10*Q5
PAC ; Q15格式
ADD #1,15 ; 防止C2 = 0
SACH CMP_2,1 ; 0.5*C2*TP,Q0格式
LACC #CCKWISE_ ; ACTRA寄存器值表首地址
ADD SECTOR ; 查表地址
TBLR SVPAT
LAR AR0,#ACTRA ;指向ACTRA
LACC * ; 读ACTRA
AND #0FFFH ; 清高四位
OR SVPAT ; 高四位赋新值
SACL * ; 更新ACTRA
LAR AR0,#CMPR1 ; 指向CMPR1
LACC CMP_1
SACL *+ ; 更新CMPR1, 指向CMPR2
ADD CMP_2
SACL * ; 更新CMPR2
SUB #500 ;减T1周期值,CMPR2是否超限
BCND IN_LMT,LEQ ;没超跳转
SPLK #500,* ;否则赋T1周期值
IN_LMT:
B REST ; 返回
.data
;--------------------------------------数据段------------------------------------------------------------------
ANGLES:
.word 01922H ; π/2, Q12格式
.word 03244H ; π, Q12格式
.word 04B66H ; 3π/2, Q12格式
.word 06488H ; 2π, 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 -20066
.word 11585
.word 0
.word -23170
.word -20066
.word -11585
.word 20066
.word -11585
.word 0
.word -23170
.word 20066
.word 11585
CCKWISE_:
.word 0001000000000000B ;逆时针旋转时ACTR高4位值,按扇区索引
.word 0011000000000000B
.word 0010000000000000B
.word 0110000000000000B
.word 0100000000000000B
.word 0101000000000000B
SIN_ENTRY_:
.word 0 ; 0-90度SIN值表,Q14格式
.word 286,572,857,1143,1428
.word 1713,1997,2280,2563,2845
.word 3126,3406,3686,3964,4240
.word 4516,4790,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 + -