'-------------------------------------------------------------------------------
'	File:		LineFollow.bas
'	Description:	Line Following algorithm
'-------------------------------------------------------------------------------
Option Explicit

Const LeftPin As Byte = 5
Const CenterPin As Byte = 19
Const RightPin As Byte = 18

	Const OffTheLine As Integer = 0
	Const Center As Integer = 1
	Const Left As Integer = 2
	Const LeftCenter As Integer = 3
	Const Right As Integer = 4
	Const RightCenter As Integer = 5
	Const EndOfLine As Integer = 7

Sub LineFollow()

	Dim State As Integer
	Dim LastKnownState As Integer
	Dim LeftDuty As Single
	Dim RightDuty as Single

	LastKnownState = Center
Do

	If LineColor = 1 Then
		State = CInt(GetPin(RightPin) * 4 + GetPin(LeftPin) * 2 + GetPin(CenterPin))
		Call PutPin(BLED, GetPin(RightPin))
		Call PutPin(ALED, GetPin(CenterPin))
		Call PutPin(RedLED, 1 - GetPin(LeftPin))

	Else
		State = CInt((1 - GetPin(RightPin)) * 4 + (1 - GetPin(LeftPin)) * 2 + (1 - GetPin(CenterPin)))
		Call PutPin(BLED, 1 - GetPin(RightPin))
		Call PutPin(ALED, 1 - GetPin(CenterPin))
		Call PutPin(RedLED, GetPin(LeftPin))

	End If

	Debug.Print "State = "; CStr(State)

	If State = OffTheLine Then
		State = LastKnownState
	Else
		LastKnownState = State
	End If

	Select Case State
		Case Center	
			LeftDuty = 1.0
			RightDuty = 0.95

		Case LeftCenter
			LeftDuty = 0.9
			RightDuty = 0.95

		Case Left
			LeftDuty = 0.8
			RightDuty = 0.95

		Case RightCenter
			LeftDuty = 1.0
			RightDuty = 0.85

		Case Right
			LeftDuty = 1.0
			RightDuty = 0.75

		Case EndOfLine
			LeftDuty = 0.0
			RightDuty = 0.0

	End Select

	Call PutPinPWM(RightMotorPWM, RightDuty)
	Call PutPinPWM(LeftMotorPWM, LeftDuty)
	Call PlayNote(PiezoPin, 2, State, 20)

Loop Until State = EndOfLine

End Sub