# ARRRGG!! --Code! (FR05 Head Sweep)

Man am I stumped...

What I want to do is pretty simple, but I just can't seem to figure out which direction to go - I have a FR05 on top of a servo in the standard "look right/ look left" arrangement. I also have an encoder on one of my wheels so the robot knows how far it has turned (the code for the encoder is solid and works great). I want the head to do a sweep of a room and decide which direction is the farthest away and then turn the body that way. --As the body is turning, I want the head to rotate the opposite way so the head seems to stay still while the body turn under it.

Now,

I have the code for the encoder

I can make the servo turn, no problem...

How do I code the "what is the farthest away" thing??!?

I'm not looking to be a Code-Mooch (have someone write it for me) I am just looking for a new way to look at this problem...

Any ideas?

## Comment viewing options

Well, I have gotten a little close, but the original question stands...

I have thought about something like this: (But it still sucks)

symbol trig = 5
symbol echo = 1

sweepRL:
for b1 = 75 to 175 step 1
if b1 = 76 then gosub distancetest1
if b1 = 100 then gosub distancetest2
if b1 = 120 then gosub distancetest3
if b1 = 140 then gosub distancetest4
if b1 = 174 then gosub distancetest5
next b1
goto whatisfarther

distancetest:
pulsout trig,1
pulsin echo,1,w1
pause 10
return

distancetest:
pulsout trig,1
pulsin echo,1,w2
pause 10
return

distancetest:
pulsout trig,1
pulsin echo,1,w3
pause 10
return

distancetest:
pulsout trig,1
pulsin echo,1,w4
pause 10
return

distancetest:
pulsout trig,1
pulsin echo,1,w5
pause 10
return

whatisfarther:
let b2=(w1+w2+w3+w4+w5)/5
if w1>b2 then b3=1
if w2>b2 then b3=2
if w3>b2 then b3=3
if w4>b2 then b3=4
ir w5>b2 then b3=5

Something along the lines if this, which I haven't tested:

w2 = 0
b2 = 0
b3 = 0

sweepRL:
;You could replace this with step25 and skip the if part
;if you don't do anything else in the loop, but I suppose you move a servo or something.
for b1 = 75 to 175 step 1
if b1 = 75 or b1 = 100 or b1 = 125 or b1 = 150 or b1 = 175 then
gosub distancetest
gosub checkdistance
end if
next b1

;At this point in the code, w2 = largest distance and b3 = measurement number of largest distance

return  ;from SweepRL

distancetest:
pulsout trig,1
pulsin echo,1,w1
pause 10
inc b2 ;increase the measurement counter
return  ;from distanctest

checkdistance:
if w1 > w2 then ;Is the new distance larger than the largest we found before?
w2 = w1  ;Then remember it
b3 = b2  ;And the measurement number
end if
return  ;From checkdistance

I would do it in the exact same way and use the step 25 since I guess the moving of the servo should only be done at these points. No point in moving the servo to positions you're not going to measure :-).

Remember you asked me what the command "Case" was?

"Select case" - look in the manual, I think that is your answer :)

PS: The code you have dumped will not even compile! There can be only one label with the same name.

And.. may I suggest thet you have all the SRF05-pulseout-stuff in one sub (gosub) - routine, ending with a "return" - and then keep the loop in one main-loop, jumping out to all the "action" :)

Hey Fritz!

I figured out the Case!!! --Not with this problem, though... I am using it with my slide/turn commands and it is working great!

Oh, and I know the code I put up was not complete -it is just an example of the direction I am going. I will put the pulse stuff in a seperate little nook -good idea.

To be honest, I don't know how I am going to stick the case thing into this problem but Thank You, Thank You for a direction to go in! --I'll get it!

--You da man!

Hey Chris,

Whenever you have one of those "if"-constructs with multiple lines where you check one variable for equality to different values (like you do in the for loop in the top of your code), you can replace that with a "select case ... endselect"-construct. I think that was what Frits referred to.

Hey Code Geeks...

When I say "I get it" (in the last comment)  is not to say I still don't want more ideas!! --This is like a huge puzzel or riddle I gotta figure out! --Don't tell me how to do it, just give me hints!

Chris!

Ok, I probably don't have the same microcontroller as you do (I have a Atom Basic Pro). And I don't have the FR05 (I have the ping instead). But here is how I did it.

'------------------------------------------------------------------------------
'-- Obstacle avoidance main subroutine (scan with head)
'-- Head turns 1000 <= x <= 4800, with 5800/2=2900 as center or neutral
'------------------------------------------------------------------------------
scanareas:

for Remember = MaxLeft to MaxRight step -200
pause 20
gosub checkareas
next
for Remember = MaxRight to MaxLeft step 200
pause 20
gosub checkareas
next
Goto scanareas

'------------------------------------------------------------------------------
'-- Obstacle avoidance check areas subroutine (what action to be taken)
'-- First get distance ahead. Then check in which area the head is, and
'-- take the appropriate action.
'------------------------------------------------------------------------------
checkareas:
high LEDA
high LEDB
high LEDC

gosub getping

low LEDC
CurrentArea = AreaA
if range < Obstacle then
gosub DriveMediumRight
else
gosub DriveForward
endif
endif
low LEDB
CurrentArea = AreaB
if range < TooClose then
gosub DriveBackward
elseif range > TooClose and range < Obstacle
gosub DriveHardRight
else
gosub DriveForward
endif
endif
low LEDB
CurrentArea = AreaC
if range < TooClose then
gosub DriveBackward
elseif range > TooClose and range < Obstacle
gosub DriveHardLeft
else
gosub DriveForward
endif
endif
low LEDA
if range < Obstacle then
gosub DriveMediumLeft
else
gosub DriveForward
endif
endif
return

'------------------------------------------------------------------------------
'-- Getping
'-- returns the distance measured by the ping))) ultrasonic sound sensor
'-- rawDist is the raw distance measured by the ping
'-- to get the distance in centimeters, rawdist is divided by 29.034
'------------------------------------------------------------------------------
getping:
low ping
high ping
pulsin ping, 0, rawDist
low ping
rawDist = rawDist / 2
range = rawDist / 29  'should be 29.034
return

'------------------------------------------------------------------------------
'-- Drive subroutines
'-- Sends pulses to the servo's to make them move
'------------------------------------------------------------------------------
DriveForward:
pulsout rightwheel,maxright
pulsout leftwheel,maxleft
pause 20
return

DriveBackward:
do
gosub getping
pulsout rightwheel,maxleft
pulsout leftwheel,maxright
pause 20
while range < TooClose
return

DriveMediumLeft:
do
gosub getping
pulsout rightwheel,maxright
pause 20
while range < Obstacle
return

DriveHardLeft:
do
gosub getping
pulsout rightwheel,maxright
pulsout leftwheel,maxright
pause 20
while range < Obstacle
return

DriveMediumRight:
do
gosub getping
pulsout leftwheel,maxleft
pause 20
while range < Obstacle
return

DriveHardRight:
do
gosub getping
pulsout leftwheel,maxleft
pulsout rightwheel,maxleft
pause 20
while range < Obstacle
return

Check out my robots - http://letsmakerobots.com/user/319/robots

Hey, Much Love Folks...

I have the basic code down now -To the point of, I know what it is doing and why it is doing it! I need to tidy it up a little but it is solid! Thanks Again, guys.

On another note:

I just found that picaxe has a random command... I think I am going to use it so the robot will stop at random times and have a look around. In trying to give this little robot some personality, I think it would be more "human" if it doesn't stick to a "see something, drive around it" sorta schedual.

One more:

I have 2 more IR distance sensors on the way! This baby is going to have R/L forward sensors AND R/L side sensors PLUS the FR05! YEAH!!!

So I have been working on this whole code thing because I thought I wanted the robot to drive into open spaces -Drive away from objects... Well, I am begining to think I might just want to do the opposite! No offence Fritz, but why is >300 "interesting"? I think the coolest part of navigation is steering around stuff so maybe I should have it look for stuff that is sorta close and drive to it -at that point the close-up sensors can take over.

Fritz (and all else),

I ran your "see what robot sees" code tonight. Absolutly Fantastic! I am having crazy "find an edge" code ideas! --So many possibilities... Thanks man.

Everyone Else,

GO CHECK THIS OUT NOW!