SPLK #0000H,B2COUNT
SPLK #0000H,STALL
SPLK #0000H,ASYM
SPLK #0000H,V_ERRORK
SPLK #0000H,TIME
;--------------------------------------准备磁定位-------------------------------------------------------------
LDP #0
LACC COMP ；装载比较初值
LDP #0E8H ;反相驱动
SPLK #03FDH,ACTRA
WM1低有效,PWM6低电平，其他高电平
SACL CMPR1 ;A相入,C相出,B相不通电
SPLK #0000H,CMPR2
SPLK #0000H,CMPR3
;--------------------------------------等待磁定位结束------------------------------------------------------
MAGSTALL
LDP #0
LACC STALL ;检查磁定位是否结束
BCND MAGSTALL,EQ ;等待还没结束
;--------------------------------------磁定位结束换相----------------------------------------------------
LACC COMP
LDP #0E8H .
SPLK #03DFH,ACTRA
WM3低有效,PWM6低电平，其他高电平
SACL CMPR2 ;B相入,C相出,A不通电
SPLK #0000H,CMPR3
SPLK #0000H,CMPR1
LDP #0
SPLK #4,CAPT ;CAPT初值
;--------------------------------------主循环-----------------------------------------------------------------
LOOP
LDP #0
LACC FLAGCUR
BCND LOOP,EQ ；跳转不更新
SPLK #0,FLAGCUR ；更新，清除标志
CALL SEQUENCE ;调用更新子程序
B LOOP ；等待下一次更新
;--------------------------------------假中断处理-------------------------------------------------------------
PHANTOM
CLRC INTM
RET
;--------------------------------------更新比较值或换相子程序-----------------------------------------
SEQUENCE
LDP #0
LACC TIME ;每转时间计数器
ADD #1
SACL TIME
LACC CAPT ；变相控制字
ADD #CAPT_DETER ;加起始地址
BACC
CAPT_DETER
B RISING1 ;相当于H1上升沿
B FALLING3 ;相当于H3下降沿
B RISING2 ;相当于H2上升沿
B FALLING1 ;相当于H1下降沿
B RISING3 ;相当于H3上升沿
FALLING2 ;相当于H2下降沿
LACC COMP
LDP #0E8H
SPLK #0D3FH,ACTRA
WM5低有效,PWM4低电平,其它高电平
SACL CMPR3 ;C相入,B相出,A相不通电
SPLK #0000H,CMPR2
SPLK #0000H,CMPR1
LDP #0 ;延时过滤干扰
LACC ASYM ;延时计数器
ADD #1 ;加1
SACL ASYM
SUB #10 ;检测是否到计数值
BCND END,LEQ ;没到,退出
SPLK #10,ASYM ;计数值已到,测量感应电动势过零点
LACC FLAG ;感应电动势符号变化?
BCND END,NEQ ;变了,退出
LACC V1,1 ;没变,检测.V1乘2
ADD V1 ;ACC=3*(BEMFA + NEUTRAL)
SUB NEUTRAL ;ACC=3*BEMFA
BCND END,LT ;<0符号没变,退出
SPLK #1,FLAG ;否则符号改变,置过零标志
LACC BCOUNT
SACL B2COUNT ;更新延迟时间
B END ;退出
RISING3
LACC COMP
LDP #0E8H
SPLK #0DF3H,ACTRA
WM5低有效,PWM2低电平,其它高电平
SACL CMPR3 ;C相入, A相出, B相不通电
SPLK #0000H,CMPR2
SPLK #0000H,CMPR1
LDP #0 ;延时过滤干扰
LACC ASYM ;延时计数器
ADD #1 ;加1
SACL ASYM
SUB #10 ;检测是否到计数值
BCND END,LEQ ;没到,退出
SPLK #10,ASYM ;计数值已到,测量感应电动势过零点
LDP #0
LACC FLAG ;感应电动势符号变化?
BCND END,NEQ ;变了,退出
LACC V2,1 ;没变,检测.V2乘2
ADD V2 ;ACC=3*(BEMFB + NEUTRAL)
SUB NEUTRAL ;ACC=3*BEMFB
BCND END,GEQ ;≥0符号没变,退出
SPLK #1,FLAG ;否则符号改变,置过零标志
LACC BCOUNT
SACL B2COUNT ;更新延迟时间
B END ;退出
FALLING3
LACC COMP
LDP #0E8H
SPLK #03FDH,ACTRA
WM1低有效,PWM6低电平,其它高电平
SACL CMPR1 ; A相入, C相出, B相不通电
SPLK #0000H,CMPR2
SPLK #0000H,CMPR3
LDP #0 ;延时过滤干扰
LACC ASYM ;延时计数器
ADD #1 ;加1
SACL ASYM
SUB #10 ;检测是否到计数值
BCND END,LEQ ;没到,退出
SPLK #10,ASYM ;计数值已到,测量感应电动势过零点
LDP #0
LACC FLAG ;感应电动势符号变化?
BCND END,NEQ ;变了,退出
LACC V2,1 ;没变,检测.V2乘2
ADD V2 ;ACC=3*(BEMFB + NEUTRAL)
SUB NEUTRAL ;ACC=3*BEMFB
BCND END,LT ;<0符号没变,退出
SPLK #1,FLAG ;否则符号改变,置过零标志
LACC BCOUNT
SACL B2COUNT ;更新延迟时间
B END ;退出
RISING2
LACC COMP
LDP #0E8H
SPLK #03DFH,ACTRA
WM3低有效,PWM6低电平,其它高电平
SACL CMPR2 ;B相入, C相出, A相不通电
SPLK #0000H,CMPR3
SPLK #0000H,CMPR1
LDP #0 ;延时过滤干扰
LACC ASYM ;延时计数器
ADD #1 ;加1
SACL ASYM
SUB #10 ;检测是否到计数值
BCND END,LEQ ;没到,退出
SPLK #10,ASYM ;计数值已到,测量感应电动势过零点
LDP #0
LACC FLAG ;感应电动势符号变化?
BCND END,NEQ ;变了,退出
LACC V1,1 ;没变,检测.V1乘2
ADD V1 ;ACC=3*(BEMFA + NEUTRAL)
SUB NEUTRAL ;ACC=3*BEMFA