' -----[ Title ]--------------------------------------------------------------
' Robotics with the Boe-Bot - FlashlightControlledBoeBot.bs2 (Light Compass)
' Boe-Bot follows flashlight beam focused in front of it.
' {$STAMP BS2} ' Stamp directive.
' {$PBASIC 2.5} ' PBASIC directive.
DEBUG "Program Running!"
' -----[ Constants ]----------------------------------------------------------

' REPLACE THESE VALUES WITH THE VALUES YOU DETERMINED AND ENTERED INTO
' YOUR CALIBRATION TABLE

LeftAmbient CON 108
RightAmbient CON 114
LeftBright CON 20
RightBright CON 22
' Average Scale factor
LeftThreshold CON LeftBright + LeftAmbient / 2 * 5 / 8
RightThreshold CON RightBright + RightAmbient / 2 * 5 / 8

' -----[ Variables ]----------------------------------------------------------
' Declare variables for storing measured RC times of the
' left & right photoresistors.
timeLeft VAR Word
timeRight VAR Word
' -----[ Initialization ]-----------------------------------------------------
FREQOUT 4, 2000, 3000

' -----[ Main Routine ]-------------------------------------------------------
DO
GOSUB Test_Photoresistors
GOSUB Navigate
LOOP

' -----[ Subroutine - Test_Photoresistors ]-----------------------------------
Test_Photoresistors:
HIGH 5 ' Left RC time measurement.
PAUSE 3
RCTIME 5,1,timeLeft
HIGH 3 ' Right RC time measurement.
PAUSE 3
RCTIME 3,1,timeRight
debug home, "L  ", dec5 timeLeft, "  R  ", dec5 timeRight
RETURN

' -----[ Subroutine - Navigate ]----------------------------------------------
Navigate:
IF (timeLeft < LeftThreshold) AND (timeRight < RightThreshold) THEN ' Both are lit, stop
 PULSOUT 13, 750 ' don't move.
 PULSOUT 12, 750
ELSEIF (timeRight < RightThreshold) THEN ' Right is lit
 PULSOUT 13, 1000 ' pivot right.
 PULSOUT 12, 1000
ELSEIF (timeLeft < LeftThreshold) THEN ' Left is lit
 PULSOUT 13, 500 ' pivot left
 PULSOUT 12, 500
ELSE      '  None are lit. Stop
 PULSOUT 13, 750 ' don't move.
 PULSOUT 12, 750
ENDIF
PAUSE 20 ' Pause between pulses.
RETURN