' -----[ Title ]--------------------------------------------------------------
' NavigateAndStop.bs2
' Navigates to a light source and stops when a switch is closed.

' {$Stamp bs2}                               ' Stamp directive.
' {$PBASIC 2.5}                              ' PBASIC directive.

' -----[ Constants ]----------------------------------------------------------

' REPLACE THESE VALUES WITH THE VALUES YOU DETERMINED AND ENTERED INTO
' TABLE 6.1.
' set LeftAmbient and Right ambient to compensate for background light
LeftAmbient    CON     15
RightAmbient   CON     15
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




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

DO

  GOSUB Test_Photoresistors
  GOSUB Navigate
' switch is on pin 5. If the switch is closed leave the loop
  IF (IN5 = 1) THEN
    GOTO LIFT_SOURCE
  ENDIF
LOOP
' Add code to lift the source and continue to the lab to drop it off
LIFT_SOURCE:
END

' -----[ Subroutine - Test_Photoresistors ]-----------------------------------

Test_Photoresistors:

  HIGH 6                                     ' Left RC time Measurement.
  PAUSE 3
  RCTIME 6,1,timeLeft

  HIGH 3                                     ' Right RC time measurement.
  PAUSE 3
  RCTIME 3,1,timeRight

  RETURN

' -----[ Subroutine - Navigate ]----------------------------------------------

Navigate:

  IF (timeLeft < LeftThreshold) AND (timeRight < RightThreshold) THEN
    PULSOUT 13, 850                          ' Both detect flashlight beam,
    PULSOUT 12, 650                          ' full speed forward.
  ELSEIF (timeLeft < LeftThreshold) THEN     ' Left detects flashlight beam,
    PULSOUT 13, 700                          ' pivot left.
    PULSOUT 12, 700
  ELSEIF (timeRight < RightThreshold) THEN   ' Right detects flashlight beam,
    PULSOUT 13, 800                          ' pivot right.
    PULSOUT 12, 800
  ELSE
    PULSOUT 13, 750                          ' No flashlight beam, sit still.
    PULSOUT 12, 750
  ENDIF

  PAUSE 20                                   ' Pause between pulses.

  RETURN