'***************************************
'*      Sample CheapBot-08 Code        *
'* This is a test of the line follower *
'*  Navigation, and Proximity Detector *
'*                boards               *
'***************************************

symbol index = b0
symbol amount = b1
symbol detect = b2
symbol clear_right = b3
symbol clear_left = b4
symbol direction = b5
symbol turn = b6
symbol right_detector = b7
symbol left_detector = b8

  low 0
  low 1
  low 4
  pause 1000

CheapBot_08:
  gosub Forwards

'**************************************
'*        Line Follower Code          *
'*    use this or the prox code       *
'**************************************

Line_Follower:
  gosub Line_Check
  if right_detector = 0 and left_detector = 0 then Line_Follower

Check_Stop:
  gosub Stop_Robot
  if right_detector = 1 and left_detector = 1 then End_CheapBot_08
  if right_detector = 0 then Adjust_Left

Adjust_Right:
  amount = 4
  gosub Right
  gosub Line_Check
  if right_detector = 1 then Adjust_Right
  goto CheapBot_08

Adjust_Left:
  amount = 4
  gosub Left
  gosub Line_Check
  if left_detector = 1 then Adjust_Left
  goto CheapBot_08

'**************************************
'*        Proximity Detector Code     *
'* use this or the line follower code *
'**************************************
Prox:
  gosub IR_Check
  if detect = 1 then Drive
  gosub Stop_Robot
  clear_right = 0
  clear_left = 0

Check_Left:
  amount = 5
  gosub Left
  gosub IR_Check
  if detect = 0 then Check_Left

Back_to_Center:
  amount = 5 * clear_left
  gosub Right
  clear_right = 0

Check_Right:
  amount = 5
  gosub Right
  gosub IR_Check
  if detect = 0 then Check_Right

Back_to_Center_Again:
  if clear_left > clear_right then Set_Right_Turn
  direction = "L"
  turn = clear_left * 5
  goto Center_Robot

Set_Right_Turn:
  direction = "R"
  turn = clear_right * 5

Center_Robot:
  amount = 5 * clear_right
  gosub Left
'  gosub Backwards
  pause 500
'  gosub Stop_Robot

Turn_to_Closest_Clear:
  amount = turn
  if direction = "R" then Closest_Right
'  gosub Left
  goto CheapBot_08

Closest_Right:
'  gosub Right
  goto CheapBot_08

IR_check:

'*******************************
'* pwmout 2 , 24, 50   '40 kHz *
'* pwmout 2 , 25, 51   '39 kHz *
'* pwmout 2 , 25, 53   '38 kHz *
'* pwmout 2 , 26, 54   '37 kHz *
'* pwmout 2 , 27, 56   '36 kHz *
'* pwmout 2 , 28, 57   '35 kHz *
'*******************************

  pwmout 2,24,50
  detect = pin3
  pwmout 2 off
  pause 10
  return

Line_Check:
  right_detector = pin2
  left_detector = pin3
  return

Right:
  clear_right = clear_right + 1
  for index = 1 to amount
    low 0
    high 1
    low 4
    pause 25
    high 0
    high 1
    low 4
    pause 25
  next
  low 0
  low 1
  low 4
  return

Left:
  clear_left = clear_left + 1
  for index = 1 to amount
    high 0
    low 1
    high 4
    pause 25
    low 0
    low 1
    high 4
    pause 25
  next
  low 0
  low 1
  low 4
  return

Forwards:
  low 0
  high 1
  high 4
  return

Backwards:
  high 0
  low 1
  low 4
  return

Stop_Robot:
  low 0
  low 1
  low 4
  pause 500
  return

End_CheapBot_08:
end