Option Explicit

public sub LineFollow()
	cleardisplay
	call display("5  ")
	delay(1.0)
	call display("4  ")
	delay(1.0)
	call display("3  ")
	delay(1.0)
	call display("2  ")
	delay(1.0)
	call display("1  ")
	delay(1.0)
	cleardisplay
	call display("Activate...")
	running = 1
	FloorSensorsOn
	startmotors
	do
		if BothBumpersPressed or LeftBumperPressed or RightBumperPressed then
			StopMotors
			exit do
		end if
	
	'white floor -----------------------------------------------
'		if (LeftFloor = false) and (RightFloor = false) then
'			Startmotors
'		elseif LeftFloor = true then
'			JogRight
'		elseif RightFloor = true then
'			JogLeft
'		end if
	'-----------------------------------------------------------

	'black floor -----------------------------------------------
		if (LeftFloor = true) and (RightFloor = true) then
			Startmotors
		elseif LeftFloor = false then
			JogRight
		elseif RightFloor = false then
			JogLeft
		end if
	'-----------------------------------------------------------

		Delay(0.02)			
	loop

	stopmotors
	FloorSensorsOff
	delay(1.0)
end sub

sub JogLeft()
    Register.OCR1AH = 0
    Register.OCR1AL = 64
    Register.OCR1BH = 0
    Register.OCR1BL = 90		' 90 120
end sub

sub JogRight()
    Register.OCR1AH = 0
    Register.OCR1AL = 85		'90 64
    Register.OCR1BH = 0
    Register.OCR1BL = 120
end sub