roaming with whiskers

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

DEBUG "program running!"


pulseCount VAR BYTE

FREQOUT 4, 2000, 3000


DO
IF (IN5 = 0) AND (IN7 = 0) THEN
GOSUB Back_Up
GOSUB Turn_Left
GOSUB Turn_Left
ELSEIF (IN5 = 0) THEN
GOSUB Back_Up
GOSUB Turn_Right
ELSEIF (IN7 = 0) THEN
GOSUB Back_Up
GOSUB Turn_Left
ELSE
GOSUB Foward_Pulse
ENDIF
LOOP

Foward_Pulse:
PULSOUT 13, 850
PULSOUT 12, 650
PAUSE 20
RETURN

Turn_Left:
FOR pulseCount = 0 TO 20
PULSOUT 13, 650
PULSOUT 12, 650
PAUSE 20
NEXT
RETURN

Turn_Right:
FOR pulseCount = 0 TO 20
PULSOUT 12, 850
PULSOUT 13, 850

PAUSE 20
NEXT
RETURN

Back_Up:
FOR pulseCount = 0 TO 40
PULSOUT 13, 650
PULSOUT 12, 850
PAUSE 20
NEXT
RETURN