' -----[ Title ]--------------------------------------------------------------
' Robotics with the Boe-Bot - RoamingWithIr.bs2
' Adapt RoamingWithWhiskers.bs2 for use with IR object detectors.
' {$STAMP BS2}                               ' Stamp directive.
' {$PBASIC 2.5}                              ' PBASIC directive.

DEBUG "Program Running!"

' -----[ Variables ]----------------------------------------------------------

  irDetectLeft  VAR     Bit
  irDetectRight VAR     Bit
  pulseCount    VAR     Byte

' -----[ Initialization ]-----------------------------------------------------

FREQOUT 4, 2000, 3000                        ' Signal program start/reset.

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

DO
  FREQOUT 8, 1, 38500                        ' Store IR detection values in
  irDetectLeft = IN9                         ' bit variables.

  FREQOUT 2, 1, 38500
  irDetectRight = IN0

  IF (irDetectLeft = 0) AND (irDetectRight = 0) THEN
    GOSUB Back_Up                            ' Both detect obstacle
    GOSUB Turn_Left                          ' Back up & U-turn (left twice)
    GOSUB Turn_Left
  ELSEIF (irDetectLeft = 0) THEN             ' Left detects
    GOSUB Back_Up                            ' Back up & turn right
    GOSUB Turn_Right
  ELSEIF (irDetectRight = 0) THEN            ' Right detects
    GOSUB Back_Up                            ' Back up & turn left
    GOSUB Turn_Left
  ELSE                                       ' None detect
    GOSUB Forward_Pulse                      ' Apply a forward pulse
  ENDIF                                      ' and check again
LOOP