sensor.asm

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

;********************************************************
; File Name : sensor.asm
;
; Target System : c240 evm
;
; Description : BLDC motor speed controlled
; 40W 18V July/97
; Sensorless Speed control based on
; Bemf measurement.
; Magnetic Stall at start up.
; Phases’ use imbalance Correction
; Commutation dV/dt and dI/dT filtering
;
;Date September 1997
;*********************************************************
.include "c240app.h"
;-------------------------------------------
;Current regulator coeff setting
;-------------------------------------------
Kp .set 280 ;Q11 (1=2048) Kp=0.12
;-------------------------------------------
;Speed regulator coeff setting
;-------------------------------------------
Kps .set 100
Kis .set 100
;-------------------------------------------
; Variable definitions
;-------------------------------------------
.bss CAPT,1 ; capt indication
.bss COMP,1
.bss Idc_ref,1
.bss Idc_errorK,1
.bss FLAGCUR,1
.bss CUR_COUNT,1
.bss SPEED_REF,1
.bss SPEED_COUNT,1
.bss V1,1
.bss V2,1
.bss V3,1
.bss NEUTRAL,1
.bss FLAG,1
.bss FLAGUP,1
.bss BCOUNT,1
.bss B2COUNT,1
.bss STALL,1
.bss ASYM,1
.bss SPEEDFLAG,1
.bss OFFSET,1
.bss stack,6
;======================================
; Reset & interrupt vectors
;======================================
.sect "vectors"
RSVECT B _c_int0
.space 16*2
INT2 B PWMINT ;Assign PWM interrupt vector
.space 16*6
INT6 B ADCINT ;Assign ADC interrupt vector
.space 16*38
.text
.global _c_int0
_c_int0
SETC CNF
CLRC OVM ;Reset overflow mode
SETC SXM ;Reset sign extension mode
CLRC XF ;Reset debugging pin
SETC INTM ;Set global interrupt mask
MAR *,AR2 ;Auxiliary Registers Init
LAR AR2,#0300h
SPLK #0,*+
SPLK #0,*+
SPLK #0,*+
SPLK #0,*+
SPLK #0,*+
SPLK #0,*+
SPLK #0,*+
SPLK #0,*+
SPLK #0,*+
SPLK #0,*+
SPLK #0,*+
SPLK #0,*+
SPLK #0,*+
SPLK #0,*+
LAR AR2,#0300h
LAR AR1,#stack
LAR AR3,#0307h
;Disable watchdog (Vccp=5v)& watchdog counter reset p6-12
LDP #00E0h
SPLK #0006Fh, WD_CNTL
SPLK #05555h, WD_KEY
SPLK #0AAAAh, WD_KEY
; initialize WDT registers
SPLK #06Fh, WD_CNTL ; clear WDFLAG, Disable WDT, set WDT for 1
; Set up CLKOUT to be SYSCLK p6-6
SPLK #40C0h,SYSCR
LACC SYSSR
AND #069FFh
SACL SYSSR
;set up PLL clockin=10Mhz,CPUCLOCK=20Mhz,SYSCLK=10Mhz
SPLK #0002h,CKCR0 ; PLL disabled
SPLK #00b1h,CKCR1
SPLK #0081h,CKCR0
;Set up one Wait States for I/O space (DAC needs one wait state)
MAR *,AR1
LACC #0004h
SACL *
OUT *,WSGR
;I/O setting p11-11
LDP #00E1h
SPLK #0000fh, OCRA
SPLK #0070h, OCRB
SPLK #02800h, PBDATDIR ;Clear Fault and no Shut Down
LACC PBDATDIR
; Clear EV control registers
LDP #0E8h
SPLK #0000h,T1CON ;no timer enable
SPLK #0000h,T1PER ;no timer features enable
SPLK #0000h,T1CNT
SPLK #0000h,T1CMP
SPLK #0000h,T2CON
SPLK #0000h,T2PER
SPLK #0000h,T2CNT
SPLK #0000h,T2CMP
SPLK #0000h,T3CON
SPLK #0000h,T3PER
SPLK #0000h,T3CNT
SPLK #0000h,T3CMP
SPLK #0000h,COMCON
SPLK #0000h,DBTCON
SPLK #0000h,ACTR
SPLK #0000h,SACTR
SPLK #0000h,CMPR1
SPLK #0000h,CMPR2
SPLK #0000h,CMPR3
SPLK #0000h,SCMPR1
SPLK #0000h,SCMPR2
SPLK #0000h,SCMPR3
SPLK #0000h,CAPCON ;no capture enable
SPLK #00ffh,CAPFIFO
LACC FIFO1
LACC FIFO2
LACC FIFO3
;PWM Unit setting
SPLK #0125,T1PER
SPLK #0000h,T1CNT
SPLK #0FFFh,ACTR
SPLK #0508h,DBTCON
SPLK #00125,CMPR1
SPLK #00125,CMPR2
SPLK #00125,CMPR3
SPLK #0287h,COMCON
SPLK #8287h,COMCON
SPLK #2800h,T1CON
SPLK #2840h,T1CON
SPLK #0000h,GPTCON
;Capture Unit Setting
SPLK #0b0fch,CAPCON
SPLK #00ffh,CAPFIFO
;Core Mask Setting
LDP #0
LACC #022H ;ADC and Group A Interrupt Core Mask
SACL IMR
LACC IFR ;Clear Core Flag Register
SACL IFR
;EV Mask Setting, Vector & Flag reset p11-46
LDP #0E8h
LACC IFRA
SACL IFRA
LACC IFRB
SACL IFRB
LACC IFRC
SACL IFRC
SPLK #0200H,IMRA
SPLK #0,IMRB
SPLK #7,IMRC
LACC IVRA
LACC IVRB
LACC IVRC
;ADC Unit setting p3-8
LDP #0E0h
SPLK #0003h,ADCTRL2
SPLK #1b6ah,ADCTRL1 ;ADC 14&5
CLRC INTM
LDP #0
SPLK #020H,Idc_ref ;Magnetic Stall Desired Current
SPLK #0,Idc_errorK
SPLK #0300H,SPEED_REF ;Speed Reference
SPLK #00112,COMP ;Minimum Duty Cycle
SPLK #0000,CUR_COUNT
SPLK #0000,FLAGCUR
SPLK #0000,SPEED_COUNT
SPLK #0000H,CAPT
SPLK #0000H,V1
SPLK #0000H,V2
SPLK #0000H,V3
SPLK #0000H,NEUTRAL
SPLK #0000H,FLAG
SPLK #0001H,FLAGUP
SPLK #0000H,BCOUNT
SPLK #0000H,B2COUNT
SPLK #0000H,STALL
SPLK #0000H,ASYM
SPLK #0000H,OFFSET
MAR *,AR2
LAR AR2,#0300h
FAULT_CLEAR
LDP #0E1h ;Check if the Pre Driver Signal
LACC PBDATDIR ;Is Cleared
AND #010h
BZ FAULT_CLEAR
SPLK #02820h,PBDATDIR ;If cleared then stop Clear Fault
LDP #0
LACC COMP ;Load Duty Cycle and Set
LDP #0E8h ;PWM Register for the Magnetic
SPLK #03FDh,ACTR ;Stall Function
SACL CMPR1
SPLK #0FFFH,CMPR2
SPLK #0FFFH,CMPR3
MAGSTALL
LDP #0 ;Check if the Magnetic Stall is
LACC STALL ;Terminated
BZ MAGSTALL
;Set next commutation sequence
LACC COMP ;If so then set the ACTR Register
LDP #0E8h ;to commutate the next phase pair.
SPLK #03DFh,ACTR
SACL CMPR2
SPLK #0FFFFH,CMPR3
SPLK #0FFFFH,CMPR1
LDP #0 ;Trace of the current 60o sector
SPLK #4,CAPT
LOOP
LDP #0
LACC FLAGCUR
BZ LOOP ;Time to update the Duty Cycle?
SPLK #0,FLAGCUR
CALL SEQUENCE ;If yes the Call Sequence function
B LOOP ;Then wait for the next updated Duty Cycle
SEQUENCE
MAR *,AR3` ;Revolution Time Counter
LAR AR3,#0307H
LACC *
ADD #1
SACL *
LDP #0 ;Which CMPR register should be updated
LACC CAPT ;with the new duty cycle?
ADD #CAPT_DETER
BACC
CAPT_DETER
B RISING1
B FALLING3
B RISING2
B FALLING1
B RISING3
FALLING2
LACC COMP ;Input Current Path: Phase C
LDP #0E8h ;Output Current Path: Phase B
SPLK #0D3FH,ACTR ;Non Fed Phase: Phase A
SACL CMPR3
SPLK #0FFFH,CMPR2
SPLK #0FFFH,CMPR1
;Commutation glitches filter
LDP #0
LACC ASYM
ADD #1
SACL ASYM
SUB #10
BLEZ END
SPLK #10,ASYM
;Zero already crossed?
LDP #0
LACC FLAG ;Did BemfA sign already changed?
BNZ END ;If yes then END
;Else:
;Bemf zero crossing detection
LDP #0
LACC V1,1
ADD V1 ;ACC=3*(BemfA + Neutral)
SUB NEUTRAL ;ACC=3*BemfA
SUB OFFSET ;ACC=3*BemfA corrected
BLZ END ;Sign Change?
SPLK #1,FLAG ;BemfA Sign has changed
LACC BCOUNT ;Load Shift Time.
SACL B2COUNT
B END
RISING3
LACC COMP ;Input Current Path Phase C
LDP #0E8h ;Output Current Path Phase A
SPLK #0DF3H,ACTR ;Non Fed Phase phase B
SACL CMPR3
SPLK #0FFFH,CMPR2
SPLK #0FFFH,CMPR1
;Commutation glitches filter
LDP #0
LACC ASYM
ADD #1
SACL ASYM
SUB #10
BLEZ END
SPLK #10,ASYM
;Zero crossed?
LDP #0
LACC FLAG ;Did bemfB sign already changed?
BNZ END ;If yes then END
;Else
;Bemf zero detection
LACC V2,1
ADD V2 ;ACC=3*(BemfB + Neutral)
SUB NEUTRAL ;ACC=3*BemfB
BGEZ END ;Sign Changed?
SPLK #1,FLAG ;Bemf Sign Has Changed
LACC BCOUNT ;Load Shift Time
SACL B2COUNT
B END
FALLING3
LACC COMP ;Input Current Path Phase A
LDP #0E8h ;Output Current Path Phase C
SPLK #03FDH,ACTR ;Non Fed Phase Phase B
SACL CMPR1
SPLK #0FFFH,CMPR2
SPLK #0FFFH,CMPR3
;Commutation glitches filter
LDP #0
LACC ASYM
ADD #1
SACL ASYM
SUB #10
BLEZ END
SPLK #10,ASYM
;Zero crossed?
LDP #0
LACC FLAG ; Did BemfB sign changed already?
BNZ END ;If yes then END
;Else:
;Bemf zero detection
LACC V2,1
ADD V2 ;ACC=3*(BemfB + Neutral)
SUB NEUTRAL ;ACC=3*BemfB
SUB OFFSET ;ACC=3*BemfB corrected
BLZ END ;Sign Changed?
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
;Commutation glitches filter
LDP #0
LACC ASYM
ADD #1
SACL ASYM
SUB #10
BLEZ END
SPLK #10,ASYM
;Zero crossed?
LDP #0
LACC FLAG ;Did BemfA sign already changed?
BNZ END ;If yes then END
;Else:
;Bemf zero detection
LACC V1,1
ADD V1 ;ACC=3*(BemfA + Neutral)
SUB NEUTRAL ;ACC=3*BemfA
BGEZ END ;Sign Changed?
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
;Commutation glitches filter
LDP #0
LACC ASYM
ADD #1
SACL ASYM
SUB #10
BLEZ END
SPLK #10,ASYM
;Zero crossed?
LDP #0
LACC FLAG ;Did BemfC sign already changed?
BNZ END ;If yes then END
;Else:
;Bemf zero detection
LDP #0
LACC V3,1
ADD V3 ;ACC=3*(BemfC + Neutral)
SUB NEUTRAL ;ACC=3*BemfC
BGEZ END ;Sign Changed?
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
;Commutation glitches filter
LDP #0
LACC ASYM
ADD #1
SACL ASYM
SUB #10
BLEZ END
SPLK #10,ASYM
;Zero crossed?
LDP #0
SPLK #0,FLAGUP
LACC FLAG ; Did BemfC sign already changed?
BNZ END ;If Yes then END
;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?
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
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
ABS
SPLK #-32,SPEED_COUNT
POS
SACL *
SUB #03FFH
BLEZ OKPOS
SPLK #03FFH,*
OKPOS
LT * ;-1024 < Speed error < 1024
MPY SPEED_COUNT
PAC
SACL * ;Speed error <<5
;Speed regulation
LT *
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
SPLK #0,Idc_ref
;Reset Speed loop timer
RES
LACC SPEED_REF,11 ;Bemf correction computation
SACH OFFSET
SPLK #0,SPEED_COUNT
RET
PWMINT
;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 ;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 ;Completed then branch to Vdc_or_Idc
;Time to speed loop?
LACC SPEED_COUNT
SUB #2000
BNZ NO_SPEED_REG
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
SPLK #0,COMP
B COMP_OK
SUP_LIM
SUB #0112
BLZ COMP_OK
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