ARRRGG! --Code! (FR05 Head Sweep)

christhecarpenter · June 17, 2008, 5:19pm

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?

fritsl · June 17, 2008, 6:11pm

We talked about this

We talked about this before!

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"

christhecarpenter · June 17, 2008, 6:18pm

Hey – I got the case man!!

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!

www.rocketbrandcustom.com baby!!

christhecarpenter · June 17, 2008, 9:19pm

Not to say I don’t want more help…

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!

www.rocketbrandcustom.com baby!!

jka · June 18, 2008, 1:50am

Something along the lines if

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

GobliZ · June 18, 2008, 2:53am

Ok, I probably don’t have

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
head = Remember
Pulsout servopin,head
pause 20
gosub checkareas
next
for Remember = MaxRight to MaxLeft step 200
head = Remember
Pulsout servopin,head
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

‘Get distance ahead
gosub getping

‘head is in areaA
if head < areaALeft and head > areaAright then
low LEDC
CurrentArea = AreaA
if range < Obstacle then
gosub DriveMediumRight
else
gosub DriveForward
endif
endif
‘head is in areaB
if head < areaBLeft and head > areaBright then
low LEDB
CurrentArea = AreaB
if range < TooClose then
gosub DriveBackward
elseif range > TooClose and range < Obstacle
gosub DriveHardRight
else
gosub DriveForward
endif
endif
‘head is in areaC
if head < areaCLeft and head > areaCright then
low LEDB
CurrentArea = AreaC
if range < TooClose then
gosub DriveBackward
elseif range > TooClose and range < Obstacle
gosub DriveHardLeft
else
gosub DriveForward
endif
endif
‘head is in areaD
if head < areaDLeft and head > areaDright then
low LEDA
CurrentArea = AreaD
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 - https://www.robotshop.com/letsmakerobots/user/319/robots

christhecarpenter · June 18, 2008, 6:18pm

I got it guys!!

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

www.rocketbrandcustom.com baby!!

christhecarpenter · June 18, 2008, 6:37pm

One more thought…

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!

https://www.robotshop.com/letsmakerobots/node/316

www.rocketbrandcustom.com baby!!

jip · June 19, 2008, 3:29am

select

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.

jip · June 19, 2008, 3:31am

I would do it in the exact
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 :-).

fritsl · June 22, 2008, 1:38pm

What is FR05, actually?
What is FR05, actually?

christhecarpenter · June 22, 2008, 3:11pm

Not a FR05… SRF05!!!

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

www.rocketbrandcustom.com baby!!

fritsl · June 22, 2008, 3:26pm

A) Where is the "sonar"

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?

christhecarpenter · June 22, 2008, 3:36pm

Well, neener neener…

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?

www.rocketbrandcustom.com baby!!

fritsl · June 22, 2008, 3:41pm

Should it not be b6=b5?I

Should it not be b6=b5?

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

christhecarpenter · June 22, 2008, 3:45pm

Its right there!!!

Here:

(look above)

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

www.rocketbrandcustom.com baby!!

fritsl · June 22, 2008, 3:52pm

Oh… It is because I read it

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

christhecarpenter · June 22, 2008, 4:07pm

Good advice, my friend…

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.

www.rocketbrandcustom.com baby!!

fritsl · June 22, 2008, 4:20pm