Let's Make Robots!

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

Select your preferred way to display the comments and click "Save settings" to activate your changes.

 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!

 http://letsmakerobots.com/node/316

www.rocketbrandcustom.com baby!!

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

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 - http://letsmakerobots.com/user/319/robots

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

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" :) 

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

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.

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


 

www.rocketbrandcustom.com baby!!

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 :-).