SYMBOL Detectors = B0
SYMBOL RightDetect = BIT0
SYMBOL LeftDetect = BIT1
SYMBOL Beacon_Detect = BIT2
SYMBOL Rotations = B1
SYMBOL Index = B2
SYMBOL Radio_Command = B2
SYMBOL Radio = 2

CheapBot_14:

Beacon_Locate:
  IF PIN3 = 0 THEN Beacon_Detected
  GOSUB Left
  PAUSE 100
  GOSUB Freeze
  GOTO Beacon_Locate
    
Beacon_Detected:
  Rotations = Rotations + 1  
  GOSUB Left
  PAUSE 100
  GOSUB Freeze  
  IF PIN3 = 0 THEN Beacon_Detected

Center_on_Beacon:
  Rotations = Rotations/2
  GOSUB Right
  FOR Index = 1 TO Rotations  
    PAUSE 100
  NEXT
  GOSUB Freeze
  
Travel_to_Beacon:
  GOSUB Forwards
  PAUSE 20000
  
Line_Follower:
  LOW 2
  HIGH 3
  LOW 4
  HIGH 5
    
Check_Line_Follower:  
  GOSUB Line_Finder_Check
  IF Detectors = 0 THEN Check_Line_Follower
  IF Detectors = 1 THEN Line_Right
  IF Detectors = 2 THEN Line_Left
  IF Detectors = 3 THEN Line_in_Front

Line_Right:
  GOSUB Left
  PAUSE 120
  GOTO Line_Follower

Line_Left:
  GOSUB Right
  PAUSE 120
  GOTO Line_Follower

Line_in_Front:
  GOSUB Freeze
  GOTO CheapBot_14  

Dont_Crash:
  LOW 2
  HIGH 3
  LOW 4
  HIGH 5
  
Check_Promity_Detector: 
  GOSUB IR_Bumper_Check
  IF Detectors = 0 THEN Wall_in_Front
  IF Detectors = 3 THEN Dont_Crash  
  IF LeftDetect = 0 THEN Wall_Right
  IF RightDetect = 0 THEN Wall_Left
  
Wall_Right:
  GOSUB Left
  PAUSE 120
  GOTO Dont_Crash

Wall_Left:
  GOSUB Right
  PAUSE 120
  GOTO Dont_Crash

Wall_in_Front:
  LOW 2
  HIGH 3
  HIGH 4
  LOW 5
  PAUSE 500
  GOTO Dont_Crash 
  
Line_Finder_Check:
  Detectors = 0
  RightDetect = PIN0
  LeftDetect = PIN1
  RETURN
  
IR_Bumper_Check:
  Detectors = 0
  HIGH 1                        
  RightDetect = PIN4
  LOW 1
  PAUSE 5
  HIGH 0                        
  LeftDetect = PIN4
  LOW 0
  PAUSE 5
  RETURN       
  
Right:
  HIGH 2
  LOW 3
  LOW 4
  HIGH 5
  RETURN

Left:
  LOW 2
  HIGH 3
  HIGH 4
  LOW 5
  RETURN

Backwards:
  HIGH 2
  LOW 3
  HIGH 4
  LOW 5
  RETURN
  
Forwards:
  LOW 2
  HIGH 3
  LOW 4
  HIGH 5
  RETURN
  
Freeze:
  LOW 2
  LOW 3
  LOW 4
  LOW 5
  PAUSE 500
  RETURN 
  
End_CheapBot_14:
  END