'-----------------------------------------------------------------
' Program to control a LEGO-robot (type NANOBOT)
'
' NANO_BOT.BAS 08.03.98
' Author Gerhard Nöcker
' 1998 Copyright Elektor, Beek (L), The Netherlands
'-----------------------------------------------------------------

'----- Port Pin Assignment ---------------------------------------
'Port Function Type
'Pin 0 Motor current 0/1 == on/off DigOut
'Pin 1 Right motor 0/1 == back/fw DigOut
'Pin 2 Left motor (buzzer) 0/1 == back/fw DigOut (sound)
'Pin 7 Switch right/left AnIn
'Pin 4 Right IR obstacle det. 0/1 == obstacle/free DigIn
'Pin 5 Right LDR AnIn
'Pin 6 Left LDR AnIn
'Pin 3 Left IR obstacle det. 0/1 == obstacle/free DigIn

'----- Remark ----------------------------------------------------
'Pin 2 is also used to produce sounds. Both motors will be halted.
'The direction bit of the left motor controls the buzzer.
'There are four different sounds:
'Obstacle on the right side: high/low
'Obstacle on the left side: low/high
'Too much light: high
'Too dark: low

'----- Declaration of constants ----------------------------------
symbol sw_pin = 7 'switch input at port pin 7
symbol sw_scale = 33 'switch scale factor
symbol ri_ldr = 5
symbol ri_ldr_sc = 82
symbol le_ldr = 6
symbol le_ldr_sc = 82
symbol l_thres = 15
symbol halt = 0
symbol fwd = 7
symbol backw = 1
symbol Right = 5
symbol Left = 3

'----- Declaration of variables ----------------------------------
symbol r_sw = bit0 'storage for right switch (on/off)
symbol l_sw = bit1 'storage for left switch (on/off)
symbol f_flag = bit2 'NOT USED
symbol b_flag = bit3 '
symbol l_flag = bit4
symbol sw_val = b4 'storage for analog switch value
symbol lw_re = b5
symbol lw_li = b6
symbol duration = w4
symbol Rnd = w5

'----- Program start ---------------------------------------------
dirs=%00000111 'port bit 7..3 input, bit 2..0 output
r_sw = 0
l_sw = 0

'
' If a switch is opened/closed at power up the robot will
' search/avoid light.
'
pot sw_pin, sw_scale, sw_val 'read switches
b_flag = 1
If sw_val < 240 Then lbl1
goto loop
lbl1: b_flag = 0
l_flag = 0

'----- Program main loop -----------------------------------------
Loop: pot ri_ldr, ri_ldr_sc, lw_re 'read right LDR
pot le_ldr, le_ldr_sc, lw_li 'read left LDR
lw_re=lw_re min l_thres
lw_li=lw_li min l_thres
b8 = lw_li - l_thres
b9 = lw_re - l_thres
If lw_re < 200 Then clearfl

cont0:
If lw_re < b8 And b_flag = 0 Then turn_l 'compare light
If lw_li < b9 And b_flag = 0 Then turn_r 'and
If lw_re < b8 And b_flag = 1 Then turn_r 'behaviour
If lw_li < b9 And b_flag = 1 Then turn_l
If lw_re = l_thres And lw_li = l_thres Then turn 'too much light
If lw_re = 255 And lw_li = 255 And l_flag = 0 Then wait 'too dark

forw: pins = fwd
GoTo switch

turn_r: pins = Right
goto loop

turn_l: pins = Left
goto loop

switch: 'switches and IR obstacle detection
pot sw_pin, sw_scale, sw_val 'read switches
If sw_val > 180 And sw_val < 240 Or pin3 = 0 Then sl
If sw_val < 180 Or pin4 = 0 Then sr
If sw_val > 240 Then Loop

sl: r_sw = 0
l_sw = 1
GoTo cont1

sr: r_sw = 1
l_sw = 0

cont1:
pins = halt
pause 1000
pins = backw
pause 1500
pins = halt
If l_sw = 1 Then cont2
sound 2,(123,40,110,40)
pins = Left
GoTo cont3

cont2:
sound 2,(110,40,123,40)
pins = Right

cont3:
random Rnd
duration = 1000 + b10
pause duration
pins = halt
pause 1000
goto loop

turn:
pins = halt
sound 2,(125,30)
pins = Right
duration = 2800
pause duration
pins = halt
pins = fwd
pause 3000
GoTo switch

wait:
pins = halt
sound 2,(80,30)
pause 10000
l_flag = 1
GoTo switch

clearfl:
l_flag = 0
GoTo cont0