sensor.asm

该软件是关于无刷直流电动机无传感器调速控制的汇编程序

SUB OFFSET ;ACC=3*BemfB corrected
BLZ END ;Sign Changed?（2407对应BCND      END,LT）
SPLK #1,FLAG ;Bemf sign has changed
LACC BCOUNT ;Load Shift Time
SACL B2COUNT
B END
RISING2
LACC COMP ;Input Current Path: Phase B
LDP #0E8h ;Output Current Path: Phase C
SPLK #03DFH,ACTR ;Non fed phase Phase A
SACL CMPR2
SPLK #0FFFH,CMPR3
SPLK #0FFFH,CMPR1;2407为0000H
;Commutation glitches filter
LDP #0
LACC ASYM
ADD #1
SACL ASYM
SUB #10
BLEZ END;2407对应于BCND      END,LEQ
SPLK #10,ASYM
;Zero crossed?
LDP #0
LACC FLAG ;Did BemfA sign already changed?
BNZ END ;If yes then END（2407对应于 BCND      END,NEQ）
;Else:
;Bemf zero detection
LACC V1,1
ADD V1 ;ACC=3*(BemfA + Neutral)
SUB NEUTRAL ;ACC=3*BemfA
BGEZ END ;Sign Changed?（2407对应于BCND      END,GEQ）
SPLK #1,FLAG ;BemfA Sign has Changed
LACC BCOUNT ;Load Shift Time
SACL B2COUNT
B END
RISING1
LACC COMP ;Input Current Path Phase A
LDP #0E8h ;Output Current Path Phase B
SPLK #0F3DH,ACTR ;Non fed phase Phase C
SACL CMPR1
SPLK #0FFFH,CMPR2
SPLK #0FFFH,CMPR3;2407为0000H
;Commutation glitches filter
LDP #0
LACC ASYM
ADD #1
SACL ASYM
SUB #10
BLEZ END;2407对应BCND      END,LEQ
SPLK #10,ASYM
;Zero crossed?
LDP #0
LACC FLAG ;Did BemfC sign already changed?
BNZ END ;If yes then END（2407对应BCND      END,NEQ ）
;Else:
;Bemf zero detection
LDP #0
LACC V3,1
ADD V3 ;ACC=3*(BemfC + Neutral)
SUB NEUTRAL ;ACC=3*BemfC
BGEZ END ;Sign Changed?（2407对应BCND      END,GEQ）
SPLK #1,FLAG ;BemfC Sign has changed
LACC BCOUNT ;Load Shift Time
SACL B2COUNT
B END
FALLING1
LACC COMP ;Input Current Path Phase B
LDP #0E8h ;Output Current Path Phase A
SPLK #0FD3H,ACTR ;Non fed phase Phase C
SACL CMPR2
SPLK #0FFFH,CMPR3
SPLK #0FFFH,CMPR1;2407为0000H
;Commutation glitches filter
LDP #0
LACC ASYM
ADD #1
SACL ASYM
SUB #10
BLEZ END;2407对应BCND      END,LEQ 
SPLK #10,ASYM
;Zero crossed?
LDP #0
SPLK #0,FLAGUP
LACC FLAG ; Did BemfC sign already changed?
BNZ END ;If Yes then END（2407对应BCND      END,NEQ ）
;Else
;Bemf zero detection
LDP #0
LACC V3,1
ADD V3 ;ACC=3*(BemfC + Neutral)
SUB NEUTRAL ;ACC=3*BemfC
SUB OFFSET ;ACC=3*Bemf3 corrected
BLZ END ;Sign Changed?（2407对应 BCND      END,LT ）
SPLK #1,FLAG ;BemfC Sign has changed
LACC BCOUNT ;Load Shift Time
SACL B2COUNT
END
RET
SPEED_REG
MAR *,AR2
LAR AR2,#0303h ;Speed error Pointer Init
LDP #0
SPLK #32,SPEED_COUNT ;Speed Error Shifter setting
;Speed and speed error calc
CLRC SXM
ZAC;240独有的指令:清除累加器的指令为0.（对应的2407程序指令为"LACC  #0"）
OR #0FFFFH
RPT #15
SUBC BCOUNT
AND #0FFFFH ;Acc=1 / (rev. time/ 12) = Speed
SETC SXM
SUB SPEED_REF
NEG ;Acc= Spd ref-Spd Fb= Spd error
;Speed error limitation
BGEZ POS;BGEZ的意思是:如果累加器大于或等于零则跳转.对应的2407指令为: "BCND  POS,GEQ"   
ABS
SPLK #-32,SPEED_COUNT
POS
SACL *;对应2407指令:"SACL  V_ERRORK" , V_ERRORK在本程序中被辅助寄存器替代了
SUB #03FFH
BLEZ OKPOS;BLEZ的意思是:如果累加器小于等于零则跳转. 对应的2407指令为"BCND  OKPOS,LEQ" 
SPLK #03FFH,*;对应的2407指令为: "SPLK    #03FFH, V_ERRORK"
OKPOS
LT * ;对应的2407指令为"LT    V_ERRORK":-1024 < Speed error < 1024
MPY SPEED_COUNT
PAC
SACL * ;*=2407程序中的V_ERRORK ,Speed error <<5
;Speed regulation
LT *;*=2407中的V_ERRORK
MPY #Kps
PAC
ADD Idc_ref,16
SACH Idc_ref ;Idc_ref(k)=Idc_ref(k-1) + K*Speed_error(k)
;Idc_ref compensation
LACC Idc_ref
BGEZ RES;BGEZ的意思是:如果累加器大于或等于零则跳转
SPLK #0,Idc_ref
;Reset Speed loop timer
RES
LACC SPEED_REF,11 ;Bemf correction computation
SACH OFFSET
SPLK #0,SPEED_COUNT
RET
PWMINT    ;2407程序中无此中断
;save status registers
MAR *,AR1
MAR *+ ; skip one position
SST #1, *+ ; save ST1
SST #0, *+ ;save ST0
SACH *+ ;save acc high
SACL * ;save acc low
LDP #0E8H ;Clear Interrupt Vector
LACC IVRA
LDP #0
LACC CUR_COUNT
ADD #1
SACL CUR_COUNT
SUB #2
BGZ CONV ;Time to start a Conversion?
LDP #0E0H
LACC ADCTRL1
OR #02000H
SACL ADCTRL1
B CONTEXT
CONV
SUB #2 ;Reset CUR_COUNT variable?
BLZ CONTEXT
SPLK #0,CUR_COUNT
CONTEXT
;restore status registers
MAR *, AR1 ; make stack pointer active
LACL *- ;Restore Acc low
ADDH *- ;Restore Acc high
LST #0, *- ; load ST0
LST #1, *- ; load ST1
CLRC INTM
RET
ADCINT
MAR *,AR1
MAR *+ ; skip one position
SST #1, *+ ; save ST1
SST #0, *+ ; save ST0
SACH *+ ;save acc high
SACL * ;save acc low
;Magnetic stall flag
LDP #0
LACC STALL
BZ Vdc_or_Idc ;BZ指令的意思是:如果累加器为0则跳转.If Magnetic Stall is cur. Performed then
;Branch to Vdc_or_Idc
LACC SPEEDFLAG ;If the first Revolution has not been
BNZ Vdc_or_Idc ;BNZ的意思是:如果累加器不等于零则跳转.对应于2407的指令为"BCND      VDC_IDC,NEQ:Completed then branch to Vdc_or_Idc
;Time to speed loop?
LACC SPEED_COUNT
SUB #2000
BNZ NO_SPEED_REG;对应于"BCND      NO_SPEED_REG,NEQ"
CALL SPEED_REG ;If Time then Speed Loop
NO_SPEED_REG
LACC SPEED_COUNT
ADD #1
SACL SPEED_COUNT
;Vdc OR Idc?
Vdc_or_Idc
LDP #0E0h
LACC SYSIVR ;Clear interrupt vector
LDP #0E0H
LACC ADCTRL1
AND #0002H
BZ Vdc ;If second conv then branch to Vdc
;current error calculation
CLRC SXM
LACC ADCFIFO1,10 ;high Acc = Idc
ADD ADCFIFO2 ;low Acc = V2<<6
LDP #0
SACH Idc_errorK ;Store Idc
SACL V2 ;Store V2<<6
LACC V2,10
SACH V2 ;Store V2
SETC SXM
LACC Idc_errorK,5
SUB Idc_ref,5  ;得到电流偏差，开始调节电流
SACL Idc_errorK ;Store Current error<<5
;Current regulation
LT Idc_errorK
MPY #Kp
PAC ;Acc = Kp*Current_error
ADD COMP,16
SACH COMP ;Store Kp*Current_error + COMP(k-1)
LACC COMP ;Load updated Duty cycle
;current reg output limitation
BGZ SUP_LIM;BGZ的意思是:如果累加器大于零则跳转
SPLK #0,COMP
B COMP_OK
SUP_LIM
SUB #0112
BLZ COMP_OK;BLZ的意思是:如果累加器小于零则跳转.对应2407程序指令为:BCND      COMP_OK,LT
SPLK #0112,COMP
COMP_OK ;0 < Updated Duty cycle < Max duty
LACC STALL
BNZ SPEEDUP ;If running mode Branch to SPEEDUP
CLRC SXM ;Else (Magnetic Stall Performing):
SPLK #2,CUR_COUNT ;Cancel second conversion start
LACC COMP
LDP #0E8H
SACL CMPR1 ;Output Updated Magnetic Stall Duty Cycle
LDP #0
LACC BCOUNT ;Magnetic Stall Counter Increment
ADD #1
SACL BCOUNT
SUB #0FFFFH
SETC SXM
BNZ RESTO ;Is magnetic stall to the end?
MAR *,AR3 ;If yes then
LAR AR3,#0307H
SPLK #0,*
SPLK #050H,BCOUNT ;Load first Time Shift
SPLK #1,STALL ;Enable running mode
SPLK #1,SPEEDFLAG
B RESTO
;Update speed?
SPEEDUP
LACC CAPT
SUB #4
BNZ RELOAD ;Time to update speed feedback?
LACC FLAGUP ;Speed feedback already updated?
BNZ RELOAD ;If yes branch to RELOAD
MAR *,AR3
LAR AR3,#0307H
SPLK #0,SPEEDFLAG ;Release the first revolution indicator flag
CLRC SXM
LACC * ;Load the revolution time
SPLK #012,BCOUNT
RPT #15
SUBC BCOUNT ;Divide it by 12 (i.e. 30o)
AND #0FFFFH
SACL BCOUNT ;Store the new Shift Time
SETC SXM
SPLK #0,*
SPLK #1,FLAGUP ;Speed feedback updated
;Reload Vdc channel
RELOAD
LDP #0E0H
SPLK #1B5Ch,ADCTRL1 ;ADC 6&13
;restore context
RESTO
MAR *, AR1 ; make stack pointer active
LACL *-
ADDH *-
LST #0, *- ; load ST0
LST #1, *- ; load ST1
CLRC INTM
RET
Vdc
;Read Vshunt values
LDP #0E0H
CLRC SXM
LACC ADCFIFO1,10 ;Acc high = V3
ADD ADCFIFO2 ;Acc low = V1<<6
LDP #0
SACH V3 ;Store V3
SACL V1 ;Store V1<<6
LACC V1,10
SACH V1 ;Store V1
;Zero crossed?
LACC FLAG ;Did the Bemf already crossed zero?
BZ NEU ;If Not then Branch to NEU
;Commutation instant
LACC B2COUNT ;If yes decrement shift time counter
SUB #1
SACL B2COUNT ;Store new shift time counter value
SETC SXM
BNZ NEU ;Is it time to commutate a new phase pair?
LACC CAPT ;If yes then upgrade the commutated phases
ADD #2 ;remainder
SACL CAPT ;Store the commutated phases remainder
SUB #0CH
BNZ OKCAPT
SPLK #0,CAPT
OKCAPT
SPLK #0,FLAG ;Reset flags for the new Bemf scanning
SPLK #0,ASYM
;Neutral calculation
NEU
LACC V1
ADD V2
ADD V3 ;Acc=3*neutral point voltage
SACL NEUTRAL ;Store 3*neutral point voltage
;output PWM
SPLK #1,FLAGCUR ;Set flag to let the PWM unit to be updated
;Reload Idc channel
LDP #0E0H
SPLK #1b6ah,ADCTRL1 ;Loaded ADC lines: Idc and V2
;restore context
MAR *, AR1 ; make stack pointer active
LACL *-
ADDH *-
LST #0, *- ; load ST0
LST #1, *- ; load ST1
CLRC INTM
RET