********BLDC for Ma.Changshan's PCB******	
********PWM1~6*************
;注意：其中PWM在cpld中進(jìn)行了反相
********CAP1~3**************
	.include "LF2407.H"
	.def _c_int0
	
	.bss	X1,1
	.bss	IMAX,1
	.bss	IMIN,1
	;------------
	.bss 	SPEEDG,1
	.bss 	VCOUNT,1
	.bss 	N,1
	.bss 	KP,1
	.bss 	KI,1
	.bss 	EN,1
	.bss 	EN_1,1
	.bss 	X2,1
	.bss 	X3,1
	;------------
HUANXIANG
	;.word 07FDh,0FD7h,07DFh,0D7Fh,0F7Dh,0DF7h,0FFFh
	.word  0802H,0028H,0820H,0280H,0082H,0208H,0000H  ;CPLD is useless
	;.word	#0FFFH,#07FDH,#0FD7H,#07DFH,#0D7FH,#0F7DH,#0DF7H
	;ACTRA(11~0)--CMP6CTL...CMP1CTL	
	;00	forced low
	;01	active low
	;10	active high
	;11	forced high
	;注意：在CPLD中進(jìn)行了反相，所以實(shí)際為PWM高有效
	.sect "vectors"
REVECT B _c_int0
INT1 B PHANTOM
INT2 B PHANTOM	
INT3 B PHANTOM
INT4 B CAPINT	
INT5 B PHANTOM
INT6 B ADCINT	
	.text
_c_int0:
	LDP #0H
	CLRC CNF 	
	CLRC OVM
	CLRC SXM
	SETC INTM
	;initial
	;system initial
	LDP #0H
	SPLK #0H,IMR
	SPLK #0FFFFH,IFR
	MAR *,AR1
	LAR AR1,#300H
	LDP #0E0H
	SPLK #208DH,SCSR1
	SPLK #006DH,SCSR2
	SPLK #006FH,WDCR		;WATCHDOG is disable	
;----------------------
	LDP #4H
	SPLK #0H,X1		
	SPLK #25H,IMAX			;
	SPLK #20H,IMIN
	SPLK	#200H,SPEEDG
	SPLK 	#0H,VCOUNT
	SPLK 	#0H,N
	SPLK 	#10H,KP
	SPLK 	#10H,KI
	SPLK 	#0H,EN
	SPLK 	#0H,EN_1
	SPLK	#0H,X2
	SPLK 	#0H,X3
;------------------------	

;IO 
	LDP #0E1H
	SPLK #0FF8H,MCRA	;PWM1~6 ;CAP1_3
	
;EVA
	LDP #0E8H
	SPLK #6145H,GPTCONA	;T1 period enable ADC
	SPLK #1046H,T1CON
	SPLK #3999,T1PR	;AD 10KHZ
	SPLK #0555H,ACTRA	;pwm1~6 low efficiency
	SPLK #0,CMPR1
	SPLK #0,CMPR1
	SPLK #0,CMPR3
	SPLK #0A000H,COMCONA
	SPLK #0FECH,DBTCONA	
	SPLK #0,T1CNT
	
	SPLK #0,T2CNT
	SPLK #0F4FH,T2CON	;X/128		
;CAP
	SPLK #30FFH,CAPCONA	; T2 count for cap time 
	SPLK #00FFH,CAPFIFOA
	SPLK #0FFFFH,T2PR

;ADC	
	LDP #0E1H
	SPLK #4000H,ADCTRL1
	NOP
	SPLK #2932H,ADCTRL1	;啟動(dòng)/停止 模式
	SPLK #4000H,ADCTRL2
	NOP
	SPLK #0504H,ADCTRL2	;enable EVA start
	SPLK #05H,MAXCONV	;IA,IB,IC
	SPLK #44EEH,CHSELSEQ1	
	SPLK #00DDH,CHSELSEQ2  	
;CLRAE INTS AND ENABLE INTS
	LDP	#0E8H
	LACC	EVAIFRA
	SACL	EVAIFRA	
	LACC	EVAIFRB
	SACL	EVAIFRB
	LACC	EVAIFRC
	SACL	EVAIFRC
	SPLK	#0000H,EVAIMRA	
	SPLK	#0000H,EVAIMRB	
	SPLK	#0007H,EVAIMRC	;enable capint
	LDP	#0EAH
	LACC	EVBIFRA
	SACL	EVBIFRA
	LACC	EVBIFRB
	SACL	EVBIFRB
	LACC	EVBIFRC
	SACL	EVBIFRC
	SPLK	#0000H,EVBIMRA
	SPLK	#0000H,EVBIMRB
	SPLK	#0000H,EVBIMRC	
	;ENABLE INTS
	LDP #0H
	LACL IMR
	OR  #0028H	;enable INT4 AND INT6
	SACL IMR
	SPLK #0FFFFH,IFR
	CLRC INTM
;INITIAL POSTION JUDGEING
 	LDP     #0E1H
	LACC    PADATDIR
	RPT     #2
	SFR
	AND     #0007H
	LDP     #4H
	ADD     #HUANXIANG
	LDP     #0E8H
	TBLR    ACTRA
;------------------------------------
WAIT:
	NOP
	B WAIT
;------------------------------------
CAPINT:
	INT_PUSH
	LDP #0E1H
	LACC PADATDIR
	RPT #2
	SFR
	AND #0007H
	ADD #HUANXIANG
	LDP #0E8H
	TBLR ACTRA
;SPEED CALCULATE
	

	INT_POP
	NOP
	CLRC INTM
	RET

;------------------------------------
ADCINT:
	INT_PUSH
	;--------------- speed rgular or not
	LDP #4
	LACC N
	ADD #1
	SACL N	;N++
	LACC N
	SUB #9
	BCND  IREGULAR,LEQ 
;SPEEDREGULAR
	SPLK #0,N	;CLEAR N,from 0 to 9
	;SPEED PI ???????????????????????????????????????????		
	;---------------
IREGULAR	
	LDP #0E1H
	LACC ADC_RESULT0,10	;sample IA
	ADD ADC_RESULT1,10
	SFR
	LDP #4
	SACH X1	;IA BANG_BANG control
	CALL BANG_BANG
	LDP #4
	LACL X1
	LDP #0E8H
	SACL CMPR1
	
	LDP #0E1H
	LACC ADC_RESULT2,10	;sample IB
	ADD ADC_RESULT3,10
	SFR
	LDP #4 	
	SACH X1	;IB BANG_BANG control
	CALL BANG_BANG
	LDP #4
	LACL X1
	LDP #0E8H
	SACL CMPR2
		
	LDP #0E1H
	LACC ADC_RESULT4,10	;sample IC
	ADD ADC_RESULT5,10
	SFR
	LDP #4	
	SACH X1	;IB BANG_BANG control
	CALL BANG_BANG
	LDP #4
	LACL X1
	LDP #0E8H
	SACL CMPR3	

	LDP #0E1H
	LACL ADCTRL2
	OR #0202H
	SACL ADCTRL2
	;SPLK #2600H,ADCTRL2 	;
	INT_POP
	CLRC INTM
	RET

;--------------------------------------
BANG_BANG:
;電流滯環(huán)控制
	LDP #4
	LACL X1
	SUB IMAX
	BCND I_BIG,GEQ
	LACL X1
	SUB IMIN
	BCND I_SAMLL,LEQ
	B RETURN
;FOR MACHANGSHAN	
I_BIG
	LDP #4	;由于在CPLD中進(jìn)行了反相，所以作此更改
	SPLK #100,X1	;not 0 for IPM
	B RETURN
I_SAMLL
	LDP	#4
	SPLK #3600,X1	;3999 is the period -max duty(10kHz)
RETURN 
	RET
;-------------------------------------------------	
;FOR LIJICHEN
;I_BIG
;	LDP #4	;由于在CPLD中進(jìn)行了反相，所以作此更改
;	SPLK #3600,X1	;not 0 for IPM
;	B RETURN
;I_SAMLL
;	LDP	#4
;	SPLK #100,X1	;3999 is the period -max duty(10kHz)
;RETURN 
;	RET
;------------------------------------------------
PHANTOM:
	CLRC INTM 
	NOP
	RET
;--------------------------------------------------------------	
	.end