TactileNavigation.BS2

' -----[ Title ]--------------------------------------------------------------
' An example of the robot Boe-Bot with whiskers according the chapter 5
' Boe-Bot uses whiskers to detect and avoid obstacles.
' {$STAMP BS2}                        ' Which Stamp processor to use.
' {$PBASIC 2.5}                       ' Which version of PBASIC to use.


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

Counter VAR Byte                      ' counter for FOR/NEXT loop

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

 Left   PIN    5                      ' Left whisker contact
Right   PIN    7                      ' Right whisker contact

INPUT Left
INPUT Right


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

DO                                    ' Neverending loop

  IF (Left = 0) AND (Right = 0) THEN  ' Front obstacle
    GOSUB Go_Back                     ' Back up & U-turn (left twice)
    GOSUB Go_Left
    GOSUB Go_Left
  ELSEIF (Left = 0) THEN              ' Obstacle on left
    GOSUB Go_Back                     ' Back up & turn right
    GOSUB Go_Right
  ELSEIF (Right = 0) THEN             ' Obstacle on right
    GOSUB Go_Back                     ' Back up & turn left
    GOSUB Go_Left
  ELSE                                ' Nothing in front
    GOSUB Go_Forward                  ' Apply a forward pulse
  ENDIF

LOOP                                  ' and repeat again

' -----[ Subroutines ]--------------------------------------------------------

Go_Forward:                           ' Send a single forward pulse.
  PULSOUT 13,850
  PULSOUT 12,650
  PAUSE 20
RETURN

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

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

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