Ok
Ok thanks, i have modified the clean navigation code very lightly… seems that the robot want to drive about in a small area, after a few tweaks i think i will have it going properly.
‘==’
’ Set these up to match your Hardware
Symbol servopin = 0’ Defines output pin to which the servo turning the head is connected
’- And in the very last part of the code, the 3 routines “RightSpin”,“Leftspin” and “Driveahead”
’==’
’================ Symbols and variables set up to the performance you whish. You should optimize these to your robot. I am not sure of the remarks, it is just as I remember, may not be excact 
symbol ForwardLeft = 100 ’ These set where “green and red” (See ilustration on https://www.robotshop.com/letsmakerobots/node/25) starts / ends
symbol ForwardRight = 200 ’ -||- They should be trimmed so obstacles arejust avoided, and be on each side of “150” which is "servo-midle"
symbol Danger = 200 ’ Distance from which we say "Oops - too close, reverse a little"
symbol Safatydistance = 250 ’ Distance from which we say "OK, now we have reversed enough, let?s get on with it"
symbol ClearPathAhead = 300 ’ Distance needed before starting again after a stop
symbol interesting = 500 ’ Variable that sets how deep a space we need before we can call it “interesting” (such as a doorway in a wall)
symbol MaxNumberTurns = 5 ’ In case we are in a wide area, in the mIddle of a room, so all is “interesting” - how many times should we turn before we just take an “interesting route”
’================
’================ Symbols just used within the code, no change here
symbol head = b1
symbol range = w1 ’ 16 bit word variable for range
symbol Remember = w2 ’ Temorary variable just used in routines
symbol MayTurn = w3 ’ An undocomented variable
’================
‘PHASEA1 is Main routine.
‘Drive ahead!
‘Pan head from left to right, and back
’Constantly return sonar range, and jump out into the rest of the code…
PHASEA1:
for Remember = 75 to 225 step 20
head = Remember
servo Servopin,head
pause 15
gosub tester
next Remember
for Remember = 225 to 75 step -20
head = Remember
servo Servopin,head
pause 15
gosub tester
next Remember
Goto PHASEA1
’’’’’’’’’’’’’’’’’’’’’’’’’’‘
PHASEA2:
If head => ForwardLeft and head < 150 and range < Danger then goto ForwardLeft_middle
If head => 150 and head <= ForwardRight and range < Danger then goto ForwardRight_middle
If head => ForwardRight and head <= 225 and range > ClearPathAhead then goto Sharp_R
If head => 75 and head < ForwardLeft and range > ClearPathAhead then goto Sharp_L
goto ERROR ’ one of the above should be true, if not, something is wrong, this code will self-terminate ! 
’’’’’’’’‘Danger’’’’’’’’’’’’’’’’’’’’‘
ForwardLeft_middle:
Gosub RightSpin
if head > ForwardLeft and head < 150 then
head = head - 15
pause 50
servo Servopin,head
end if
gosub puls
if range < Danger then goto ForwardLeft_middle
gosub afterturn
If MayTurn < MaxNumberTurns then
MayTurn = MayTurn + 1
end if
return
’’’’’’’’’’’’’’’’’’’’’’’’’’’’’‘
ForwardRight_middle:
Gosub Leftspin
if head > 150 and head < ForwardRight then
head = head + 15
pause 50
servo Servopin,head
end if
gosub puls
if range < Danger then goto ForwardRight_middle
gosub afterturn
If MayTurn < MaxNumberTurns then
MayTurn = MayTurn + 1
end if
return
’’’’’’’’’’‘Curious’’’’’’’’’’’’’’’’’’‘
Sharp_R:
If MayTurn > 0 then
Gosub RightSpin
If head < ForwardLeft and head => 75 then
head = head + 30
pause 50
servo Servopin,head
end if
gosub puls
if range > interesting then goto Sharp_R:
gosub afterturn
MayTurn = MayTurn - 1
end if
return
’’’’’’’’’’’’’’’’’’’’’’’’’’’’’‘
Sharp_L:
If MayTurn > 0 then
Gosub leftspin
If head => ForwardRight and head < 225 then
head = head - 30
pause 50
servo Servopin,head
end if
gosub puls
if range > interesting then goto Sharp_L:
gosub afterturn
MayTurn = MayTurn - 1
end if
return
’’’’’’’’’‘’’’’’’’’’’’‘
AFTERTURN:
Gosub Driveahead’ if you are testing your robot, you should write “gosub totalhalt” here.
return
’’’’’’’’’'’’’’’’’’’’’‘
TESTER:
’ Right here might be a good place to insert things that should be looked for all the time - If a bumper is activated etc…
gosub puls
if range < Danger and head > ForwardLeft and head < ForwardRight then
GOSUB AFTERTURN
gosub PHASEA2
end if
if range > ClearPathAhead and head > ForwardRight and head <= 225 then
GOSUB AFTERTURN
gosub PHASEA2
end if
if range > ClearPathAhead and head < ForwardLeft and head >= 75 then
GOSUB AFTERTURN
gosub PHASEA2
end if
’’’’’’’’’‘’’’’’’’’’’’‘
puls:
LOW PORTC 0
HIGH PORTC 0
PAUSE 1
LOW PORTC 0
LET DIRSC = %00000000
PULSIN 0, 1, w1
return
’’’’’’’’’'’’’’’’’’’’’‘
ERROR:
gosub totalhalt
wait 1
goto ERROR
’’’’’’’’’‘’’’’’’’’’’’’
’=***************************************=’
’ Set these up to match your hardware
totalhalt:
low 4
low 5
low 6
low 7
return
RightSpin:’ if you are using slow tracks, you may just want to have the “slide right” and “return” in here to get a faster robo
select case range
Case 0 to 50
high 4 : low 5 : high 6 :low 7 ’ Reverse
Case 51 to 150
low 5 : high 4 : high 7 : low 6 ’ Spin right.
case > 150
low 5 : low 4 : high 7 : low 6 ’ Slide right.
endselect
return
Leftspin:’ if you are using slow tracks, you may just want to have the “slide left” and “return” in here to get a faster robo (no select case etc, that is…)
select case range
Case 0 to 50
high 4 : low 5 : high 7 :low 6 ’ Reverse
Case 51 to 150
low 4 : high 5 : high 7 : low 6 ’ Spin left
case > 150
low 4 : high 5 : low 7 : low 6 ’ Slide left
endselect
return
Driveahead:
high 5 : low 4 : high 6 :low 7 ’ This should be the High?s and Low?s that make the robot drive forward.
return
