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

'Sample program to navigate with whiskers from Robotics, Version 2.2, pp. 179-180
pulseCount VAR Byte                  'Loop counter for FOR .. NEXT loops
DO
  IF (IN5 = 0) AND (IN7 = 0)THEN     'Both whiskers detect obstacle
    GOSUB Back_Up                    'Back up and do U-turn
    GOSUB Turn_Left
    GOSUB Turn_Left
  ELSEIF (IN5 = 0) THEN              'Left whisker detects obstacle
    GOSUB Back_Up                    'Back up and turn right
    GOSUB Turn_Right
  ELSEIF (IN7 = 0) THEN              'Right whisker detects obstacle
    GOSUB Back_Up                    'Back up and turn left
    GOSUB Turn_Left
  ELSE                               'No contact with whiskers
    GOSUB Forward_Pulse              'Go straight forward
  ENDIF                              'and check again
LOOP
' ----- Subroutines -------------------------------------------------
Forward_Pulse:                       'Go straight ahead
  PULSOUT 13, 850                    'Left wheel full speed CCW
  PULSOUT 12, 650                    'Right wheel full speed CW
  PAUSE 20
  RETURN

Turn_Left:                           'Turn left about 90 degrees
  FOR pulseCount = 0 TO 20
    PULSOUT 13, 650                  'Right wheel full speed CW
    PULSOUT 12, 650                  'Right wheel full speed CW
    PAUSE 20
  NEXT
  RETURN

Turn_Right:                          'Turn right about 90 degrees
  FOR pulseCount = 0 TO 20
    PULSOUT 13, 850                  'Right wheel full speed CCW
    PULSOUT 12, 850                  'Right wheel full speed CCW
    PAUSE 20
  NEXT
  RETURN

Back_Up:                             'Back up a little bit
  FOR pulseCount = 0 TO 40
    PULSOUT 13, 650                  'Left wheel full speed CW
    PULSOUT 12, 850                  'Right wheel full speed CCW
    PAUSE 20
  NEXT
  RETURN