Chapter 5: Tactile Navigation with Whiskers
· Page 179
√
Try letting the Boe-Bot roam. When it contacts obstacles in its path, it should
back up, turn, and then roam in a new direction.
' -----[ Title ]--------------------------------------------------------------
' Robotics with the Boe-Bot - RoamingWithWhiskers.bs2
' Boe-Bot uses whiskers to detect objects, and navigates around them.
' {$STAMP BS2} ' Stamp directive.
' {$PBASIC 2.5} ' PBASIC directive.
DEBUG "Program Running!"
' -----[ Variables ]----------------------------------------------------------
pulseCount VAR Byte ' FOR...NEXT loop counter.
' -----[ Initialization ]-----------------------------------------------------
FREQOUT 4, 2000, 3000 ' Signal program start/reset.
' -----[ Main Routine ]-------------------------------------------------------
DO
IF (IN5 = 0) AND (IN7 = 0) THEN ' Both whiskers detect obstacle
GOSUB Back_Up ' Back up & U-turn (left twice)
GOSUB Turn_Left
GOSUB Turn_Left
ELSEIF (IN5 = 0) THEN ' Left whisker contacts
GOSUB Back_Up ' Back up & turn right
GOSUB Turn_Right
ELSEIF (IN7 = 0) THEN ' Right whisker contacts
GOSUB Back_Up ' Back up & turn left
GOSUB Turn_Left
ELSE ' Both whiskers 1, no contacts
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
