********BLDC for Ma.Changshan's PCB******	
********PWM1~6*************
;注意：其中PWM在cpld中進行了反相
********CAP1~3**************
	.include "LF2407.H"
	.def _c_int0
	
	.bss	X1,1
	.bss	IG,1
	;------------
	.bss 	SPEEDG,1
	.bss 	VCOUNT,1
	.bss 	KP,1
	.bss 	KI,1
	.bss 	STACK,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中進行了反相，所以實際為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
	LAR	AR2,#0300H
	LAR AR1,STACK
	LDP #0E0H
	SPLK #208DH,SCSR1
	SPLK #006DH,SCSR2
	SPLK #006FH,WDCR		;WATCHDOG is disable	
;----------------------
	LDP #4H
	SPLK #0H,X1		
	SPLK #25H,IG		
	SPLK	#200H,SPEEDG
	SPLK 	#0H,VCOUNT
	SPLK 	#10H,KP
	SPLK 	#10H,KI
;------------------------	

;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	;啟動/停止 模式
	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-----------
	MAR 	*,AR2
	LACL 	T2CNT
	SACL 	*+
	SUB 	*+
	SACL 	*-
	ADD 	*
	SACL 	*-
;------------------------
	INT_POP
	NOP
	CLRC INTM
	RET

;------------------------------------
ADCINT:
	INT_PUSH
	;--------------- speed rgular or not
	LDP #4
	LACC VCOUNT
	SUB #125
	BCND  IREGULAR,LEQ 
	SPLK #0,VCOUNT
;SPEEDREGULAR
	;SPEED PI 
	MAR 	*,AR2
	LAR	 	AR2,#0302H 	
	LDP 	#4
	SPLK 	#0,VCOUNT	
	CLRC SXM
	LACC #0
	OR 	#0FFFFH		
	RPT 	#15
	SUBC 	*		
	AND		#0FFFFH		
	SUB 	#SPEEDG
	NEG				
	SETC 	SXM
	LAR 	AR2,#0303H	
	SACL 	*+
	SUB 	*+
	SACL  	*
	LT 		*
	MPY 	KP
	LAR 	AR2,#0303H
	LTP 	*		
	MPY 	KI		
	LTD 	*		
	ADD 	IG,4	
	SFR				
	SFR
	SFR
	SFR
	LDP #4
	SACL 	IG
	LAR 	AR2,#0303H  
	B ADCINTEND			
	;---------------
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	
ADCINTEND
	LDP #0E1H
	LACL ADCTRL2
	OR #0202H
	SACL ADCTRL2
	;SPLK #2600H,ADCTRL2 	;
	INT_POP
	CLRC INTM
	RET

;--------------------------------------
BANG_BANG:
;電流滯環(huán)控制
	LDP #4
	LACC VCOUNT
	ADD #1
	SACL VCOUNT
	LACL X1
	SUB IG
	BCND I_BIG,GEQ
	LACL X1
	ADD #1H
	SUB IG
	BCND I_SAMLL,LEQ
	B RETURN
;FOR MACHANGSHAN	
I_BIG
	LDP #4	;由于在CPLD中進行了反相，所以作此更改
	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