;--------------------------------------速度计算和调节子程序------------------------------------------------

SPEED_REG

         LDP       #0

         SPLK       #32,SPEED_COUNT    ;2⁵

         CLRC      SXM

         LACC      #0

         OR        #0FFFFH             ;ACC=FFFFH无符号数

         RPT       #15

         SUBC      BCOUNT              ;除1/12机械转时间

         AND       #0FFFFH             ;屏蔽高位

         SETC      SXM

         SUB       SPEED_REF

         NEG                           ;速度偏差= SPEED_REF-SPEED

         BCND      POS,GEQ             ;检测偏差是否超限.≥0跳转

         ABS                           ;<0则取绝对值

         SPLK      #-32,SPEED_COUNT    ;- 2⁵

POS

         SACL      V_ERRORK            ;保存速度偏差

         SUB       #03FFH              ;检测上限

         BCND      OKPOS,LEQ           ;不超上限则跳转

         SPLK      #03FFH, V_ERRORK    ;否则=3FFH

OKPOS

         LT        V_ERRORK            ;-1024 <速度偏差< 1024

         MPY       SPEED_COUNT         ;乘2⁵

         PAC

         SACL      V_ERRORK            ;速度偏差<<5

         LT         V_ERRORK           ;速度比例调节

         MPY       #KPS                ;乘比例系数

         PAC

         ADD       IDC_REF,16

         SACH      IDC_REF  ;IDC_REF(K)=IDC_REF(K-1) + KPS*V_ERRORK

         LACC      IDC_REF             ;检测IDC_REF是否超限

         BCND      RES,GEQ             ;≥0跳转

         SPLK      #0,IDC_REF          ;超下限则=0

RES

         SPLK      #0,SPEED_COUNT      ;速度调节环计数器清0

         RET

;--------------------------------------ADC中断子程序---------------------------------------------------

ADCINT                                 ;每50微秒转换一次

         MAR       *,AR1               ;保存现场

         MAR       *+

         SST       #1, *+              ;保存ST1

         SST       #0, *+              ;保存ST0

         SACH      *+                  ;保存ACC高16位

         SACL      *                   ;保存ACC低16位

;--------------------------------------速度调节与否---------------------------------------------------------------

         LDP       #0

         LACC      STALL               ;检测磁定位是否完成?

         BCND      VDC__IDC,QE         ;STALL=0(磁定位没完成)禁止速度调节

         LACC      SPEEDFLAG           ;禁止速度调节?

         BCND      VDC__IDC,NQE     ;SPEEDFLAG=1,禁止速度调节

         LACC      SPEED_COUNT         ;检测是否该速度调节?

         SUB       #2000               ;每100ms进行一次速度调节

         BCND      NO_SPEED_REG,NQE    ;时间没到,跳转

         CALL      SPEED_REG           ;时间到,调速度调节子程序

NO_SPEED_REG

         LACC      SPEED_COUNT

         ADD       #1

         SACL      SPEED_COUNT         ;SPEED_COUNT+1

;--------------------------------------读ADC转换结果-------------------------------------------------------

VDC__IDC

         LDP       #0E0H

         LACC      PIVR                ;清ADC中断标志

         LDP      #0E8H

         LACC     EVAIFRA            ;清T1周期中断标志

         SACL     EVAIFRA

         LDP       #0E1H

         LACC      ADCTRL2

         OR        #0202H

         SACL      ADCTRL2

         LACC      RESULT0,10         ;读电流值

         LDP       #0

         SACH      IDC_ERRORK

         LDP       #0E1H

         LACC      RESULT1,10         ;读A相电压

         LDP       #0

         SACH      V1

         LDP       #0E1H

         LACC      RESULT2,10         ;读B相电压

         LDP       #0

         SACH      V2

         LDP       #0E1H

         LACC      RESULT3,10         ;读C相电压

         LDP       #0

         SACH      V3

;--------------------------------------电流比例调节-----------------------------------------------------------

         SETC      SXM

         LACC      IDC_ERRORK,5      ;Q5格式

         SUB       IDC_REF,5

         SACL      IDC_ERRORK        ;保存电流偏差,Q5格式

         LT        IDC_ERRORK         ;电流比例调节

         MPY       #KP                 ;Q11格式

         PAC                            ;ACC = KP*IDC_ERRORK

         ADD       COMP,16

         SACH      COMP               ;KP*IDC_ERRORK + COMP(K-1)

         LACC      COMP               ;检测是否超限

         BCND      SUP_LIM,GT

         SPLK       #0,COMP            ;超过下限,COMP=0

         B          COMP_OK

SUP_LIM

         SUB       #0500

         BCND      COMP_OK,LT

         SPLK      #0500,COMP          ;超过上限,COMP=500

COMP_OK                               ;在上下限内

;--------------------------------------换相准备-----------------------------------------------------------------

         LDP       #0

         LACC      FLAG                ;过零?

         BCND      NEU,QE              ;没过零,跳转

         LACC      B2COUNT            ;过零,执行延迟时间

         SUB       #1                    ;延迟时间减1

         SACL      B2COUNT

         SETC      SXM

         BCND     NEU,NQE              ;延迟时间没到,跳转

         LACC      CAPT                ;否则更新换相控制字

         ADD       #2

         SACL      CAPT

         SUB       #0CH

         BCND     OKCAPT,NQE           ;没超过12,跳转

         SPLK      #0,CAPT             ;超过12,CAPT=0

OKCAPT

         SPLK      #0,FLAG             ;置过零标志

         SPLK      #0,ASYM            ;延时计数器清0,准备延时

;--------------------------------------中性点电压计算----------------------------------------------------------

NEU

         LACC      V1

         ADD       V2

         ADD       V3

         SACL      NEUTRAL           ;保存3*NEUTRAL

         SPLK      #1,FLAGCUR         ;允许更新比较值

         LACC      STALL              ;检查磁定位是否完成

         BCND      SPEEDUP,NQE       ;磁定位已完成,跳转

;--------------------------------------磁定位更新比较值------------------------------------------------------

         CLRC      SXM                ;否则进行磁定位

         LACC      COMP

         LDP       #0E8H

         SACL      CMPR1               ;更新A相比较值

         LDP       #0

         LACC      BCOUNT            ;磁定位计数器

         ADD       #1                   ;加1

         SACL      BCOUNT

         SUB       #0FFFFH             ;延时2¹⁶*50微秒

         SETC      SXM

         BCND     RESTO,NQE          ;磁定位没完成,退出

         SPLK      #0,TIME             ;磁定位完成,修改标志和初值

         SPLK      #050H,BCOUNT        ;延迟时间初值

         SPLK      #1,STALL            ;磁定位完成标志

         SPLK      #1,SPEEDFLAG       ;禁止速度调节

         B         RESTO               ;退出

;--------------------------------------计算延迟时间------------------------------------------------------

SPEEDUP

         LACC      CAPT                ;转完一转?

         SUB       #4

         BCND      RESTO,NQE           ;没转完,退出

         LACC      FLAGUP              ;是否计算延迟时间?

         BCND      RESTO,NQE           ;不计算,退出

         SPLK      #0,SPEEDFLAG        ;否则计算,允许速度调节

         CLRC      SXM

         LACC      TIME                ;每转时间计数器

         SPLK      #12,BCOUNT

         RPT       #15

         SUBC      BCOUNT            ;除12,得到转30º所用的时间

         AND       #0FFFFH

         SACL      BCOUNT              ;保存作为延迟时间更新值

         SETC      SXM

         SPLK      #0,TIME             ;每转时间计数器清0

         SPLK      #1,FLAGUP           ;改标志

RESTO

         MAR      *, AR1               ;恢复现场

         LACL     *-

         ADDH     *-

         LST       #0, *-

         LST       #1, *-

         CLRC      INTM                ;开中断

         RET

         .END