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