# "May I Go?" Navigation

So I have coded Walter, when in RC mode, to do a sonar sweep and put the results on my LCD screen. At each servo position, the distance sensed is displayed as sort of a bar graph. There are 16 servo steps on the sonar sweep corresponding to 16 charicter spaces on the display. This is exactly the same system as Frits' "See what robot sees" just with a lcd screen instead of on the pc screen. This system now allows me to drive around and point Walter at different situations, a doorway, a chair, straight into a wall etc. and see exactly what the sonar is seeing.

On to step 2:

I am thinking of a new approch to navigation... "May I go" instead of a system of constant driving with a "if you hit something, turn away from it". In "May I go", the drive part of the code has to wait for permission in order to make a move. I.e. the sonar makes a sweep, it is determined the path is clear for a given distance and the drive system is allowed to move that much. In addition, if different situatuions come up (the sonar sees a doorway or hall, etc.) the drive system is allowed/ told to/ give a choice to go that way.

Step 3 and the question:

I now have 16 chunks of data that my human eye and brain can see on a screen. I also have these 16 chunks of data (forming an electronic picture) say, stored on an EEPROM or scratchpad memory.  ...And the \$64,000 question is coding and algorithms for the robot to now go through this data and determine paths, openings etc. This is a bit madening, I can see the dip in the bargraph, which is obviously a doorway, but how do I tell the robot that the 5 sonar steps in the middle of the sweep (the ones with really far distance readings) is a door? Really this just comes down to a bunch of comparisons but really, 16 variables? --Gotta be a better way.

This should be a very open question/answer and I expect some non-specific answers, just some ideas on directions to go. --consider this a thought excercise.

I will post a video of the system I am using as soon as Kari gets home with the digital...

## Comment viewing options

Select your preferred way to display the comments and click "Save settings" to activate your changes.
I have begun to implement this in mark 2.5 but I am at a loss as to the comparison method. perhaps compare the individual values to eachother, tournament style, or against an average. How would I tell the greatest number from 16 variables?

It`s pretty easy to do this, rik explained it perfectly with his straws analogy. You want to go through the whole array of 16 values checking each one against the previous. The first straw you check will of course be the best fit at first because you have nothing to compare against yet.

If you`re using your Arduino you can do it very easily like this..

int longestStrawLocation = 0;    // the location of the longest straw in the array. We just started, so its 0.
for(int x = 0; x <= 15; x++)
{
if(strawArray[x] >= strawArray[longestStrawLocation]) longestStrawLocation = x;
}

After the loop is done, longestStrawLocation should hold the array location of the longest straw and the most recent if more than 1 are the same length.

Imagine going through a heap of straws. Pick one up. Compare it to the one you are already holding. Is it longer? Keep the new one and toss the old one away. Not longer? Toss the new one.

Pick another straw and continue until the entire heap is gone. You will be holding the longest straw in the heap.

You could do the same for the shortest, thickest, thinnest, smelliest, bendiest, strawiest or berriest. You wouldn't even need to process the entire heap several times. Just compare each new straw against the shortest, thickest, ...., etc. before deciding what to do with it.

Check my other comment. It has example code for picaxe basic.

So far it seems that you are just looking for pathways big enough for Walter to go through. I tend to agree with Riks approach for this. Looking for a block of values wide enough for Walter (allowing for the distance at which the readings are taken).

The next BIG querstion, "why should I go" or perhaps, "will going there be good or bad"

Something like this? This is an imaginary example. I inverted the graph. Dots are empty space.

`     ...    .....   .................    ..................................`

`3334566654222233       ^`

The 16 numbers are distance readings. The "mountain of dots" is a void in the room. My Mr Basic would always go for the biggest number, the direction with the most room (indicated with a little ^). It is that stupid. Or perhaps "simple". Simple can be good, right?

You are asking for a slightly more sophisticated approach. How about searching for rectangles? Here is the same situation:

`     :::    .:::.   ..``:::``.......``:::``..    .......``:::``.............``:::``........`

`3334566654222233      ^^^`

The three readings "greater than five" are highlighted. As your code "sweeps" through these readings, it must remember how long ago the readings started to get >5. When they get <=5 again, the rectangle of interest has been found. The code now knows it starts at direction 6 and ends at direction 8. It must be 3 directions wide (3 servo stops).

The "cut off limit" of "five" may seem arbitrary, or conveniently chosen to suit this example. But it need not be. You can sweep the readings twice. The first time to assess the maximum distance read. The second time can assess where the readings are > 90% of the maximum.

This approach is only slightly more sophisticated. It does not map the room at all. The readings can be dropped as soon as Walter decides how to proceed.

Al I did differently was draw "infinite" values with dots, instead of whitespace.

That is a great bit of info... the double sweep. First sweep gives you a baseline and a baseline of the longest distances... from there you refine based on the longest of the longest... Damn good work.

This might be as simple as putting the values in to an EEPROM and at the end of the sweep, read them back. Compare 1st to 2nd, winner gets re-written to the eprom at a special address. The winner then gets compared with the 3rd and on and on. The winner (or winners) of each compare-round get written to this eeprom address. After 16 rounds of comparison, you have your final winner.

Good start, just gotta figure out corners etc...

It occurred to me that the first of the sweeps gives you more interesting data. Not just the absolute maximum reading, but also the average reading. This is relevant because that will give you a relative baseline. Is that a hole in a nearby wall, or just a piece of horizon that's even further away than all my other readings?

Here's my proposed code, in untested, make belief syntax:
I am using the word "segment" for a position on the horizon, numbered 0 through 15. Sixteen in total.

`servoposition = 0servo servopin, servopositiondist_max = 0segm_max = 0dist_avg = 0dist_tot = 0; sweep 1 = actually sweeping with the servofor segment = 0 to 15   servoposition = segment * 9 + 78    ; this will set servo at 78, 87, 96, 105, 114, 123, 132, 131, 140, 159, 168, 177, 186, 195, 204, 213, 222   servopos servopin, servoposition   distance = analogpin                ; 0 for inyourfacety, 255 for infinity ``   put segment, distance               ; store the reading in eeprom/scratchball whatever``   dist_tot = dist_tot + distance``   if (distance > dist_max) then;     dist_max = distance              ; we don't really need this one right now      segm_max = segment               ; because we can find it here later on   endifnext segmentget segm_max, dist_max                 ; we check the memory for the absolute maximumdist_avg = dist_max / 16               ; repair this code for integer math and desired precision; at this point we have an average and an absolute maximum distance; the difference between these two values is an indication of how much variation we found across the entire horizon; any reading around the dist_avg value is probably boring; an old fashioned obstacle avoider would just go there unless it is too close for comfort; where "comfort" probably lies somewhere between distances 1 and 80; a gaping hole in a series of average readings would have to show significantly higher readings than dist_avg ; let's go for an arbitrary threshold of 120% of dist_avg; no wait, that might be "to inifinity and beyond", if dist_avg is close to dist_max; that would be a stupid value to search for; let's take (dist_max - dist_avg) into account: let's put the threshold, say, halfway between the twothreshold = ``(dist_max - dist_avg) * .5      ; mind the integer dummies from rev-ed!threshold = threshold + dist_avg`
`; sweep 2 = just a stroll through memory lanegap_size = 0max_gap_size = 0max_gap_start = 0for segment = 0 to 15   get segment, distance      ; retrieve distance from memory   if (distance >= threshold)      ; we are strolling through a "gap" in our readings       if (gap_size = 0) then         ; this must be a new gap``         gap_start = segment         gap_size = 1``      else         ; still strolling through the same gap as last segment         gap_size = segment - gap_start      endif``      if (gap_size > max_gap_size) then         max_gap_size  = gap_size          max_gap_start = gap_start      endif ``   else      ; we are not strolling in a "gap" (anymore?)      gap_size = 0    endifnext segment; now we have found the biggest (widest) gap in the series. ; we know it start at max_gap_start and that ; it is max_gap_size segments wide`

Instead of measuring the "size" of a gap by its width (in number of segments), we could also measure it in "surface area". This would be a product of width and depth (segments * distance). We could use the threshold value as some sort of average value for all distances inside the gap, or we could alter the code to sum up all the distances inside the gap.

I hope you enjoyed the code. Note: there are no zeroes and ones involved. And it would be a lot easier to program this in Processing (without all the integer restrictions).

Yup, I put a couple hours staring at your code thoughts and mentally adding them to chunks of my code. Once again, awesome work --I love how your puzzle-solving mind works, rik. However, it was one line at the end that caught my eye... segments*distance   This one equation solves so many issues... I.e if the robot is simply looking for the center of a pack of the farthest distances we just made a robot that is designed to drive directly into a corner. The thought of surface area needed to move catches a lot of funny situations (a chair leg in the middle of an open field for example) and would allow a turn away to happen in time. It is really just the average that is so good.

Lets say 2 chunks of segments show that there are 2 pathways we could use --big thing straight ahead but a right and left path we can steer to... On one path there a little toy or something, that would bring down the average to that side. Hmmmm. Much to think about...

I never considered corners in the room. That would be an (obvious?) pitfall.

My mind is now also filled with thoughts on this. I was hoping the segments*distance would be some indication of how wide a gap is, regardless of the distance to it. Is it a proper doorway or a mouse hole?

But, as you point out, it is not. It's merely an indication of how much free floor space is in front of the sensor. In order to make a useful comparison between values of segments*distance, you would have to weigh segments and distance against each other. Because they have very different ranges as it is (0-15 against 0-255). The distance factor will easily blow the segments factor out of the water.

The proper solution is probably to divide the distance by a number (16 maybe). That number must be determined by experimentation.

In order to really determine the width of an opening, you must measure the distance to the surrounding obstacles, like door posts. That would require some "edge detection" in the code. And that makes for a different and new coding challenge. I really should go to work now.... 8-(