.data 
halfsqrt3	.word 6ed9h;1.732/2 Q15
halfvdc		.int 6e35h;1.722/2 Q15 
;vdcint	.int 122h;PWMPRD/vdc 290
half32  .int 6000h;0.75
;pwmt	.int 01f4h;03e8h;

table1	
	.int 7f83h,0b29h;5度
	;.int 6ed9h,4000h;30度
	.int 5247h,620ch;50度
	.int 3617h,7401h ;65度
	.int 0e9c8h,7e0dh;100度
	.int 9df3h,5247h;140度
	.int 9127h,4000h;150度
	.int 87b8h,0d43ah;200度 
	.int 9127h,0c000h;210度
	.int 0d43ah,87b8h;250度
	.int 1638h,81f3h;280度
	.int 5247h,9df3h;310度
	.int 7848h,0d43ah;340度
	
	
	.int 1999h,0ccch ;3
	.int 0ccch,1999h;1
	.int 0f334h,0ccch;5
	.int 0f334h,0f334h;4
	.int 0ccch,0e667h;6
	.int 1999h,0f334h;;2
	.bss vdcint,1	
	.bss pwmt,1
	.bss number,1
	.bss direction,1
	.bss step,1
	.bss valfref,1;
	.bss vbltref,1;
	.bss datacmpr1,1
	.bss datacmpr2,1
	.bss datacmpr3,1
	.bss x,1
	.bss y,1
	.bss z,1 
	.bss sector,1
	.bss temp,1
	.bss temp1,1
	.bss temp2,1
	.bss temp3,1 
	.bss signa,1
	.bss signb,1
	.bss signc,1
	.bss error,1
	;.bss error1,1
	.bss storest0,1
	.bss storest1,1
	.bss adresult,1
	.bss sw_status,1
	.bss led_status,1	
		
	.def start
	.def PHANTOM
    .def GISR2 
    .def GISR3 
    .def STABLE
imr		.set 0004h
ifr		.set 0006h
;****I/O
swithes	.set 0008h
leds	.set 000ch
dac0	.set 0000h
dac1	.set 0001h
dac2	.set 0002h
dac3	.set 0003h
dac_update .set 0004h
wsgr	.set 0ffffh
;****dp:e0h
syscr	.set 7018h
syssr	.set 701ah
sysivr	.set 701eh

wdcntr	.set 7023h
wdkey	.set 7025h

wdcr	.set 7029h
ckcr0	.set 702bh
ckcr1	.set 702dh

adctrl1	.set 7032h
adctrl2	.set 7034h
adcfifo1	.set 7036h 
adcfifo2	.set 7038h
;*****dp:e1h
ocra	.set 7090h
ocrb	.set 7092h
;*****dp:e8h
gptcon	.set 7400h
t1cnt	.set 7401h
t1cmpr	.set 7402h
t1pr	.set 7403h
t1con	.set 7404h
t2cnt	.set 7405h
t2cmpr	.set 7406h
t2pr	.set 7407h
t2con 	.set 7408h

t3con 	.set 740ch
comcon 	.set 7411h

actr	.set 7413h
sactr	.set 7414h
dbtcon	.set 7415h
cmpr1	.set 7417h
cmpr2	.set 7418h
cmpr3	.set 7419h
scmpr1	.set 741ah
scmpr2	.set 741bh
scmpr3	.set 741ch
capcon	.set 7420h  

evimra	.set 742ch
evimrb	.set 742dh
evimrc	.set 742eh
evifra	.set 742fh
evifrb	.set 7430h
evifrc	.set 7431h
evivra	.set 7432h
evivrb	.set 7433h
evivrc	.set 7434h

kick_dog	.macro
	ldp #0e0h
	splk #05555h,wdkey
	splk #0aaaah,wdkey
	ldp #4h
	.endm 
t2timer		.macro
	ldp #232;e8h
	
	;splk #0fffh,evifra;evivra
	;splk #0fffh,evifrb;aevivrb	
	;splk #200h,evimra
	;splk #06h,evimrb
	
	
	splk #0h,t2cnt
	splk #0a002h,t2con
	;splk #0a042h,t2con
	ldp #4
	lacc pwmt
	ldp #t2cmpr>>7
	;mar ar0
	;lar ar0,#pwmt
	;ldp #t2pr>>7
	;lacc #0
	;lacc *+
	;sfr 
	sacl t2cmpr
	;splk #0,t2cnt
	ldp #232
	splk #6ah,gptcon
					
	splk #0a042h,t2con
	.endm
tabcon	.macro
	clrc sxm
	lacc pwmt
    sub temp1
    sub temp2
	sfr
	sfr
	sacl signa ; taon    
	lacl temp1
	sfr
	add signa
	sacl signb ;tbon
	lacl temp2
	sfr
	add signb
	sacl signc ;tcon
	.endm

	.text
start:	
;**** decide sector number value begin****	
    
    ldp #0
	lacc ifr
	splk #3fh,ifr
	splk #02h,imr
	
	ldp #232;e8h
	splk #0fffh,evifra;evivra
	splk #0fffh,evifrb;aevivrb	
	splk #0080h,evimra
	splk #02h,evimrb
	
	splk #56h,t2cmpr
	
	ldp #0e0h
	splk #002fh,wdcr
	kick_dog

	ldp #ckcr0>>7
	splk #00c3h,ckcr0 ;00h
	splk #00bbh,ckcr1 ;60h
	splk #40c0h,syscr; out cpuclk ;10mhz

	
	;b start
	ldp #ocra>>7
	splk #7f00h,ocra;1800	
    
    ldp #4
    splk #0,number
    
    ;splk #0122h,vdcint
    ;splk #01f4h,pwmt
    splk #0,step
    splk #0,direction
mainrution:    
    splk #4,temp
    out temp,wsgr
    clrc sxm
    kick_dog
	in sw_status,swithes
    
    ;lacc sw_status
    bit sw_status,8 
    ;sub #1
    bcnd pwmf2,ntc
   	ldp #4
	splk #4251,vdcint  ;2.3khz, 9hz
    splk #8000,pwmt
    splk #0,direction
	clrc intm		
    call pwmout
    ;t2timer                  
    idle
    call svpwm
    b mainrution    
pwmf2
    bit sw_status,9
    bcnd pwmf3,ntc
    ldp #4
	splk #2070,vdcint ;4khz,16hz
    splk #4000,pwmt
	;	splk #1135,vdcint
    ;splk #1953,pwmt
	;t2timer
	splk #0,direction
	clrc intm
    call pwmout
    idle 
    call svpwm
    b mainrution
pwmf3
    bit sw_status,10
    bcnd pwmf4,ntc
    ldp #4
	splk #1000,vdcint ;7.3khz,28hz
    splk #2000,pwmt
	;	splk #1135,vdcint
    ;splk #1953,pwmt
	;t2timer
	splk #0,direction
	clrc intm
    call pwmout
    idle 
    call svpwm
    b mainrution
pwmf4               ;direction=1,
    bit sw_status,11
    
    bcnd pwmf5,ntc
   	ldp #4
	splk #4251,vdcint  ;2.3khz, 9hz
    splk #8000,pwmt
	splk #1,direction
	clrc intm		
    call pwmout
    ;t2timer                  
    idle
    call svpwm
    b mainrution
pwmf5    
    call adchange
    ldp #4
    lacl adresult
    sacl led_status
    out adresult,dac0
    out adresult,dac_update
    out led_status,leds
	b start
	;splk #0,number
	;lar ar1,#table1


start1:
	kick_dog
	clrc intm
	idle
	;call pwmout
	;lacc #0fh
	;call delay
    b start1
    
    
    
    
    
    
    
    
    
    
    
;******************
* subroutines      ;
;******************
;***------
delay
	sub #1
    bcnd delay,neq
    retc unc
;***--------    
adchange      ;a/d conversion
	ldp #adctrl1>>7
	splk #4100h,adctrl1
    splk #0005h,adctrl2 
	splk #7804h,adctrl1;1804
adconversion
	bit adctrl1,8
	bcnd adconversion,tc
	lacc adcfifo1,10
	ldp #4
	sach adresult
	retc unc
;*****------
svpwm	
	ldp #4
	setc sxm
	spm 1 
	setc ovm      
	
	splk #1,signa
	splk #1,signb
	splk #1,signc
	bit vbltref,0
	bcnd jump1,ntc
	splk #0,signa
jump1:
	mar *,ar0
	lar ar0,#halfsqrt3
	lt *
	mpy valfref
	pac
	;sfl
	sach temp
	lacc #0
	lacc vbltref
	sfr
	sacl temp1
	lacc temp
	sub temp1
	sacl temp
	bit temp,0
	bcnd jump2,ntc
	splk #0,signb
jump2:
	lt halfsqrt3
	mpy valfref
	pac        
	;sfl
	neg 
	sach temp
	lacc #0
	lacc vbltref
	sfr
	sacl temp1
	lacc temp
	sub temp1
	sacl temp
	bit temp,0
	bcnd jump3,ntc
	splk #0,signc
jump3:
	lacl signb
	sfl
	add signc,2
	add signa
	sacl sector
;**** decide sector number value end****	
;****calculate	x y z value begin****
	ldp #4
	lacc #0
	setc sxm
	spm 1
    mar *,ar0
	lar ar0,#halfsqrt3
	lt *
	mpy vbltref
	pac
	sach temp     
	lt temp
	mpy vdcint
	pac
	sach temp1

	lar ar0,#half32
	lt *
	mpy valfref
	pac
	sach temp     
	lt temp
	mpy vdcint
	pac
	sach temp2,1
	
	lacl temp1
	sacl x,1
	lacl temp1
	add temp2
	sacl y
	lacl temp1
	sub temp2
	sacl z
;****calculate	x y z value end****	

	splk #0,error
	lacl sector
	sub #1
	bcnd sectorno2,neq
	lacl z
	sacl temp1
	lacl y
	sacl temp2
    
    call compare_adjust
	
    tabcon
    lacl signb
	sacl datacmpr1
	lacl signa
	sacl datacmpr2
	lacl signc
	sacl datacmpr3	
	b compareend
sectorno2:
	lacl sector
	sub #2
	bcnd sectorno3,neq
	lacl y
	sacl temp1
	lacl x
	neg 
	sacl temp2
	
	call compare_adjust
	
    tabcon
    lacl signa
	sacl datacmpr1
	lacl signc
	sacl datacmpr2
	lacl signb
	sacl datacmpr3	
	b compareend
sectorno3:
	lacl sector
	sub #3
	bcnd sectorno4,neq
	lacl z
	neg
	sacl temp1
	lacl x
	sacl temp2
	
	call compare_adjust
	
    tabcon
    lacl signa
	sacl datacmpr1
	lacl signb
	sacl datacmpr2
	lacl signc
	sacl datacmpr3	
	b compareend
sectorno4:
	lacl sector
	sub #4
	bcnd sectorno5,neq
	lacl x
	neg
	sacl temp1
	lacl z
	sacl temp2
    
    call compare_adjust
	
    tabcon
    lacl signc
	sacl datacmpr1
	lacl signb
	sacl datacmpr2
	lacl signa
	sacl datacmpr3	
	b compareend
sectorno5:
	lacl sector
	sub #5
	bcnd sectorno6,neq
	lacl x
	sacl temp1
	lacl y
	neg
	sacl temp2   
		
	call compare_adjust
	
    tabcon
    lacl signc
	sacl datacmpr1
	lacl signa
	sacl datacmpr2
	lacl signb
	sacl datacmpr3	
	b compareend
sectorno6:
	lacl sector
	sub #6
	bcnd sectorerror,neq
	lacl y
	neg
	sacl temp1
	lacl z
	neg
	sacl temp2