智能小车控制程序
XDEF Entry
ABSENTRY Entry
INCLUDE 'PWM.inc'
INCLUDE 'ECT.inc'
INCLUDE 'PORT.inc'
INCLUDE 'ATD.inc'
ROMStart EQU $4000
INTCR EQU $001E
F_FINISH EQU $2000
TCNT EQU $0044
C_LINE EQU $2002
C_POINT EQU $2003
P_SAVE EQU $2004
P_LINE EQU $2006
P_LINE2 EQU $2008
P_ROW EQU $200A
P_POSITION EQU $200C
P_YOU EQU $200E
P_YOU1 EQU $2010
P_COMPARE0 EQU $2012
COUNT10 EQU $2014
TEM EQU $2016
P_LINE1 EQU $2018
LEFT_S EQU $201A
RIGHT_S EQU $201B
ADD_NUM0 EQU $201C
ADD_SUM0 EQU $201E
COUNT_L EQU $2020
AVERAGE EQU $2021
TEM0 EQU $2023
ERROR EQU $2025
TCNT0 EQU $2027
TCNT1 EQU $2029
COMPARE0 EQU $202B
COMPARE EQU $202C
RIGHT1 EQU $202D
LEFT1 EQU $202E
M EQU $202F
M0 EQU $2030
LINE_CHA EQU $2031
POINT_CHA EQU $2032
SUDU01 EQU $2034
SUDU02 EQU $2035
SUDU03 EQU $2036
LIANXU_ERROR EQU $2037
MIN EQU $2038
MAX EQU $2039
E_HIGH EQU $203A
P_POINTL EQU $203C
P_POINTR EQU $203D
ADD_SUM EQU $203E
ADD_NUM EQU $2040
C_MIN EQU $2042
C_MAX EQU $2043
TEM10 EQU $2044
FIRST EQU $2046
LAST EQU $2047
FIRST_LINE EQU $2048
LAST_LINE EQU $2049
RESULT0 EQU $204A
RESULT1 EQU $204C
RESULT2 EQU $204E
RESULT3 EQU $2050
RESULT4 EQU $2052
RESULT5 EQU $2054
DD EQU $2056
DD1 EQU $2057
LINE_LINE_MAX EQU $2058
LIANXU EQU $2059
GAOSU EQU $205A
DDD EQU $205B
RESULT6 EQU $205D
RESULT7 EQU $205F
STIME_COUNT EQU $2061
STIME_COUNT0 EQU $2063
FLOW EQU $2065
FIRST0 EQU $2066
PERIOD1 EQU $2068
PERIOD2 EQU $206A
PERIOD EQU $206C
ZHUANJIAO EQU $206E
SUDU EQU $2070
ZHI_XIAN EQU $2072
WAN_XIAN EQU $2074
ORG $2080
CRRNT_INS RMB 2
FUZ_INS RMB $0A
FUZ_OUTS RMB 7
COG_OUT RMB 1
CRRNT_INS1 RMB 2
FUZ_INS1 RMB $0A
FUZ_OUTS1 RMB 7
COG_OUT1 RMB 1
ORG $B600
INPUT_MFS EQU *
FCB $0001,$001A,$00ff,$000E
FCB $000E,$002B,$000E,$000E
FCB $001F,$003C,$000E,$000E
FCB $0030,$004D,$000E,$000E
FCB $0041,$005C,$000E,$00ff
FCB $0001,$0032,$00ff,$0006
FCB $0019,$0055,$0006,$0006
FCB $003C,$0078,$0006,$0006
FCB $005F,$009B,$0006,$0006
FCB $0082,$00b4,$0006,$00ff
SGLTN_POS EQU *
FCB 60,90,120,130,140,165,210
INPUT_MFS1 FCB $0001,$0032,$00ff,$0006
FCB $0019,$0055,$0006,$0006
FCB $003C,$0078,$0006,$0006
FCB $005F,$009B,$0006,$0006
FCB $0082,$00b4,$0006,$00ff
FCB $0001,$0008,$00ff,$003f
FCB $0004,$000e,$003f,$003f
FCB $000a,$0014,$003f,$003f
FCB $0010,$001a,$003f,$003f
FCB $0016,$001F,$003f,$00ff
SGLTN_POS1 EQU *
FCB 01,40,70,90,110,140,178
RULE EQU *
FDB FUZ_INS+$0003,FUZ_INS+$0005,$FFFE,FUZ_INS+$000c,$FFFF
FDB FUZ_INS+$0003,FUZ_INS+$0006,$FFFE,FUZ_INS+$000d,$FFFF
FDB FUZ_INS+$0003,FUZ_INS+$0007,$FFFE,FUZ_INS+$000e,$FFFF
FDB FUZ_INS+$0003,FUZ_INS+$0008,$FFFE,FUZ_INS+$000f,$FFFF
FDB FUZ_INS+$0003,FUZ_INS+$0009,$FFFE,FUZ_INS+$0010,$FFFF
FDB FUZ_INS+$0004,FUZ_INS+$0005,$FFFE,FUZ_INS+$000d,$FFFF
FDB FUZ_INS+$0004,FUZ_INS+$0006,$FFFE,FUZ_INS+$000e,$FFFF
FDB FUZ_INS+$0004,FUZ_INS+$0007,$FFFE,FUZ_INS+$000f,$FFFF
FDB FUZ_INS+$0004,FUZ_INS+$0008,$FFFE,FUZ_INS+$0010,$FFFF
FDB FUZ_INS+$0004,FUZ_INS+$0009,$FFFE,FUZ_INS+$0010,$FFFF FDBFUZ_INS+$0002,FUZ_INS+$0005,$FFFE,FUZ_INS+$000b,$FFFF
FDB FUZ_INS+$0002,FUZ_INS+$0006,$FFFE,FUZ_INS+$000c,$FFFF
FDB FUZ_INS+$0002,FUZ_INS+$0007,$FFFE,FUZ_INS+$000d,$FFFF
FDB FUZ_INS+$0002,FUZ_INS+$0008,$FFFE,FUZ_INS+$000e,$FFFF
FDB FUZ_INS+$0002,FUZ_INS+$0009,$FFFE,FUZ_INS+$000f,$FFFF
FDB FUZ_INS+$0001,FUZ_INS+$0005,$FFFE,FUZ_INS+$000a,$FFFF
FDB FUZ_INS+$0001,FUZ_INS+$0006,$FFFE,FUZ_INS+$000b,$FFFF
FDB FUZ_INS+$0001,FUZ_INS+$0007,$FFFE,FUZ_INS+$000c,$FFFF
FDB FUZ_INS+$0001,FUZ_INS+$0008,$FFFE,FUZ_INS+$000d,$FFFF
FDB FUZ_INS+$0001,FUZ_INS+$0009,$FFFE,FUZ_INS+$000e,$FFFF
FDB FUZ_INS+$0000,FUZ_INS+$0005,$FFFE,FUZ_INS+$000a,$FFFF
FDB FUZ_INS+$0000,FUZ_INS+$0006,$FFFE,FUZ_INS+$000a,$FFFF
FDB FUZ_INS+$0000,FUZ_INS+$0007,$FFFE,FUZ_INS+$000b,$FFFF
FDB FUZ_INS+$0000,FUZ_INS+$0008,$FFFE,FUZ_INS+$000c,$FFFF
FDB FUZ_INS+$0000,FUZ_INS+$0009,$FFFE,FUZ_INS+$000d,$FFFF
RULE1 EQU *
FDB FUZ_INS1+$0003,FUZ_INS1+$0005,$FFFE,FUZ_INS1+$0010,$FFFF
FDB FUZ_INS1+$0003,FUZ_INS1+$0006,$FFFE,FUZ_INS1+$000F,$FFFF
FDB FUZ_INS1+$0003,FUZ_INS1+$0007,$FFFE,FUZ_INS1+$000E,$FFFF
FDB FUZ_INS1+$0003,FUZ_INS1+$0008,$FFFE,FUZ_INS1+$000E,$FFFF
FDB FUZ_INS1+$0003,FUZ_INS1+$0009,$FFFE,FUZ_INS1+$000E,$FFFF
FDB FUZ_INS1+$0004,FUZ_INS1+$0005,$FFFE,FUZ_INS1+$0010,$FFFF
FDB FUZ_INS1+$0004,FUZ_INS1+$0006,$FFFE,FUZ_INS1+$0010,$FFFF
FDB FUZ_INS1+$0004,FUZ_INS1+$0007,$FFFE,FUZ_INS1+$000F,$FFFF
FDB FUZ_INS1+$0004,FUZ_INS1+$0008,$FFFE,FUZ_INS1+$000E,$FFFF
FDB FUZ_INS1+$0004,FUZ_INS1+$0009,$FFFE,FUZ_INS1+$000E,$FFFF
FDB FUZ_INS1+$0002,FUZ_INS1+$0005,$FFFE,FUZ_INS1+$000D,$FFFF
FDB FUZ_INS1+$0002,FUZ_INS1+$0006,$FFFE,FUZ_INS1+$000D,$FFFF
FDB FUZ_INS1+$0002,FUZ_INS1+$0007,$FFFE,FUZ_INS1+$000D,$FFFF
FDB FUZ_INS1+$0002,FUZ_INS1+$0008,$FFFE,FUZ_INS1+$000D,$FFFF
FDB FUZ_INS1+$0002,FUZ_INS1+$0009,$FFFE,FUZ_INS1+$000D,$FFFF
FDB FUZ_INS1+$0001,FUZ_INS1+$0005,$FFFE,FUZ_INS1+$000A,$FFFF
FDB FUZ_INS1+$0001,FUZ_INS1+$0006,$FFFE,FUZ_INS1+$000B,$FFFF
FDB FUZ_INS1+$0001,FUZ_INS1+$0007,$FFFE,FUZ_INS1+$000C,$FFFF
FDB FUZ_INS1+$0001,FUZ_INS1+$0008,$FFFE,FUZ_INS1+$000C,$FFFF
FDB FUZ_INS1+$0001,FUZ_INS1+$0009,$FFFE,FUZ_INS1+$000C,$FFFF
FDB FUZ_INS1+$0000,FUZ_INS1+$0005,$FFFE,FUZ_INS1+$000A,$FFFF
FDB FUZ_INS1+$0000,FUZ_INS1+$0006,$FFFE,FUZ_INS1+$000A,$FFFF
FDB FUZ_INS1+$0000,FUZ_INS1+$0007,$FFFE,FUZ_INS1+$000B,$FFFF
FDB FUZ_INS1+$0000,FUZ_INS1+$0008,$FFFE,FUZ_INS1+$000C,$FFFF
FDB FUZ_INS1+$0000,FUZ_INS1+$0009,$FFFE,FUZ_INS1+$000C,$FFFF
WEIGHT EQU *
FCB $FF,$FF,$FF,$FF,$FF
ORG ROMStart
Entry
LDAA #$40
STAA PWMCTL
LDAA #$33
STAA PWMPRCLK
LDAA #$00
STAA PWMCLK
LDAA #$FF
STAA PWMPOL
LDAA #$64
STAA PWMPER0
LDAA #$64
STAA PWMPER1
LDAA #$00
STAA PWMDTY0
LDAA #$00
STAA PWMDTY1
LDAA #$EA
STAA PWMPER4
LDAA #$60
STAA PWMPER5
LDAA #$11
STAA PWMDTY4
LDAA #$94
STAA PWMDTY5
LDAA #$33
STAA PWME
LDAA #$0F
STAA TIOS
LDAA #$00
STAA TIE
LDAA #$03
STAA TSCR2
LDAA #$40
STAA TCTL3
LDAA #$81
STAA TSCR1
LDAA #$00
STAA ATD0CTL2
LDAA #$08
STAA ATD0CTL3
LDAA #$80
STAA ATD0CTL4
LDAA #$00
STAA ATD0DIEN
LDAA #$00
STAA ATD1CTL2
LDAA #$08
STAA ATD1CTL3
LDAA #$80
STAA ATD1CTL4
LDAA #$00
STAA ATD1DIEN
LDAA #$00
STAA DDRH
LDAA #$00
STAA DDRB
LDAA #$FF
STAA DDRK
LDAA #$08
STAA PORTK
LDAA #$00
STAA INTCR
LDAA #$00
STAA M0
LDAA #$2D
STAA DD
LDD #$1173
STD RESULT0
LDD #$10D0
STD RESULT6
LDD #$1200
STD RESULT7
LDAA PORTB
STAA LIANXU
CMPA #$FE
BEQ SUDU1
CMPA #$F7
BEQ SUDU2
CMPA #$F6
BEQ SUDU3
BRA SUDU0
SUDU0 LDAA #$40
STAA GAOSU
LDD #$14A0
STD ZHI_XIAN
LDD #$16A0
STD WAN_XIAN
LDAA #$42
STAA SUDU01
LDAA #$41
STAA SUDU02
LDAA #$41
STAA SUDU03
LBRA AGAIN
SUDU1 LDAA #$41
STAA GAOSU
LDD #$1400
STD ZHI_XIAN
LDD #$1630
STD WAN_XIAN
LDAA #$42
STAA SUDU01
LDAA #$41
STAA SUDU02
LDAA #$41
STAA SUDU03
BRA AGAIN
SUDU2 LDAA #$42
STAA GAOSU
LDD #$1350
STD ZHI_XIAN
LDD #$16A0
STD WAN_XIAN
LDAA #$42
STAA SUDU01
LDAA #$42
STAA SUDU02
LDAA #$41
STAA SUDU03
BRA AGAIN
SUDU3 LDAA #$42
STAA GAOSU
LDD #$12A0
STD ZHI_XIAN
LDD #$16A0
STD WAN_XIAN
LDAA #$42
STAA SUDU01
LDAA #$42
STAA SUDU02
LDAA #$41
STAA SUDU03
BRA AGAIN
AGAIN
WF_F LDAA PORTH
ANDA #$02
CMPA #$02
BEQ WF_F
CLI
LDAA #$00
STAA C_LINE
STAA F_FINISH
LDD #$2100
STD P_SAVE
LDD TCNT
ADDD #$0EE0
STD TC0
LDAA #$FF
STAA TFLG1
LDAA #$01
STAA TIE
WF_FINISH LDAA F_FINISH
CMPA #$01
BNE WF_FINISH
SEI
LDD #$2C3F
STD P_LINE
STD P_ROW
LDD #$3159
STD P_YOU1
LDD #$2E00
STD P_POSITION
LDD #$3100
STD P_LINE1
LDX P_LINE1
NEXT_POINT0 LDY P_LINE
LDAA Y
STAA X
INX
LDD P_LINE
SUBD #$0020
STD P_LINE
CPD #$2100
BHI NEXT_POINT0
NEXT_LINE0 LDD P_ROW
SUBD #$0001
STD P_ROW
STD P_LINE
CPD #$2C21
BHI NEXT_POINT0
LDX #$3268
LDAA X
STAA C_MIN
STAA C_MAX
JUX INX
LDAA X
CMPA C_MIN
BHI MAX1
STAA C_MIN
MAX1 CMPA C_MAX
BLO MIN1
STAA C_MAX
MIN1 CPX #$3B32
BGT FINISH2
BRA JUX
FINISH2 LDAA #$00
LDAB C_MIN
STD TEM10
LDAA #$00
LDAB C_MAX
ADDD TEM10
LDX #$0002
IDIV
TFR X,D
SUBD #$0010
STAB COMPARE
LDD #$3100
STD P_LINE2
LDAA #$05
STAA P_POINTL
LDAA #$55
STAA P_POINTR
NEXT_LINE LDX P_LINE2
LDAB P_POINTR
ABX
STX E_HIGH
LDX P_LINE2
LDAB P_POINTL
ABX
NEXT_POINT LDAA X
CMPA COMPARE
LBHIN_BLACK1
TFR X,D
SUBD P_LINE2
STAB LEFT_S
N_WHITE INX
CPX E_HIGH
BGT LINE_FULL
LDAA X
CMPA COMPARE
BLO N_WHITE
N_WHITE1 TFR X,D
SUBD P_LINE2
STAB RIGHT_S
LDX E_HIGH
RIGHT LDAA X
CMPA COMPARE
BHI N_BLACK2
TFR X,D
SUBD P_LINE2
CMPB RIGHT_S
LBLO HUAN
FINISH_0 SUBB RIGHT_S
CMPB #$0A
LBGT LINE_ERROR
LINE_FULL TFR X,D
SUBD P_LINE2
STAB RIGHT_S
ADDB LEFT_S
LSRB
LDY P_POSITION
STAB Y
CMPB #$10
BLS L_OVERFLOW
CMPB #$49
BGE R_OVERFLOW
SUBB #$10
STAB P_POINTL
ADDB #$20
STAB P_POINTR
BRA INC_LINE
L_OVERFLOW TBA
ADDA #$10
STAA P_POINTR
LDAA #$01
STAA P_POINTL
BRA INC_LINE
R_OVERFLOW TBA
SUBA #$10
STAA P_POINTL
LDAA #$58
STAA P_POINTR
BRA INC_LINE
INC_LINE LDD P_LINE2
ADDD #$005A
STD P_LINE2
CPD #$3B32
BHI FIND_FINISH
LDX P_POSITION
INX
STX P_POSITION
LBRA NEXT_LINE
N_BLACK1 INX
CPX E_HIGH
BGT LINE_ERROR
LBRA NEXT_POINT
N_BLACK2 DEX
CPX P_LINE2
BLT LINE_ERROR
LBRA RIGHT
LINE_ERROR LDAA #$FF
LDY P_POSITION
STAA Y
BRA INC_LINE
HUAN STAB TEM
LDAB RIGHT_S
LDAA TEM
STAA RIGHT_S
LBRA FINISH_0
FIND_FINISH
SS LDX #$2E00
LDAA #$00
STAA ERROR
AGAIN00 LDAB X
CPX #$2E1E
LBEQ H
INX
CMPB #$FF
BNE AGAIN00
INC ERROR
BRA AGAIN00
H LDAA ERROR
CMPA #$18
LBHI FIGURE_ERROR
LDAA #$1E
STAA LAST_LINE
LDX #$2E1D
CHA0 LDAA X
CMPA #$FF
BEQ AGAIN002
STAA LAST
LDAA #$01
STAA FIRST_LINE
LDX #$2DFF
CHA1 INX
LDAA X
CMPA #$FF
BEQ AGAIN000
STAA MIN
STAA MAX
STAA FIRST
CHA2 INX
LDAA X
CMPA #$FF
BEQ AGAIN001
CMPA MAX
BLT MIN0
STAA MAX
MIN0 CMPA MIN
BGT MAX0
STAA MIN
MAX0 CPX #$2E1D
BGT FINISH1
BRA CHA2
AGAIN000 INC FIRST_LINE
CPX #$2E1D
LBEQ AGAIN
BRA CHA1
AGAIN001 CPX #$2E1D
BEQ FINISH1
BRA CHA2
AGAIN002 DEC LAST_LINE
DEX
CPX #$2E00
LBLT AGAIN
BRA CHA0
FINISH1 LDAA LAST
LDAB FIRST
SBA
STAA POINT_CHA
LDAA POINT_CHA
ADDA #$5A
STAA POINT_CHA
LDAA LAST_LINE
LDAB FIRST_LINE
SBA
STAA LINE_CHA
LDAA POINT_CHA
ADDA #$01
STAA CRRNT_INS1
LDAA LINE_CHA
ADDA #$01
STAA CRRNT_INS1+1
FUZZIFY1: LDX #INPUT_MFS1
LDY #FUZ_INS1
LDAA CRRNT_INS1
LDAB #5
GRAD_LOOP1: MEM
DBNE B,GRAD_LOOP1
LDAA CRRNT_INS1+1
LDAB #5
GRAD_LP1: MEM
DBNE B,GRAD_LP1
LDAB #07
RULE_EVAL1: CLR 1,Y+
DBNE B,RULE_EVAL1
LDX #RULE1
LDY #WEIGHT
LDAB #25
EVALUE1: LDAA #$FF
REVW
DBNE B,EVALUE1
DEFUZ1: LDX #SGLTN_POS1
LDY #FUZ_OUTS1
LDAB #07
WAV
EDIV
TFR Y,D
STAB COG_OUT1
LDAA LAST
ADDA #$01
STAA CRRNT_INS
LDAA COG_OUT1
ADDA #$01
STAA CRRNT_INS+1
FUZZIFY: LDX #INPUT_MFS
LDY #FUZ_INS
LDAA CRRNT_INS
LDAB #5
GRAD_LOOP: MEM
DBNE B,GRAD_LOOP
LDAA CRRNT_INS+1
LDAB #5
GRAD_LP: MEM
DBNE B,GRAD_LP
LDAB #07
RULE_EVAL: CLR 1,Y+
DBNE B,RULE_EVAL
LDX #RULE
LDY #WEIGHT
LDAB #25
EVALUE: LDAA #$FF
REVW
DBNE B,EVALUE
DEFUZ: LDX #SGLTN_POS
LDY #FUZ_OUTS
LDAB #07
WAV
EDIV
TFR Y,D
STAB COG_OUT
LDAA #$0A
LDAB COG_OUT
MUL
ADDD #$0C26
STD RESULT1
CPD RESULT6
BLT XIAO
CPD RESULT7
BGT DA
ZHUAN LDD RESULT1
CPD #$14A0
BGT GUO
CPD #$0E00
BLT GAO
STAA PWMDTY4
STAB PWMDTY5
STD RESULT0
LBRA MOTOR
GUO LDD #$14A0
STAA PWMDTY4
STAB PWMDTY5
STD RESULT0
LBRA MOTOR
GAO LDD #$0E03
STAA PWMDTY4
STAB PWMDTY5
STD RESULT0
LBRA MOTOR
XIAO LDD RESULT0
CPD RESULT7
BGT S
BRA ZHUAN
DA LDD RESULT0
CPD RESULT6
BLT S
BRA ZHUAN
S LDD RESULT0
ADDD RESULT1
LDX #$0002
IDIV
TFR X,D
CPD #$14AF
BGT GUO0
CPD #$0E00
BLO GAI0
STAA PWMDTY4
STAB PWMDTY5
STD RESULT0
STD RESULT4
JMP MOTOR
GUO0 LDD #$14AF
STD RESULT0
STD RESULT4
STAA PWMDTY4
STAB PWMDTY5
BRA MOTOR
GAI0 LDD #$0E00
STAA PWMDTY4
STAB PWMDTY5
STD RESULT0
STD RESULT4
STAA PWMDTY4
STAB PWMDTY5
BRA MOTOR
FIGURE_ERROR LDAA #$AA
STAA PORTB
LDAA #$41
STAA PWMDTY0
LDAA #$00
STAA PWMDTY1
LDD RESULT0
CPD #$1194
BHI FIGURE_ERROR1
LDD RESULT0
SUBD #$0064
CPD #$0E00
BLO GAI1
STAA PWMDTY4
STAB PWMDTY5
STD RESULT0
LDAA #$2D
STAA DD
JMP AGAIN
GAI1 LDD #$0E00
STAA PWMDTY4
STAB PWMDTY5
STD RESULT0
LDAA #$2D
STAA DD
JMP AGAIN
FIGURE_ERROR1 LDD RESULT0
ADDD #$0064
CPD #$14AF
BHI GUO1
STAA PWMDTY4
STAB PWMDTY5
STD RESULT0
LDAA #$2D
STAA DD
JMP AGAIN
GUO1 LDD #$14AF
STAA PWMDTY4
STAB PWMDTY5
STD RESULT0
LDAA #$2D
STAA DD
JMP AGAIN
MOTOR LDAA COG_OUT1
CMPA #$51
LBLT B1
CMPA #$64
LBGT B1
BRA B0
B0 INC M0
LDD #$0000
STD STIME_COUNT
STD STIME_COUNT0
LDAA #$80
STAA TFLG1
CE0 LDAA TFLG1
ANDA #$80
CMPA #$80
BEQ CAMPURE0
LDD STIME_COUNT0
ADDD #$0001
STD STIME_COUNT0
LDD STIME_COUNT0
CPD ZHI_XIAN
BNE CE0
BRA MAN
CAMPURE0LDD TC7
STD FIRST0
LDAA #$80
STAA TFLG1
CE LDAA TFLG1
ANDA #$80
CMPA #$80
BEQ KUAI
LDD STIME_COUNT
ADDD #$0001
STD STIME_COUNT
LDD STIME_COUNT
CPD ZHI_XIAN
BNE CE
BRA MAN
MAN LDAA #$00
STAA SUDU
BRA XIELV
KUAI LDAA #$01
STAA SUDU
BRA XIELV
XIELV LDAA SUDU
CMPA #$01
BEQ CHULI
LDAA GAOSU
STAA PWMDTY0
LDAA #$00
STAA PWMDTY1
LBRA ANGLE00
CHULI LDAA #$00
STAA PWMDTY0
LDAA #$00
STAA PWMDTY1
CALL STIME
LDAA #$00
STAA M0
LBRA ANGLE00
B1 LDAA M0
CMPA #$03
BLT BU
BRA XIAN
BU LDAA SUDU02
STAA PWMDTY0
LDAA #$00
STAA PWMDTY1
LDAA #$00
STAA M0
LBRA ANGLE00
XIANLDD #$0000
STD STIME_COUNT
STD STIME_COUNT0
LDAA #$80
STAA TFLG1
CE01 LDAA TFLG1
ANDA #$80
CMPA #$80
BEQ CAMPURE00
LDD STIME_COUNT0
ADDD #$0001
STD STIME_COUNT0
LDD STIME_COUNT0
CPD WAN_XIAN
BNE CE01
LBRA MAN
CAMPURE00LDAA #$80
STAA TFLG1
CE00 LDAA TFLG1
ANDA #$80
CMPA #$80
BEQ KUAI0
LDD STIME_COUNT
ADDD #$0001
STD STIME_COUNT
LDD STIME_COUNT
CPD WAN_XIAN
BNE CE00
BRA MAN0
MAN0 LDAA #$00
STAA SUDU
BRA XIELV0
KUAI0 LDAA #$01
STAA SUDU
BRA XIELV0
XIELV0 LDAA #$00
STAA M0
LDAA SUDU03
STAA PWMDTY0
LDAA #$00
STAA PWMDTY1
LDAA #$F0
STAA PORTB
LDAA SUDU
CMPA #$01
BNE ANGLE00
BEQ CHULI0
BRA ANGLE00
CHULI0 LDAA #$00
STAA PWMDTY0
LDAA #$64
STAA PWMDTY1
CALL STIME0
LDAA SUDU01
STAA PWMDTY0
LDAA #$00
STAA PWMDTY1
LDAA #$F0
STAA PORTB
LDAA #$00
STAA M0
BRA ANGLE00
ANGLE00 JMP AGAIN
STIME LDX #$12FF
AGAIN5 LDY #$008F
DBNE Y,*
DBNE X,AGAIN5
RTC
STIME0 LDX #$12FF
AGAIN7 LDY #$008F
DBNE Y,*
DBNE X,AGAIN7
RTC
STIME1 LDX #$12FF
AGAIN6 LDY #$001F
DBNE Y,*
DBNE X,AGAIN6
RTC
TC0INT LDAA #$00
STAA TIE
LDAA #$40
STAA INTCR
RTI
TC1INT LDAA #$00
STAA TIE
LDX P_SAVE
TC1INT_B1 LDAA #$C0
STAA ATD0CTL2
LDAA #$80
STAA ATD0CTL5
NOP
NOP
NOP
NOP
NOP
NOP
NOP
NOP
NOP
NOP
NOP
NOP
LDAA #$C0
STAA ATD1CTL2
LDAA #$80
STAA ATD1CTL5
TC1INT_B2 LDAA ATD0STAT1
ANDA #$01
CMPA #$01
BNE TC1INT_B2
LDAA ATD0DR0L
STAA X
INX
TC1INT_B3 LDAA ATD1STAT1
ANDA #$01
CMPA #$01
BNE TC1INT_B3
LDAA ATD1DR0L
STAA X
INX
INC C_POINT
LDAA C_POINT
CMPA #$10
BNE TC1INT_B1
LDD P_SAVE
ADDD #$0020
STD P_SAVE
LDD TCNT
ADDD #$0178
STD TC2
LDAA #$FF
STAA TFLG1
LDAA #$04
STAA TIE
RTI
TC2INT LDAA #$00
STAA TIE
LDAA #$40
STAA INTCR
RTI
INTV LDAA #$00
STAA INTCR
INC C_LINE
LDAA C_LINE
CMPA #$5B
BEQ INTV_B1
LDAA #$00
STAA C_POINT
LDD TCNT
ADDD #$001D
STD TC1
LDAA #$FF
STAA TFLG1
LDAA #$02
STAA TIE
JMP INTV_B2
INTV_B1 LDAA #$01
STAA F_FINISH
INTV_B2 RTI
ORG $EFEA
FDB TC2INT
ORG $EFEC
FDB TC1INT
ORG $EFEE
FDB TC0INT
ORG $EFF2
FDB INTV
ORG $EFFE
FDB Entry