# 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??!?

So far I have tried having the servo stop in 5 different spots, take a reading and then I have about 900 lines of "if w1>w2 then b1=1 : if w2>w1 then b1=2 : if w3>w1 then b1=3 : if w1 > w3 then b1=1 : if w3>w2 then b1=3 : if w2>w3 then b1=2..........." etc. This code would probably work but it is a mess, WAY too long and uses a ton of available variables. There is got to be an easier way! In addition, it would be great that, instead of the servo taking reading at only a few spots it would be awesome if I could tie the distance readings in with the actual servo positions. --For example: for sweep = 75 to 175 step 1, take reading, next sweep --I was also thinking the FR05 could take a reading every step and then just compare the reading to the last reading it took --The head might have to do a few sweeps back and forth to "zero in" on the fartherst away spot but I could do it with just 2 variables used over and over.

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

I don't like the look of that. I hate not knowing what's going on. I'm going to do it from scratch in assembler.
What is FR05, actually?

Sorry SRF05...

So here's a little update, folks.

I thought I had the "what is farther away" code thing down, but I am still running in to problems... I have tried a ton of stuff and if really just comes down to the if w1>w2 then let w2=w1 thing. (If the second thing you look at is farther away then remember it)

For some reason, when I run a debug, w1 comes in at a range that falls into what the SRF05 should be reading, however w2 which should be the "remember the last value (if it is bigger)) is coming in at about 48000. -A value that could not be spit out by the sonar distance sensor. I know I am missing something very small here.

Enclosed is the code I am working on if you guys want to take a look -Again please don't re-write it for me, just make suggestions... I have got to figure this out myself!

**Servo 4 swings the sensor back and forth

servo 4,75
pause 1000
let w2 =0

main:
for b5=75 to 175 step 25
servo 4,b5
pause 500
gosub check
if b5=175 then gosub ifoundit
next b5

sonar:
pulsout 5,1
pulsin 1,1,w1
pause 10
return

check:
gosub sonar
if w1>w2 then let b5=b6
end if
if w1>w2 then let w2=w1
end if
debug
return

ifoundit:
servo 4,b6
pause 5000
return

A) Where is the "sonar" sub-routine? I think it may contain a problem.

B) Take a look at this strange piece of code:

if w1>w2 then let b5=b6
end if
if w1>w2 then let w2=w1
end if

- That would be

if w1>w2 then
b5=b6
w2=w1
end if

.. but b6 = "null, zero, nothing"? Should it not be b6=b5?

If you fix that, your "Main" routine will count up, and then go to the last point where range was higher than before. But that would also mean that any "noise" would mark this point.

It is not enough to realize that one reading is bigger than another, there has to be a certain minimum value for "detection", or you will get unpredictable / late results at "ifoundit".

C) Does this mean you will spell my name right as well?

:D

Spelling is over-rated.

Thanks for the suggestions.

I thought that the b5=b6 vs. b6=b5 would make a difference but i didn't know that  both the b5=b6 AND the w2=w1 would be covered by one "If" command -I guess it is all covered until the end if.

if w1>w2 then
b5=b6
w2=w1
end if

-------------

As for the sonar command, it is a subroutine from the check subroutine. Is there a better place to put it?

Should it not be b6=b5?

I want to SEE the sonar-sub-routine - it must have some variables-messup!

Here:

(look above)

sonar:
pulsout 5,1
pulsin 1,1,w1
pause 10
return

Oh.. It is because I read it like a Picaxe would do..

main.. Where does that end?

After this line is "true":

if b5=175 then gosub ifoundit

comes 5 seconds.. and then trouble! (Hint)

:)

My general advice; Try to erase the whole thing. Make one thing work 100%, and THEN write the next. You have several little bugs left as I can see.. Make one routine work 100% before adding another.

Your code will hit "Return" without "Gosub" - that makes straange things :)

I know, I know Baby Steps...

I will do what you say and try to rebuild this little by little. I swear I am going to get this!!

Thanks again, my friend.

:)