Boe-Bot program
' {$STAMP BS2} ' Stamp directive.
' {$PBASIC 2.5} ' PBASIC directive.
'-----[ Title ]--------------------------------------------------------------
'Robotics with the Boe-Bot - RoamingWithWhiskers.bs2
'Boe-Bot uses whiskers to detect objects, and navigates around them.
DEBUG " Progam 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
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