' Robotics with the Boe-Bot - FastIrRoaming.bs2
' Higher performance IR object detection assisted navigation

' {$STAMP BS2}
' {$PBASIC 2.5}

DEBUG "Program Running!"

irDetectLeft   VAR     Bit                   ' Variable Declarations
irDetectRight  VAR     Bit
pulseLeft      VAR     Word
pulseRight     VAR     Word

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

DO                                           ' Main Routine

  FREQOUT 8, 1, 38500                        ' Check IR Detectors
  irDetectLeft = IN9
  FREQOUT 2, 1, 38500
  irDetectRight = IN0
                                             ' Decide how to navigate.
  IF (irDetectLeft = 0) AND (irDetectRight = 0) THEN
    pulseLeft = 650
    pulseRight = 850
  ELSEIF (irDetectLeft = 0) THEN
    pulseLeft = 850
    pulseRight = 850
  ELSEIF (irDetectRight = 0) THEN
    pulseLeft = 650
    pulseRight = 650
  ELSE
    pulseLeft = 850
    pulseRight = 650
  ENDIF
   PULSOUT 13,pulseLeft                       ' Apply the pulse.
  PULSOUT 12,pulseRight
  PAUSE 15

LOOP                                         ' Repeat Main Routine