.code
.org 8000h
ljmp start
.org h'800B
ljmp int_tf0
.equ NUMLOOP_F, 43
.equ NUMLOOP_T, 13
.equ NUMSTEPS_F, 8
.equ NUMSTEPS_1cm, 55
.equ NUMSTEPS_GAP, h'ee
.equ NUMSTEPS_T,18
.equ MIN_DELAY_U, -2
.equ MIN_DELAY_L,0
.equ MIN_DELAY_UT, -2
.equ MIN_DELAY_LT, 0
.equ MAZE_X, h'06
.equ MAZE_Y, h'06
.equ MAZE_GOAL, h'55
.equ ar0, h'00
.equ ar1, h'01
.equ ar2, h'02
.equ ar3, h'03
.equ ar4, h'04
.equ ar5, h'05
.equ numsteps, h'18
.equ sensors, h'19
.equ location, h'1a
.equ map_mem_loc, h'1b
.equ wall_count , h'1c
.equ direction , h'1d
.equ mclk_left , h'20
.equ mclk_right , h'21
.equ fullwave, h'22
.equ map_doupdate,h'23
.equ gap_detect , h'24
.equ gap_bit1, h'25
.equ Y_ADR, h'f400
.equ S_ADR, h'fc00
.equ R_ADR, h'f800
.equ MAP, h'c000
.org 8100h
int_tf0:
jb fullwave, skip1
dec numsteps
skip1:
cpl fullwave
jnb mclk_left, int_skip1
cpl p1.7
int_skip1:
jnb mclk_right, int_skip2
cpl p1.0
int_skip2:
jb p1.3, turnfreq
jnb p1.2, turnfreq
mov th0, #MIN_DELAY_U
mov tl0, #MIN_DELAY_L
ljmp normalfreq
turnfreq:
mov th0, #MIN_DELAY_UT
mov tl0, #MIN_DELAY_LT
normalfreq:
reti
.org 8500h
start:
mov direction, #b'00001000
setb map_doupdate
clr p1.4
clr tr0
lcall delay
clr p1.1
mov location, #h'00
lcall waittostart
setb p1.4
lcall delay
lcall delay
lcall delay
lcall delay
lcall init_timers
setb p1.1
lcall check_sensors
setb mclk_left
setb mclk_right
mainloop:
mov dptr, #R_ADR
mov a, location
movx @dptr, a
lcall move_forward
lcall check_sensors
mov a, sensors
jb acc.4, main_end
jnb acc.1, s_left
jnb acc.2, s_left
jnb acc.3, s_left
jnb acc.5, s_right
jnb acc.6, s_right
jnb acc.7, s_right
jnb acc.4, s_right
jnb acc.0, s_right
ljmp main_end
s_right: lcall turn_right
ljmp main_end
s_left: jnb acc.5, s_180
jnb acc.6, s_180
jnb acc.7, s_180
lcall turn_left
ljmp main_end
s_180: lcall turn_180
ljmp main_end
main_end:
ljmp mainloop
init_timers:
mov a, tmod
orl a, #00000001b
anl a, #11110001b
mov tmod, a
setb ea
setb et0
setb pt0
mov th0, #MIN_DELAY_U
mov tl0, #MIN_DELAY_L
ret
check_sensors:
push ar0
clr p1.4
mov r0, #10
delay1: djnz r0, delay1
clr p1.6
mov dptr, #S_ADR
movx a, @dptr
mov sensors, a
mov r0, #10
delay2: djnz r0, delay2
setb p1.6
setb p1.4
mov dptr, #Y_ADR
mov a, sensors
xrl a, #h'FF
movx @dptr, a
pop ar0
ret
move_forward:
push ar0
push ar1
setb p1.2
clr p1.3
mov wall_count, #0
mov r1, #NUMLOOP_F
f_loop1:
mov numsteps, #NUMSTEPS_F
setb tr0
m_forward_loop:
mov r0, numsteps
lcall check_sensors
lcall check_forward
jb p1.5, nostopbutton1
ljmp start
nostopbutton1:
cjne r0, #0, m_forward_loop
lcall check_sensors
lcall drift_correct
mov a, sensors
jnb acc.1, no_right_gap
jnb acc.2, no_right_gap
jnb acc.3, no_right_gap
inc wall_count
mov r6, wall_count
cjne r6, #h'9, no_right_gap_skip
gap_skip:
left_wall_present:
mov r0, #0
mov r1, #1
lcall move_forward_gap
lcall turn_right
ljmp f_end1
no_right_gap:
mov wall_count, #0
no_right_gap_skip:
dec r1
cjne r1, #0, f_loop1
f_end1: clr tr0
lcall update_map
pop ar1
pop ar0
ret
check_forward:
push ar4
setb map_doupdate
mov a, sensors
jb acc.4, cf_endall
mov r4, #NUMLOOP_F / 2
mov a, r1
subb a, r4 ; (r1 - (numloop_f/2))
jc cf_skip
clr map_doupdate
cf_skip:
mov r0, #0
mov r1, #1
cf_endall:
pop ar4
ret
drift_correct:
push ar0
push ar1
mov a, sensors
jnb acc.4, dr_endS
mov a, sensors
jb acc.7, dr_skip7
lcall dr_holdL
mov a, sensors
jnb acc.6, dr_skip7
lcall dr_holdL
dr_skip7:
mov a, sensors
jb acc.1, dr_skip1
lcall dr_holdL
mov a, sensors
jnb acc.2, dr_skip1
lcall dr_holdL
dr_skip1:
mov a, sensors
jb acc.5, dr_skip5
lcall dr_holdR
mov a, sensors
jnb acc.6, dr_skip5
lcall dr_holdR
dr_skip5:
mov a, sensors
jb acc.3, dr_skip3
lcall dr_holdR
mov a, sensors
jnb acc.2, dr_skip3
lcall dr_holdR
dr_skip3:
ljmp dr_endall
dr_endS:
pop ar1
pop ar0
mov r0, #0
mov r1, #1
ret
dr_holdL:
mov b, numsteps
dec b
dr_dlyL0:
mov a, numsteps
cjne a, b, dr_dlyL0
clr mclk_left
mov b, numsteps
dec b
dr_dlyL1:
mov a, numsteps
cjne a, b, dr_dlyL1
setb mclk_left
ret
dr_holdR:
mov b, numsteps
dec b
dr_dlyR0:
mov a, numsteps
cjne a, b, dr_dlyR0
clr mclk_right
mov b, numsteps
dec b
dr_dlyR1:
mov a, numsteps
cjne a, b, dr_dlyR1
setb mclk_right
ret
dr_endall:
pop ar1
pop ar0
ret
move_forward_1cm:
push ar0
setb p1.2
clr p1.3
setb tr0
mov numsteps, #NUMSTEPS_1cm
setb tr0
m_forward_loop_1cm:
lcall drift_correct
mov r0, numsteps
cjne r0, #0, m_forward_loop_1cm
f_end1_1cm:
clr tr0
pop ar0
ret
move_forward_gap:
push ar0
setb p1.2
clr p1.3
setb tr0
mov numsteps, #NUMSTEPS_GAP
setb tr0
m_forward_loop_gap:
lcall drift_correct
mov r0, numsteps
cjne r0, #0, m_forward_loop_gap
f_end1_gap:
clr tr0
pop ar0
ret
turn_right:
push ar0
push ar1
mov r4, a
mov a, direction
rr a
mov direction, a
lcall move_forward_1cm
clr p1.2
clr p1.3
setb tr0
mov r1, #NUMLOOP_T
r_loop1:
mov numsteps, #NUMSTEPS_T
setb tr0
turn_right_loop:
mov r0, numsteps
cjne r0, #0, turn_right_loop
dec r1
cjne r1, #0, r_loop1
r_end1: clr tr0
pop ar1
pop ar0
ret
turn_left:
push ar0
push ar1
mov r4, a
mov a, direction
rl a
mov direction, a
lcall move_forward_1cm
setb p1.2
setb p1.3
setb tr0
mov r1, #NUMLOOP_T
l_loop1:
mov numsteps, #NUMSTEPS_T
setb tr0
turn_left_loop:
mov r0, numsteps
cjne r0, #0, turn_left_loop
dec r1
cjne r1, #0, l_loop1
l_end1: clr tr0
pop ar1
pop ar0
ret
turn_180:
push ar0
push ar1
mov r4, a
mov a, direction
rl a
rl a
mov direction, a
lcall move_forward_1cm
setb p1.2
setb p1.3
setb tr0
lcall time_90_loop
lcall time_90_loop
a180_end1: clr tr0
pop ar1
pop ar0
ret
time_90_loop:
push ar0
push ar1
mov r1, #NUMLOOP_T
a180_loop1:
mov numsteps, #NUMSTEPS_T
setb tr0
turn_180_loop:
mov r0, numsteps
cjne r0, #0, turn_180_loop
dec r1
cjne r1, #0, a180_loop1
pop ar1
pop ar0
ret
waittostart:
mov dptr, #Y_ADR
mov a, #b'10000000
movx @dptr, a
lcall shortdelay
jnb p1.5, endstart
mov a, #b'01000000
movx @dptr, a
lcall shortdelay
jnb p1.5, endstart
mov a, #b'00100000
movx @dptr, a
lcall shortdelay
jnb p1.5, endstart
mov a, #b'00010000
movx @dptr, a
lcall shortdelay
jnb p1.5, endstart
mov a, #b'00000010
movx @dptr, a
lcall shortdelay
jnb p1.5, endstart
mov a, #b'00000100
movx @dptr, a
lcall shortdelay
jnb p1.5, endstart
mov a, #b'00001000
movx @dptr, a
lcall shortdelay
jnb p1.5, endstart
mov dptr, #Y_ADR
mov a, #b'00001000
movx @dptr, a
lcall shortdelay
jnb p1.5, endstart
mov a, #b'00000100
movx @dptr, a
lcall shortdelay
jnb p1.5, endstart
mov a, #b'00000010
movx @dptr, a
lcall shortdelay
jnb p1.5, endstart
mov a, #b'00000001
movx @dptr, a
lcall shortdelay
jnb p1.5, endstart
mov a, #b'00100000
movx @dptr, a
lcall shortdelay
jnb p1.5, endstart
mov a, #b'01000000
movx @dptr, a
lcall shortdelay
jnb p1.5, endstart
mov a, #b'10000000
movx @dptr, a
lcall shortdelay
ljmp waittostart
endstart:
ret
shortdelay:
push ar1
push ar2
mov r1, #h'66
shortdelayx: mov r2, #h'ff
shortdelay1: djnz r2, shortdelay1
djnz r1, shortdelayx
pop ar2
pop ar1
ret
delay:
push ar1
push ar2
mov r1, #h'ff
delayx:
mov r2, #h'ff
delay1y:
djnz r2, delay1y
djnz r1, delayx
pop ar2
pop ar1
ret
update_map:
push ar0
jnb map_doupdate, up_end
setb map_doupdate
lcall update_direction
mov a, direction
jb acc.3, up_inc_low
jb acc.2, up_inc_high
jb acc.1, up_dec_low
jb acc.0, up_dec_high
ljmp start
up_inc_low:
inc location
ljmp up_end
up_dec_low:
dec location
ljmp up_end
up_inc_high:
mov a, location
add a, #h'10
mov location, a
ljmp up_end
up_dec_high:
mov a, location
subb a, #h'10
mov location, a
ljmp up_end
up_end:
mov a, location
cjne a, #MAZE_GOAL, up_skip
mov dptr, #R_ADR
mov a, location
movx @dptr, a
ljmp start
up_skip:
pop ar0
ret
update_direction:
mov a, direction
mov c, acc.4
orl c, acc.0
mov acc.0, c
mov c, acc.7
orl c, acc.3
mov acc.3, c
mov c, acc.6
orl c, acc.2
mov acc.2, c
mov c, acc.5
orl c, acc.1
mov acc.1, c
anl acc, #b'00001111
mov direction, a
ret
get_map_mem_loc:
mov dptr, #MAP
mov a, dpl
add a, location
mov dpl, a
movx a, @dptr
mov map_mem_loc, a
ret
put_map_mem_loc:
mov dptr, #MAP
mov a, dpl
add a, location
mov dpl, a
mov a, map_mem_loc
movx @dptr, a
ret
wait:
jnb p1.5, endstartx
ljmp wait
endstartx:
lcall delay
lcall delay
ret
.end