twitch
thanks for your comments
here is the code it might be a bit rough around the edges but it seems to work
Symbol dangerlevel = 40’ how far away should thing be, before we react?
Symbol turn = 300 ’ this sets how much should be turned
Symbol servo_turn = 700 ’ This sets for how long time we should wait for the servo to turn (depending on it’s speed) before we measure distance
wait 5
if pin2= 0 then gosub line
goto main
main: ’ the main loop
readadc 1, b1 ’ read how much distance ahead
if b1 < 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 ’ this ends the loop, the rest are only sub-routines
nodanger:’ this should be your combination to make the robot drive forward, these you most likely need to adjust to fit the way you have wired your robots motors
high 4 : high 7 : low 5 : low 6
return
whichway:
gosub totalhalt ’ first stop!
‘Look one way:
gosub lturn ’ look to one side
pause servo_turn ’ wait for the servo to be finished turning
gosub totalhalt
readadc 1, b1
’Look the other way:
gosub rturn ’ look to another side
pause servo_turn ’ wait for the servo to be finished turning
gosub totalhalt
readadc 1, b2
’ Decide which is the better way:
if b1<b2 then
gosub body_lturn
else
gosub body_rturn
end if
return
body_lturn:
low 4 : low 6 : high 7 :high 5 ’ this should be your combination that turns the robot one way
pause turn : gosub totalhalt
return
body_rturn:
high 4 : low 7 : low 5 : high 6 ’ this should be your combination that turns the robot the other way
pause turn : gosub totalhalt
return
rturn:
servo 0, 100 ’ look to one side
return
lturn:
servo 0, 200 ’ look to the other side
return
totalhalt:
low 4 : low 5 : low 6 : low 7 ’ low on all 4 halts the robot!
Servo 0,150 ’ face forward
wait 1 ’ freeze all for one second
return
line:
servo 0,150 ’ head forward
high 4 low 5 high 7 low 6 'drive forward
readadc 1,b1 'read eyes
readadc 2,b2 'read left line sensor
readadc 3,b3 'read right line sensor
if b2>250 and b3<250 then gosub right 'turn right
if b3>250 and b2<250 then gosub left 'turn left
if b2>252 and b3>252 then gosub allstop ’ all stop
if b1>65 then gosub uturn ’ do a u turn
goto line
left:
high 7 low 6 low 4 high 5 'turn left
pause 100 'wait 100 mili sec
goto line
right:
high 4 low 5 low 7 high 6 ‘turn’ right
pause 100 ’ wait 100 milli sec
goto line
uturn:
readadc 2,b2 'read line sensor
low 4 low 5 low 7 low 6 ’ stop all motors
wait 1 'wait 1 second
gosub uturn1
uturn1:
readadc 2,b2 'read left line sensor
high 7 low 6 high 5 low 4 ‘turn right
if b2>251 then ’ find the edge of the line
pause 500’ cross line
else goto uturn1 ’ keep turning until the line is found
endif
if b2>251 then
low 4 low 5 low 7 low 6 'stop when the other side of the line is found
pause 500
endif
goto line
allstop:
low 4 low 5 low 6 low 7 ’ stop all motors
servo 0,125 ’ shake head
pause 500
servo 0,175 ’ shake head
pause 500
goto allstop