Roaming with Photoresistors and Whiskers

JimTheProgrammer

Here is a routine I wrote. Would like to share it to everyone!

Jim



' Robotics with the Boe-Bot - RoamToLightWithWhiskers.bs2
' Boe-Bot uses elements from RoamtoLight and Whisker Navigation
' {$STAMP BS2}
' {$PBASIC 2.5}

DEBUG "START PROGRAM",CR

timeleft VAR Word
timeright VAR Word
average VAR Word
difference VAR Word
pulsecount VAR Byte

FREQOUT 4, 500,4000
GOSUB forward
DO
··· GOSUB test_photoresistors
··· GOSUB average_and_difference
··· GOSUB navigate
LOOP

test_photoresistors:
HIGH 6
PAUSE 3
RCTIME 6,1,timeleft
HIGH 3
PAUSE 3
RCTIME 3,1,timeright
DEBUG HOME, "timeright =· ", DEC5 timeright· ," timeleft =· ", DEC5 timeleft
RETURN

average_and_difference:
average = timeright + timeleft / 2
difference = average / 6
RETURN
navigate:
DEBUG HOME, "NAVIGATE", CR
··· IF (timeleft > timeright + difference) THEN
····· PULSOUT 13,850
····· PULSOUT 12,850
··· ELSEIF (timeright > timeleft + difference) THEN
····· PULSOUT 13,650
····· PULSOUT 12,650
··· ELSE
····· 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 forward
····· ENDIF
··· ENDIF
··· PAUSE 10
RETURN

turn_left:
DEBUG HOME, "TURN LEFT", CR
FOR pulsecount = 0 TO 20
··· PULSOUT 13,650
··· PULSOUT 12,650
··· PAUSE 20
NEXT
RETURN

turn_right:
DEBUG HOME, "TURN RIGHT", CR
FOR pulsecount = 0 TO 20
··· PULSOUT 13,850
··· PULSOUT 12,850
··· PAUSE 20
NEXT
RETURN
back_up:
DEBUG HOME,· "BACK UP", CR
FOR pulsecount = 0 TO 40
··· PULSOUT 13,650
··· PULSOUT 12,850
··· PAUSE 20
NEXT
RETURN

forward:
DEBUG HOME, "FORWARD", CR
PULSOUT 13,850
PULSOUT 12,650
PAUSE 20
RETURN

Special_K

nice code.
thanks for sharing

JimTheProgrammer

Thank you!

Jim