'TH stands for threashold of sensors

dim ir1 as byte
dim ir2 as byte
dim ir3 as byte
dim ir4 as byte
dim ir5 as byte
dim ir6 as byte

'const TH=140 under light
'const TH=49 in no light 
DIM I AS BYTE
ADCON1 = 0

DELAY 1000

ADCON0 = 11000001b
bADSTART = 1
ir1 = ADRES
ADCON0 = 11001001b
bADSTART = 1
ir2 = ADRES
ir3 = ir1 + 15
ir4 = ir2 + 15

DELAY 1000
		
FOR I=0 TO 5
Call ForwardRobot
NEXT
Servo1 = 1

start:

ADCON0 = 11000001b
bADSTART = 1
ir1 = ADRES
ADCON0 = 11001001b
bADSTART = 1
ir2 = ADRES

IF (ir1 > ir3) OR (ir2 > ir4) THEN
	Call BackwardRobot
	ir5 = ir1 - ir3
	ir6 = ir2 - ir4
	IF (ir5 > ir6 + 2) THEN
		Call TurnLeft
		Call TurnLeft
		Call ForwardRobot
	END IF
	IF (ir6 > ir5 + 2) THEN
		Call TurnRight
		Call TurnRight
		Call ForwardRobot
	END IF
ELSE
	Call StepByStep
END IF

DELAY 300

GOTO start

SUB ForwardRobot
'left forward
   Servo1 = 1
   DELAY 1000	
   Servo7 = 1
   Servo8 = 1	
   DELAY 1000
'right forward
   Servo1 = 255   
   DELAY 1000	
   Servo8 = 255
   Servo7 = 255
   DELAY 1000
END SUB

SUB StepByStep
'left forward
IF Servo1 = 1 THEN
   Servo7 = Servo7 - 50
   Servo8 = Servo8 - 50	
   DELAY 300
   IF Servo7 < 50 THEN
   Servo1 = 255
   DELAY 1000	
   END IF	
ELSE
'right forward
   Servo8 = Servo8 + 50
   Servo7 = Servo7 + 50
   DELAY 300
   IF Servo7 > 200 THEN
   Servo1 = 1
   DELAY 1000	
   END IF	
END IF
END SUB

SUB BackwardRobot
IF Servo1 = 1 THEN
	Call LeftBackward
	Call RightBackward
ELSE
	Call RightBackward
	Call LeftBackward
END IF
END SUB

SUB LeftBackward
   Servo1 = 1
   DELAY 1000	
   Servo7 = 255
   Servo8 = 255	
   DELAY 1000
END SUB	
SUB RightBackward
   Servo1 = 255   
   DELAY 1000	
   Servo8 = 1
   Servo7 = 1
   DELAY 1000
END SUB

SUB TurnLeft
IF Servo1 > 128 THEN
   Servo1 = 1
   DELAY 500	
   Servo7 = 255
   DELAY 1000	
   Servo1 = 255
   DELAY 500	
   Servo8 = 255
   DELAY 1000	
END IF
   Servo1 = 128
   DELAY 1500	
   Servo7 = 1
   Servo8 = 1
   DELAY 2000	
   Servo1 = 1
   DELAY 500	
   Servo1 = 1
   DELAY 500	
   Servo7 = 255
   DELAY 1000	
   Servo1 = 255
   DELAY 500	
   Servo8 = 255
   DELAY 1000	
END SUB

SUB TurnRight
IF Servo1 < 128 THEN
   Servo1 = 255
   DELAY 500	
   Servo7 = 1
   DELAY 1000	
   Servo1 = 1
   DELAY 500	
   Servo8 = 1
   DELAY 1000	
END IF
   Servo1 = 128
   DELAY 1500	
   Servo7 = 255
   Servo8 = 255
   DELAY 2000	
END SUB