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;死区时间0.5us
SPLK #00125,CMPR1; 占空比为0
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          ;对应的2407程序中这一句为"BCND  MAGSTALL,EQ" 
;Set next commutation sequence
LACC COMP ;If so then set the ACTR Register
LDP #0E8h ;to commutate the next phase pair.
SPLK #03DFh,ACTR   ;2407对应于动作控制寄存器ACTRA
SACL CMPR2
SPLK #0FFFFH,CMPR3
SPLK #0FFFFH,CMPR1   ;2407程序中这里对应的CMPR3、CMPR1都是0000H
LDP #0 ;Trace of the current 60o sector
SPLK #4,CAPT
LOOP
LDP #0
LACC FLAGCUR
BZ LOOP ;Time to update the Duty Cycle?（2407程序对应 "BCND      LOOP,EQ"）
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（旋转时间计数器，对应于2407程序中的TIME）
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   ;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 already crossed?
LDP #0
LACC FLAG ;Did BemfA sign already changed?
BNZ END ;If yes then END（2407对应"BCND      END,NEQ"）
;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?(2407程序中对应于 "BCND      END,LT") 
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;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 bemfB sign already changed?
BNZ END ;If yes then END（2407对应于 BCND      END,NEQ）
;Else
;Bemf zero detection
LACC V2,1
ADD V2 ;ACC=3*(BemfB + Neutral)
SUB NEUTRAL ;ACC=3*BemfB
BGEZ END ;Sign Changed?（2407对应于 BCND      END,GEQ）
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;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 BemfB sign changed already?
BNZ END ;If yes then END（2407程序对应于BCND      END,NEQ ）
;Else:
;Bemf zero detection
LACC V2,1
ADD V2 ;ACC=3*(BemfB + Neutral)
SUB NEUTRAL ;ACC=3*BemfB