.include	"f2407.h"
	
KP	.set	001
KI	.set	001
	.bss	SCI_FLAG,1
	.bss	COMP1,1
	.bss	COMP2,1
	.bss	COMP3,1
	.bss	COMP4,1
	.bss	SPEED_REF1,1
	.bss	MOTOR1_DIRk,1
	.bss	MOTOR1_DIRk_1,1
	.bss	SPEED_REF2,1
	.bss	MOTOR2_DIRk,1
	.bss	MOTOR2_DIRk_1,1
	.bss	SPEED_REF3,1
	.bss	MOTOR3_DIRk,1
	.bss	MOTOR3_DIRk_1,1
	.bss	SPEED_REF4,1
	.bss	MOTOR4_DIRk,1
	.bss	MOTOR4_DIRk_1,1	
	.bss	LOOK_FLAG,1
	.bss	READ_FLAG,1
	.bss	STACK,6
	
	.def	_c_int0,RXINT,PHANTOM
	
	.text
;------------------------------初始化程序-------------------------------------
_c_int0: 
		CLRC	CNF    
        CLRC	OVM                
        SETC	SXM                ;符號擴展
        SETC	INTM               ;關中斷                     
              
        LDP		#0E0H				;系統配置初始化
		SPLK	#68H,WDCR   		;不用看門狗
		SPLK    #0001H,SCSR1
		SPLK	#0044H,SCSR1

		CALL	DRAM_INIT 
		CALL	SCI_INIT
		CALL	EV_INIT
		CALL	IO_INIT
		
		LDP     #0E8H
		LACC    EVAIFRC
        SACL    EVAIFRC
        LDP     #0EAH
        LACC    EVBIFRC
        SACL    EVBIFRC
        LDP     #0E8H
        SPLK    #0,EVAIMRA
        SPLK    #0,EVAIMRB
        SPLK    #3,EVAIMRC         	;允許捕捉1,2中斷,其它關閉
        LDP     #0EAH
        SPLK    #0,EVBIMRA
        SPLK    #0,EVBIMRB
        SPLK    #3,EVBIMRC         	;允許捕捉4,5中斷,其它關閉        
        
        LDP     #0
        SPLK	#0010H,IMR		   	;允許INT1(RX)中斷
        LACC    IFR                	;清標志
        SACL    IFR        
        CLRC    INTM               	;開總中斷
;--------------------------------------------------------------------------------        
LOOP:	
		LDP		#0
		BIT		SCI_FLAG,BIT0
		BCND	SPEED1_CAL,TC
		BIT		LOOK_FLAG,BIT0
		BCND	CLR_FLAG,TC
		BIT		READ_FLAG,BIT0
		BCND	CLR_FLAG,TC
		B		WAIT
SPEED1_CAL:
		LDP		#4H
		LACC	5H
		LDP		#0H
		SACL	7FH
		LDP		#4
		BIT		5H,BIT7
		BCND	MOTOT1_0,NTC
		LACC	5H
		SUB		#80H
		SACL	5H
		LT		5H
		MPY		#15
		LTP		6H
		LDP		#0
		DMOV	66H
		SPLK	#01,MOTOR1_DIRk
		SACL	SPEED_REF1
		B       SPEED2_CAL
MOTOT1_0:
		LT		5H
		MPY		#15
		LTP		6H
		LDP		#0
		DMOV	66H
		SPLK	#00,MOTOR1_DIRk
		SACL	SPEED_REF1
SPEED2_CAL:
		LDP		#4H
		LACC	6H
		LDP		#0H
		SACL	7EH
		LDP		#4
		BIT		6H,BIT7
		BCND	MOTOT2_0,NTC
		LACC	6H
		SUB		#80H
		SACL	6H
		LT		6H
		MPY		#15
		LTP		7H
		LDP		#0
		DMOV	69H
		SPLK	#01,MOTOR2_DIRk
		SACL	SPEED_REF2
		B       SPEED3_CAL
MOTOT2_0:
		MPY		#15
		LTP		7H
		LDP		#0
		DMOV	69H
		SPLK	#00,MOTOR2_DIRk
		SACL	SPEED_REF2
SPEED3_CAL:
		LDP		#4H
		LACC	7H
		LDP		#0H
		SACL	7DH
		LDP		#4
		BIT		7H,BIT7
		BCND	MOTOT3_0,NTC
		LACC	7H
		SUB		#80H
		SACL	7H
		LT		7H
		MPY		#15
		LTP		8H
		LDP		#0
		DMOV	6CH
		SPLK	#01,MOTOR3_DIRk
		SACL	SPEED_REF3
		B       SPEED4_CAL
MOTOT3_0:
		MPY		#15
		LTP		8H
		LDP		#0
		DMOV	6CH
		SPLK	#00,MOTOR3_DIRk
		SACL	SPEED_REF3
SPEED4_CAL:
		LDP		#4H
		LACC	8H
		LDP		#0H
		SACL	7CH
		LDP		#4
		BIT		8H,BIT7
		BCND	MOTOT4_0,NTC
		LACC	8H
		SUB		#80H
		SACL	8H
		LT		8H
		MPY		#15
		LTP		5H
		LDP		#0
		DMOV	6FH
		SPLK	#01,MOTOR4_DIRk
		SACL	SPEED_REF4
		B       DIR_CFG
MOTOT4_0:
		MPY		#15
		LTP		5H
		LDP		#0
		DMOV	6FH
		SPLK	#00,MOTOR4_DIRk
		SACL	SPEED_REF4
;--------------------------------------------------------------------------------
DIR_CFG:
		LACC	MOTOR1_DIRk
		SUB		MOTOR1_DIRk_1
		BCND	MOTOR1_STOP,NEQ
DIR2_JUDG:
		LDP		#0
		LACC	MOTOR2_DIRk
		SUB		MOTOR2_DIRk_1
		BCND	MOTOR2_STOP,NEQ
DIR3_JUDG:
		LDP		#0
		LACC	MOTOR3_DIRk
		SUB		MOTOR3_DIRk_1
		BCND	MOTOR3_STOP,NEQ
DIR4_JUDG:
		LDP		#0
		LACC	MOTOR4_DIRk
		SUB		MOTOR4_DIRk_1
		BCND	MOTOR4_STOP,NEQ
		CALL	DUTY_CAL
		CALL	PWM_OUT
		B       CLR_FLAG
		
MOTOR1_STOP:
		LDP		#0E8H
		SPLK	#2000,T1CMPR				;控制MOTOR1的PWM
		CALL	DELAY_ROUT
		B       DIR2_JUDG
MOTOR2_STOP:
		LDP		#0E8H
		SPLK	#2000,CMPR1					;控制MOTOR2的PWM
		CALL	DELAY_ROUT
		B       DIR3_JUDG
MOTOR3_STOP:
		LDP		#0E8H
		SPLK	#2000,CMPR2					;控制MOTOR3的PWM
		CALL	DELAY_ROUT
		B       DIR4_JUDG
MOTOR4_STOP:
		LDP		#0E8H
		SPLK	#2000,CMPR1					;控制MOTOR4的PWM
		CALL	DELAY_ROUT						;延時
		CALL	DUTY_CAL
		CALL	PWM_OUT
;--------------------------------------------------------------------------------
CLR_FLAG:		
		LDP		#4
		LACC	201H
		SACL	211H
		LDP		#0H
		LACC	7FH
		LDP		#4H
		SACL	214H
		LDP		#0H
		LACC	7EH
		LDP		#4H
		SACL	215H
		LDP		#0H
		LACC	7DH
		LDP		#4H
		SACL	216H
		LDP		#0H
		LACC	7CH
		LDP		#4H
		SACL	217H
		LACC	#0H
		SACL	218H
		MAR		*,AR0
		LAR		AR0,#210H
		ADRK	#10
		MAR		*,AR4
		LACC	*+
		CMPR	00
		BCND	TXD_DATA_END,TC		;判斷是否發送完畢
		MAR		*,AR2
		SACL	*,AR4		
XMIT_RDY:
		LDP		#0E0H
		BIT		SCICTL2,BIT7
		BCND	XMIT_RDY,NTC		;判斷發送器是否為空
		B		CLR_FLAG
TXD_DATA_END:
		MAR		*,AR4
		LAR		AR4,#210H
		MAR		*,AR3
		LAR		AR3,#200H
		LDP		#0
		SPLK	#00H,SCI_FLAG
		SPLK	#00H,READ_FLAG
		SPLK	#00H,LOOK_FLAG	
WAIT:
	    NOP
		B		LOOP		
PHANTOM:   
        CLRC    INTM
        RET
;---------------------------------------------------------------------------------
RXINT:	
		MAR		*,AR5
		MAR		*+
		SST		#1,*+
		SST		#0,*
		LDP		#0E0H
		LACC	PIVR
RX_DATA:
		MAR		*,AR1
		LACC	*,AR3				;SCIRXBUF->ACC
		SACL	*+,AR0					;ACC低位->RXD存儲區
		LAR		AR0,#0200H
		LDP		#4
		LACC	0H
		SUB		#04
		BCND	CON_SAVE1,EQ
		LACC	0H
		SUB		#05
		BCND	CON_SAVE,EQ		
		ADRK	#9
		B  		DATA_SAVE
CON_SAVE:
		ADRK	#4
		MAR		*,AR3
		CMPR	00
		BCND	SCI_RX_END,NTC
		LAR		AR3,#0200H
		LDP		#0E0H
		SPLK	#0023H,SCICTL1
		B  		SCI_RX_END
CON_SAVE1:
		LDP		#4H
		LACC	1H
		SUB		#06H
		BCND	CON_SAVE,EQ
		ADRK	#11
		MAR		*,AR3
		CMPR	00
		BCND	SCI_RX_END,NTC
		LAR		AR3,#0200H
		LDP		#0E0H
		SPLK	#0023H,SCICTL1
		B  		SCI_RX_END
DATA_SAVE:		
		MAR		*,AR3
		CMPR	00
		BCND	SCI_RX_END,NTC
		LDP		#4
		LACC	4H
		SUB		#00H
		BCND	SCI_W_FLAG,EQ
		LACC	4H
		SUB		#01H
		BCND	SCI_S_FLAG,	EQ
		LACC	4H
		SUB		#02H
		BCND	RE_CUR,NEQ
		LDP		#0
		SPLK	#01H,READ_FLAG
		B		RE_CUR
SCI_S_FLAG:
		LDP		#0
		SPLK	#01H,LOOK_FLAG
		B		RE_CUR
SCI_W_FLAG: 	
		LDP		#0
		SPLK	#01H,SCI_FLAG		
RE_CUR:	
		LAR		AR3,#0200H
		LDP		#0E0H
		SPLK	#0023H,SCICTL1
SCI_RX_END:	
		MAR		*,AR5
		LST		#0,*-
		LST		#1,*-
			
		CLRC	INTM
		RET
;-----------------------------------------------------------------------------
DUTY_CAL:
		LDP		#0
		LACC	SPEED_REF1
		SUB		#2000
		NEG
		SACL	COMP1
		LACC	SPEED_REF2
		SUB		#2000
		NEG
		SACL	COMP2
		LACC	SPEED_REF3
		SUB		#2000
		NEG
		SACL	COMP3
		LACC	SPEED_REF4
		SUB		#2000
		NEG
		SACL	COMP4
		RET
PWM_OUT:
		LDP		#0
		LACC	MOTOR1_DIRk,4
		ADD		MOTOR2_DIRk,2
		ADD		MOTOR3_DIRk,1
		ADD		MOTOR4_DIRk,3
		OR		#1E00H
		LDP		#0E1H
		SACL	PEDATDIR
		LDP		#0
		LACC	COMP1
		LDP		#0E8H
		SACL	CMPR2
		LDP		#0
		LACC	COMP2
		LDP		#0E8H
		SACL	CMPR3
		LDP		#0
		LACC	COMP3
		LDP		#0E8H
		SACL	CMPR1
		LDP		#0
		LACC	COMP4
		LDP		#0E8H
		SACL	T1CMPR
		NOP
		NOP
		RET
DELAY_ROUT:
		LACC	#6000
		
DELAY1:	SUB		#1
		RPT		#255
		NOP
		BCND	DELAY1,NEQ
		
		RET
;--------------------------------------------------------------------------------        
DRAM_INIT:
		MAR		*,AR2			   ;變量及存儲單元初始化
        LAR		AR2,#0060H
        SPLK	#0,*+
        SPLK	#2000,*+		   ;初始占空比	
        SPLK	#2000,*+
        SPLK	#2000,*+
        SPLK	#2000,*+
        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,*+
        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,*   
        MAR		*,AR5
		LAR		AR5,#STACK
	
		MAR		*,AR4
		LAR		AR4,#210H
		SPLK	#02H,*+
		SPLK	#00H,*+
		SPLK	#06H,*+
		SPLK	#04H,*+
		SPLK	#0H,*+
		SPLK	#0H,*+
		SPLK	#0H,*+
		SPLK	#0H,*+
		SPLK	#0H,*+
		SPLK	#0H,*+
		SPLK	#0H,*+
		SPLK	#0H,*+
		SPLK	#0H,*+
		SPLK	#0H,*+
		SPLK	#0H,*+
		SPLK	#0H,*+
		SPLK	#0H,*
		LAR		AR4,#210H
CLC_RAM: 
	    MAR		*,AR3
		LAR		AR3,#200H
		SPLK	#0H,*+
		SPLK	#0H,*+
		SPLK	#0H,*+
		SPLK	#0H,*+
		SPLK	#0H,*+
		SPLK	#0H,*+
		SPLK	#0H,*+
		SPLK	#00,*+
		SPLK	#0H,*+
		SPLK	#0H,*+
		SPLK	#0H,*+
		SPLK	#0H,*+
		SPLK	#0H,*+
		SPLK	#0H,*+
		SPLK	#0H,*+
		SPLK	#0H,*
		LAR		AR3,#200H
	    RET
SCI_INIT:
		LDP		#0E0H
		SPLK	#0030H,SCIPRI
		SPLK	#0007H,SCICCR
		SPLK	#0003H,SCICTL1
		SPLK	#0002H,SCICTL2
		SPLK	#0002H,SCIHBAUD
		SPLK	#0007H,SCILBAUD
		SPLK	#0023H,SCICTL1
		LAR		AR1,#SCIRXBUF
		LAR		AR2,#SCITXBUF
		LAR		AR3,#0200H		;接收數據存儲區
		LAR		AR4,#0210H		;發送數據存儲區
		RET
EV_INIT:
		LDP		#0E8H			
		SPLK	#0FFFFH,T2PR    	;GPT2提供CAP1/2時基,最大周期計時0.2S
        SPLK    #0000H,T2CNT 		;計數初值
        SPLK    #1740H,T2CON    	;連續增計數,1/128分頻,T1啟動,內部時鐘
        LDP		#0EAH
        SPLK	#0FFFFH,T4PR    	;GPT4提供CAP4/5時基,最大周期計時0.2S
        SPLK    #0000H,T4CNT 		;計數初值
        SPLK    #1740H,T4CON    	;連續增計數,1/128分頻,T1啟動,內部時鐘
        SPLK    #0000H,GPTCONB
        
        LDP		#0E8H
		SPLK    #2000,T1PR       	;PWM設置,周期200us,頻率5K
        SPLK    #0000H,T1CNT
        SPLK	#2000,T1CMPR		;T1PWM占空比
        SPLK    #0555H,ACTRA    	;PWM1～6全部陀行