symbol dangerlevel=50 'how far away should things be before we react?
symbol turn=750 'this sets how much should be turned
symbol servo_turn= 300 'this sets for how long time we should wait for the servo to turn
symbol trig= 3 'define output pin for trigger pulse
symbol echo= 6 'define input pin for echo pulse
symbol range= w1 '16 bit word variable for range
servo 0, 150
wait 1
main: ' the main loop
low 2
pulsout trig,2 'produce 20uS trigger pulse (must be minimum of 10Us)
pulsin echo,1,range 'measures the range in 10uS steps
pause 10 'recahrge period after ranging completes
'now convert range to cm (divide by 5.8) or inches (divide by 14.8)
'as picaxe cannot use 5.8, multiply by 10 then divide by 58 instead
let range = range*10/58 ' multiply by 10 then divide by 58
debug range 'display range via debug command
if range > dangerlevel then
gosub nodanger 'if nothing ahead, drive forward
else
gosub whichway 'if obstacle ahead then decide which way is better
end if
goto main 'end of main loop, all others are sub-routines
nodanger: 'drive forward
high 4 : low 5 : high 6 : low 7
return
whichway:
high 2
gosub totalhalt 'first stop!
'look one way
gosub lturn 'look to one side
pause servo_turn ' wait for the servo to be finished turning
pulsout trig,2 'produce 20uS trigger pulse (must be minimum of 10Us)
pulsin echo,1,range 'measures the range in 10uS steps
pause 10 'recahrge period after ranging completes
'now convert range to cm (divide by 5.8) or inches (divide by 14.8)
'as picaxe cannot use 5.8, multiply by 10 then divide by 58 instead
let range = range*10/58 ' multiply by 10 then divide by 58
debug range
w2 = range
gosub totalhalt
'look the other way
gosub rturn 'to another side
pause servo_turn
pulsout trig,2 'produce 20uS trigger pulse (must be minimum of 10Us)
pulsin echo,1,range 'measures the range in 10uS steps
pause 10 'recahrge period after ranging completes
'now convert range to cm (divide by 5.8) or inches (divide by 14.8)
'as picaxe cannot use 5.8, multiply by 10 then divide by 58 instead
let range = range*10/58 ' multiply by 10 then divide by 58
debug range
w3 = range
gosub totalhalt
'decide which is the better way
if w2<dangerlevel and w3<dangerlevel then
gosub backwards
else
gosub decide
end if
return
decide:
if w2<w3 then
gosub body_lturn
else
gosub body_rturn
end if
return
rturn:
servo 0, 200
return
lturn:
servo 0, 100
return
body_lturn:
high 4 : low 5 : high 7 : low 6
pause turn : gosub totalhalt
return
body_rturn:
high 5 : low 4 : high 6 : low 7
pause turn : gosub totalhalt
return
totalhalt:
low 4 : low 5 : low 6 : low 7
servo 0, 150
pause servo_turn
return
backwards:
high 5 : low 4 : high 7 : low 6
pause turn
high 4 : low 5 : high 7 : low 6
pause turn
pause turn
return