📄 3_vf_main.asm
字号:
LACC theta_360 ;
SUB theta_r ;
SACL theta_m ; 360-theta_h=>theta_m
E_Q
;-------------------------------------------------------------------
; sin(theta), cos(theta)
;-------------------------------------------------------------------
lt theta_m ; D3. Find index
mpy theta_i ; D3*D6=D(9+1)
pac ; D10
sach sin_indx ; D10
lacc sin_indx,11 ; r/s 5 by l/s 11 -> integer (D15)
sach sin_indx ; right shift 5 bits => D15
lacc sin_entry ; Look up sin
add sin_indx ;
tblr sin_theta ;
lacc sin_end ;
sub sin_indx ;
tblr cos_theta ;
LT SS ; Look up cos
MPY sin_theta ; modify sign: D15*D1=D(16+1)
PAC ;
SACL sin_theta ; left shift 16 bits and save: D1
LT SC ;
MPY cos_theta ; modify sin: D15*D1=D(16+1)
PAC ;
SACL cos_theta ; left shift 16 bits and save: D1
;-------------------------------------------------------------------
; Calcualte Ud & Uq
;-------------------------------------------------------------------
LT set_v ; set v -> T: D1
MPY cos_theta ; set v*cos(theta): D1*D1=D(2+1)
PAC ; product -> ACC: D3
SACH Ud,1 ; d component of ref Uout: D2
MPY sin_theta ; set v*sin(theta): D1*D1=D(2+1)
PAC ; product -> ACC: D3
SACH Uq,1 ; q component of ref Uout: D2
;-------------------------------------------------------------------
; Determine sector
;-------------------------------------------------------------------
lt theta_r ; D3
mpy theta_s ; D3*D0=D4
pac
sach sector ;
lacc sector,5 ; r/s 11 by l/s 5 -> integer (D15)
sach sector ; right shift 11 bits
;-------------------------------------------------------------------
; Calculate T1&T2 based on: Tpwn Uout=V1*T1+V2*T2
;
; i.e. [T1 T2]=Tpwn*inverse[V1 V2]*Uout
; i.e. [0.5*T1 0.5*T2]=Tp*inverse[V1 V2]*Uout
; i.e. [0.5*C1 0.5*C2]=inverse[V1 V2]*Uout=M(sector)*Uout
;
; where C1=T1/Tp, C2=T2/Tp, are normalized wrt Tp
; M(sector)=inverse of [V1 V2] = decomposition matrix
; obtained through table lookup
; Uout=Transpose of [Ud Uq]
; Tp=Timer 1 period = 0.5*Tpwm
; Tpwm=PWM period Tpwm
;-------------------------------------------------------------------
LACC #dec_ms
ADD sector,2 ;
SACL temp ; get the pointer
LAR AR0,temp ; point to parameter table
; Calculate 0.5*C1 based on 0.5*C1=Ud*M(1,1)+Uq*M(1,2)
LT Ud ; D2
MPY *+ ; M(1,1) Ud: D2*D1=D(3+1)
PAC ; D4
LT Uq ; D4
MPY *+ ; M(1,2) Uq: D2*D1=D(3+1)
APAC ; 0.5*C1: D4+D4=D4
BGEZ cmp1_big0 ; continue if bigger than zero
ZAC ; set to 0 if less than 0
cmp1_big0
SACH temp ; 0.5*C1: D4
LT temp ; D4
MPY t1_periods ; D4*D10 = D(14+1)
PAC ; D15
SACH cmp_1 ; 0.5*C1*Tp: D15
; Calculate 0.5*C2 based on 0.5*C2=Ud*M(2,1)+Uq*M(2,2)
LT Ud ; D2
MPY *+ ; M(2,1) Ud: D2*D1=D(3+1)
PAC ; D4
LT Uq ; D2
MPY *+ ; M(2,2) Uq: D2*D1=D(3+1)
APAC ; 0.5*C2: D4+D4=D4
BGEZ cmp2_big0 ; continue if bigger than zero
ZAC ; zero it if less than zero
cmp2_big0
SACH temp ; 0.5*C2: D4
LT temp ; D4
MPY t1_periods ; D4*D10 = D(14+1)
PAC ; D15
SACH cmp_2 ; 0.5*C2*Tp: D15
; Calculate 0.5*C0 based on 0.5*C3*Tp=Tp*(1-0.5*C1-0.5*C2)
LACC #t1_period_ ;
SUB cmp_1 ;
SUB cmp_2 ; D15
BGEZ cmp0_big0 ; continue if bigger than zero
ZAC ; zero it if less than zero
cmp0_big0
SACL cmp_0 ;
LACC cmp_0,15 ; right shift 1b (by l/s 15b)
SACH cmp_0 ; 0.25*C0*Tp
;-------------------------------------------------------------------
; Determine channel toggling sequence and load compare registers
;-------------------------------------------------------------------
ldp #4h
LACL first_var
ADD sector ; point to entry in look up table
TBLR first_tog ; get 1st-to-toggle channel
LAR AR0,first_tog ; point to the channel
LACC cmp_0 ;
SACL * ; cmp_0 => the channel
LACL second_var ;
ADD sector ; point to entry in look up table
TBLR sec_tog ; get 2nd-to-toggle channel
LAR AR0,sec_tog ; point to the channel
LACC cmp_0 ;
ADD cmp_1 ; cmp_0+cmp_1
SACL * ; => the channel
LACC #CMPR3 ;
SUB first_tog ;
ADD #CMPR2 ;
SUB sec_tog ;
ADD #CMPR1 ;
SACL temp ; get 3rd-to-toggle channel
LAR AR0,temp ; point to the channel
LACC cmp_0 ;
ADD cmp_1 ;
ADD cmp_2 ; cmp_0+cmp_1+cmp_2
SACL * ; =>the channel
ldp #0E1H ;
lacc PBDATDIR ;
OR #0FFH
NOP
sacl PBDATDIR ;
KICK_DOG
RET ; return
;----------------------------------------------------------------------------
PHANTOM
KICK_DOG
LDP #0
SST #ST0,ST0_save ; save status register ST0
SST #ST1,ST1_save ; save status register ST1
LDP #0 ; set DP
SACH ACCH ;
SACL ACCL
LDP #0
ZALH ACCH ;
ADDS ACCL
LST #ST1,ST1_save ; restore status register ST1
LST #ST0,ST0_save ; restore status register ST0
CLRC INTM ;
RET
;---------------------------------------------------------------------------
jpan
clrc tc ;the main key program
ldp #0e1h
lacc PBDATDIR
cmpl
and #0ffh
ldp #04h
sacl count1
lacl count1
lar ar0,count1
bcnd yjaxm,neq
b jpan
yjaxm ;back wobble
call delay_time
call delay_time
ldp #0e1h
lacc PBDATDIR
cmpl
and #0ffh
ldp #04h
sacl count2
lacc count2
lar ar1,count2
bcnd yjax,neq
b jpan
yjax
;
ldp #04h
mar *,AR1
cmpr 00
bcnd jpan,ntc
sar ar0,count
splk #01h,yjax_flag ;set yiax flag
ret
;pjsf
; ldp #0e1h
; lacc PBDATDIR
; cmpl
; and #0ffh
; ret
;-----------------------------------------------------------------------------
_ci:
LDP #0E0H
BIT SCIRXST, 9 ; test RXRDY bit
BCND _ci, NTC ; if RXRDY NEQ 1, loop back on input
KICK_DOG
RET
;------------------------------------------------------------------------------
_co:
LDP #0E0H
BIT SCICTL2, 8 ; test TXRDY bit
BCND _co, NTC ; if TXRDY NEQ 1, loop back on output
KICK_DOG
RET
;-------------------------------------------------------------------------------
;---------------- HEX CONVERT TO BCD SUB_PROGRAM -------------------------------
hex_bcd ;hex to BCD convert sub_program
ldp #04h
lacl add_temp ;temp register
hb_loop
KICK_DOG
ldp #04h
lacl add_temp
sub #0ah ;decide if lagrer then 10
bcnd add_jixu,geq ;>0,jump to add_jixu
add #0ah ;<0,restore
sacl add_var_l ;
b add_ret
add_jixu
sacl add_pn ;
lacl add_cen
add #1h
sacl add_cen
lacl add_pn
sacl add_temp
lacl add_temp
LDP #0E0H ;clear the ERROR flags
BIT SCIRXST,8
BCND hb_loop,NTC
LACL SCICTL1 ;send 0 to the 8th bit of SCIRXST
AND #0FFDFH
SACL SCICTL1
LACL SCICTL1
OR #0020H ;send 1 to the 8th bit of SCIRXST
SACL SCICTL1
b hb_loop
add_ret
lacl add_cen
sacl add_var_h
splk #00h,add_cen
ret
;-----------------------------------------------------------------------------
DIS ;display sub_program
LDP #0E1H
LACL PCDATDIR
AND #0FF7FH ;iopc7=0
SACL PCDATDIR
LDP #06H
MAR *,AR0
LAR AR0,#03H ;four displayers
MAR *,AR1
LAR AR1,#LED0
DIS1
MAR *,AR1
LACL *+,AR1
ADD #led_table
LDP #06H
TBLR TEMP
LACC TEMP,8
SACL TEMP
LACC TEMP,16
SACH TEMP
MAR *,AR2
LAR AR2,#07H ;eight sects
DIS2
CLRC C ;clear the c flag
NOP
SFL ;arithmatic left shift
SACH TEMP
BCND NEG_DATA,NC ;decide the c equal 0>?
LDP #0E1H ;no
LACL PCDATDIR
OR #040H ;iopc6=1
SACL PCDATDIR
B CONTINUE
NEG_DATA ;c=0
LDP #0E1H
LACL PCDATDIR
AND #0FFBFH ;iopc6=0
SACL PCDATDIR
CONTINUE
NOP
NOP
LACL PCDATDIR
OR #080H ;iopc7=1
SACL PCDATDIR
NOP
NOP
ldp #0e1h
lacc PCDATDIR
AND #0FF7FH ;iopc6=0,generate the clk signal
SACL PCDATDIR
NOP
LDP #06H
LACC TEMP,16
BANZ DIS2
MAR *,AR0
BANZ DIS1
MAR *,AR3
LAR AR3,#00H
LOOP
call delay_time
BANZ LOOP
KICK_DOG
RET
led_table:
.word 003FH ;"0"
.word 0006H ;"1"
.word 005BH ;"2"
.word 004FH ;"3"
.word 0066H ;"4"
.word 006DH ;"5"
.word 007DH ;"6"
.word 0007H ;"7"
.word 007FH ;"8"
.word 006FH ;"9"
.word 0077H ;"A"
.word 007CH ;"B"
.word 0039H ;"C"
.word 005EH ;"D"
.word 0079H ;"E"
.word 0071H ;"F""
;---------------------------------------------------------------------------------
delay_time:
KICK_DOG
CLRC SXM
LACC #0010H
delay:
SUB #01
RPT #80h
nop
BCND delay,NEQ
NOP
NOP
RET
;---------------------------------------------------------------------------------
yanshi
KICK_DOG
CLRC SXM
LACC #0cfffH
time_loop
SUB #01
RPT #80h
nop
BCND time_loop,NEQ
NOP
NOP
RET
;---------------------------------------------------------------------------------
.data
;-------------------------------------------------------------------
; Frequently used angles
;-------------------------------------------------------------------
********************************************************************
** The order between these angles and the decomposition **
** matrices in the following must not be changed. **
********************************************************************
angles_
.word 01922h ; pi/2: D3
.word 03244h ; pi: D3
.word 04b66h ; 3*pi/2: D3
.word 06488h ; 2*pi: D3
;-------------------------------------------------------------------
; Decomposition matrices indexed by the sector Uout is in for s/w
; implemented SV PWM scheme
;-------------------------------------------------------------------
.word 20066 ; D1
.word -11585
.word 0
.word 23170
.word -20066
.word 11585
.word 20066
.word 11585
.word 0
.word 23170
.word -20066
.word -11585
.word 0
.word -23170
.word -20066
.word 11585
.word -20066
.word -11585
.word 20066
.word -11585
.word 20066
.word 11585
.word 0
.word -23170
;-------------------------------------------------------------------
; Addresses of compare registers of the 1st-to-toggle channels
; indexed by the sector, ref v is in
;-------------------------------------------------------------------
firstfz_
.word CMPR1 ;
.word CMPR2 ;
.word CMPR2 ;
.word CMPR3 ;
.word CMPR3 ;
.word CMPR1 ;
;-------------------------------------------------------------------
; Addresses of compare registers of the 2nd-to-toggle channels
; indexed by the sector, ref v is in
;-------------------------------------------------------------------
secondfz_
.word CMPR2 ;
.word CMPR1 ;
.word CMPR3 ;
.word CMPR2 ;
.word CMPR1 ;
.word CMPR3 ;
;---------------------------------------------------------------------------------
firstzz_
.word CMPR1 ;
.word CMPR3 ;
.word CMPR3 ;
.word CMPR2 ;
.word CMPR2 ;
.word CMPR1 ;
;---------------------------------------------------------------------------------
secondzz_
.word CMPR3 ;
.word CMPR1 ;
.word CMPR2 ;
.word CMPR3 ;
.word CMPR1 ;
.word CMPR2 ;
;---------------------------------------------------------------------------------
; sine table for theta from 0 to 90 per every 1 degree
;-----------------------------------------------------------
sin_entry_ ; sin table
.word 0 ; D1
.word 286
.word 572
.word 857
.word 1143
.word 1428
.word 1713
.word 1997
.word 2280
.word 2563
.word 2845
.word 3126
.word 3406
.word 3686
.word 3964
.word 4240
.word 4516
.word 4790
.word 5063
.word 5334
.word 5604
.word 5872
.word 6138
.word 6402
.word 6664
.word 6924
.word 7182
.word 7438
.word 7692
.word 7943
.word 8192
.word 8438
.word 8682
.word 8923
.word 9162
.word 9397
.word 9630
.word 9860
.word 10087
.word 10311
.word 10531
.word 10749
.word 10963
.word 11174
.word 11381
.word 11585
.word 11786
.word 11982
.word 12176
.word 12365
.word 12551
.word 12733
.word 12911
.word 13085
.word 13255
.word 13421
.word 13583
.word 13741
.word 13894
.word 14044
.word 14189
.word 14330
.word 14466
.word 14598
.word 14726
.word 14849
.word 14968
.word 15082
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -