InfraRedNavigation.BS2

' -----[ Title ]--------------------------------------------------------------
' Robotics with the Boe-Bot - RoamingWithIr.bs2
' Adapt RoamingWithWhiskers.bs2 for use with IR pairs.

' {$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) THEN
    HIGH 10
  ELSE
    LOW 10
  ENDIF

  IF (irDetectRight = 0) THEN
    HIGH 1
  ELSE
    LOW 1
  ENDIF

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


' -----[ Subroutines ]--------------------------------------------------------
Forward_Pulse: ' Send a single forward pulse.
PULSOUT 13,850
PULSOUT 12,650
PAUSE 20
RETURN

Turn_Left: ' Left turn, about 90-degrees.
FOR pulseCount = 0 TO 20
PULSOUT 13, 650
PULSOUT 12, 650
PAUSE 20
NEXT
RETURN

Turn_Right:
FOR pulseCount = 0 TO 20 ' Right turn, about 90-degrees.
PULSOUT 13, 850
PULSOUT 12, 850
PAUSE 20
NEXT
RETURN

Back_Up: ' Back up.
FOR pulseCount = 0 TO 40
PULSOUT 13, 650
PULSOUT 12, 850
PAUSE 20
NEXT
RETURN