' -----[ Title ]--------------------------------------------------------------
' Robotics with the Boe-Bot - RoamingTowardTheLight.bs2
' Boe-Bot roams, and turns away from dark areas in favor of brighter areas.
' {$STAMP BS2} ' Stamp directive.
' {$PBASIC 2.5} ' PBASIC directive.
DEBUG "Program Running!"
' -----[ Variables ]----------------------------------------------------------
' Declare variables for storing measured RC times of the
' left & right photoresistors.
timeLeft VAR Word
timeRight VAR Word
average VAR Word
difference VAR Word

' -----[ Initialization ]-----------------------------------------------------
FREQOUT 4, 2000, 3000

' -----[ Main Routine ]-------------------------------------------------------

DO
GOSUB Test_Photoresistors
' For mismatched photoresistors, use Appendix F, uncomment and use next line.
' timeLeft = (timeLeft */ 351) + 7 ' Replace 351 and 7 with your own values.

GOSUB Average_And_Difference
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
RETURN

' -----[ Subroutine - Average_And_Difference ]--------------------------------
Average_And_Difference:
average = timeRight + timeLeft / 2
difference = average / 6
RETURN

' -----[ Subroutine - Navigate ]----------------------------------------------
Navigate:
' Shadow significantly stronger on left detector, turn right.
IF (timeLeft > timeRight + difference) THEN
  PULSOUT 13, 1000
  PULSOUT 12, 1000
' Shadow significantly stronger on right detector, turn left.
ELSEIF (timeRight > timeLeft + difference) THEN
  PULSOUT 13, 500
  PULSOUT 12, 500
' Shadows in same neighborhood of intensity on both detectors.
ELSE
  PULSOUT 13, 1000
  PULSOUT 12, 500
ENDIF
PAUSE 10
RETURN