📄 svpwm.asm
字号:
IN LDP #6
LACL Fr_set
SACL Fr_set_former
ADC_measure LDP #225
SPLK #2000H,ADCTRL2 ;start conversion
NOP
CONVERSION
BIT ADCTRL2 ,BIT12 ; Wait for SEQ1 Busy bit to
BCND CONVERSION ,TC ; If TC=1, keep looping.
RSXM ;/*读取转换结果*/
LACC RESULT0,10
LDP #6
SACH UDC
LDP #225
LACC RESULT1,10
LDP #6
SACH IA
LDP #225
LACC RESULT2,10
LDP #6
SACH IB
LDP #225
LACC RESULT3,10
LDP #6
SACH CTRL
LDP #225
LACC RESULT4,10
LDP #6
SACH TEMPER
SSXM
LDP #225
SPLK #4000H ,ADCTRL2 ;/*停止转换*/
LDP #6
LT CTRL
MPY #K_Fr_set
PAC
rpt #4
SFR
SACL Speed_set
LACL Speed_set
SUB #051EH
BLZ MIN
LACL Speed_set
SUB #07D70H
BGZ MAX
LACL Speed_set
SACL Fr_set
b WAIT
MIN SPLK #028FH, Fr_set ;1hz
b WAIT
MAX SPLK #07D70H , Fr_set
;-----------------------将IA,IB,IC转换成IPUQ12格式---------------------------------------------------------
WAIT
LDP #6
LACC IA
AND #3FFH ;屏蔽高位
SUB #511 ;向下平移,产生正负电流值
SACL TMP
LT TMP
MPY #KCURRENT ;转换系数,Q8格式
PAC
SFL
SACH IA,7 ;IA, Q12格式
LACC IB
AND #3FFH
SUB #501
SACL TMP
LT TMP
MPY #KCURRENT
PAC
SFL
SACH IB,7
LACC IB
ADD IA
NEG
SACL IC
;IC = -(IB+IA)
;CALL StateDetect
LDP #6
LACL PWMCNT_Fr_set
ADD #1
SACL PWMCNT_Fr_set
LACL PWMCNT_Fr_set
SUB #1
BZ REGULATOR
LACL Fr_set_former
SACL Fr_set
B IN1
REGULATOR LACL Fr_set_former
SUB Fr_set
BLZ INCREASE
LACL Fr_set_former
SUB Fr_set_STEP
SACL Fr_set
B IN2
INCREASE
LACL Fr_set_former
ADD Fr_set_STEP
SACL Fr_set
B IN2
IN1 LACL PWMCNT_Fr_set
SUB #5
BZ NEWCNT1
B IN2
NEWCNT1
SPLK #0 ,PWMCNT_Fr_set
LACL Fr_set_former
SACL Fr_set
IN2
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*Q5
PAC
SACH SIN_INDX ; Q1
LACL SIN_INDX
SFR
SACL SIN_INDX ;相当于右移1位变成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
SACH UA ; UA ,Q12格式
MPY SIN_THETA ; Q14*Q14
PAC
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 ; 指向逆阵表
MAR * ,AR0
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*Q0
PAC ; Q10格式
SACH CMP_1,5 ; 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*Q0
PAC ; Q10格式
SACH CMP_2,5 ; 0.5*C2*TP,Q0格式
LACC #PWM_T ; T1周期值
SUB CMP_1
SUB CMP_2 ; Q0格式
BCND CMP0BIG0,GEQ ;如果大于0继续
LACC #0 ;否则0
CMP0BIG0
SACL CMP_0
LACC CMP_0,15 ; 相当于右移1位,除2
SACH CMP_0 ; 0.25*C0*TP
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 * ;送入该比较器
LACC #CMPR3
SUB FIRST_TOG
ADD #CMPR2
SUB SEC_TOG
ADD #CMPR1
SACL TEMP ;计算第三次比较匹配的比较器地址
LAR AR0,TEMP ;指向该地址
LACC CMP_0
ADD CMP_1
ADD CMP_2 ; CMP_0+CMP_1+CMP_2
SACL * ; 送入该比较器
B REST ; 返回
;---------------------------------------状态检测－－－－－－－－－－－－－
StateDetect
;----------------------------检测直流电压－－－－－－－－－－－－－－－
; LDP #6
;LACL UDC
;SUB #615
;ABS
;SUB #329
;BGZ NORMAL
;LED_D7_OFF
;B NEXT3
;NORMAL LED_D7_ON
NEXT3
LDP #6
LACC IA ; q12
ABS
SUB #MAXCURRENT
BGZ GUOLIU
LACC IB ;q12
ABS
SUB #MAXCURRENT
BGZ GUOLIU1
LACC IC ; q12
ABS
SUB #MAXCURRENT
BGZ GUOLIU2
RET
GUOLIU SETC INTM
STOP_PWM
LED_D9_ON
B GUOLIU
GUOLIU1 SETC INTM
STOP_PWM
LED_D9_ON
B GUOLIU1
GUOLIU2 SETC INTM
STOP_PWM
LED_D9_ON
B GUOLIU2
.data
;--------------------------------------数据段---------------------------------------------------------------------
ANGLES_ .word 01922H ; pi/2, Q12格式
.word 03244H ; pi, Q12格式
.word 04B66H ; 3pi/2, Q12格式
.word 06488H ; 2pi, 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 ; 用于第1次比较匹配的比较寄存器地址,
.word CMPR2 ; 按参考电压所在的扇区索引
.word CMPR2
.word CMPR3
.word CMPR3
.word CMPR1
SECOND_ .word CMPR2 ;用于第2次比较匹配的比较寄存器地址,
.word CMPR1 ; 按参考电压所在的扇区索引
.word CMPR3
.word CMPR2
.word CMPR1
.word CMPR3
.end
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -