Navigation with (2) sonars Update 2 (with data)

*****Update 2.16.10*****

Got some data to start thinking about, guys. My thoughts of getting nice, even packs of 4 or 5 is a little out the window. To be honest, I tried to forget that I could see where the robot was facing and tried to make a decision where to go based on just the readings. I couldn't . Please note that I was seeing the readings on a LCD so left was above right not in a straight line. --Now that I think of it, I have no idea why I didn't line them up as it is a 16x2 display. Huh.

All readings were taken L to R

Now, here's what I did, I added some scraps of drywall that I had around the kitchen table's base. I didn't want any sensor holes, just smooth walls just to keep this as basic as I could. So here we go, the data is under each photo and the 3 readings were at 1000, 750 and 500.

@1000

L = 00000000

R = 11100000

@750

L = 00000111

R = 11110000

@500

L = 00011111

R = 11111111

@1000

L = 00000000

R = 00000000

@750

L = 00000000

R = 00000111

@500

L = 11111000

R = 00001111

@1000

L = 00000000

R = 01111001

@750

L = 00000000

R = 11111101

@500

L = 10000001

R = 11111101

@1000

L = 00000000

R = 11000000

@750

L = 00000000

R = 11000011

@500

L = 00001111

R = 11111110

****Original Post*****

Need some brainstorming here, folks...

Walter is just friggin big and because of his size, he needs some "side checks" i.e. one sonar is just not enough to keep an eye on his total width. In the past, I have used one center-mounted sonar with one sharp on each side. The sonar was in charge while the sharps would catch the walls. Basically, the sharps would catch a wall if Walter was coming up on that wall at a shallow angle or was traveling allong a wall (not actually following it). This system worked well (except for the God-awful elec. noise from the sharps) with the sonar being a code priority and the sharps "knew their place" in terms of when they could tell the code to change course.

Now:

I have 2 sonars, one each on each of the front two corners. Each sonar can pivot from almost 90 degrees to the side to slightly past center (i.e. the left sensor can see straight out to the left side and, when fwd, can see slightly past the centerline --a little to the right)

Question:

I need some brainstorming here. I have tried about 9 different versions of navigation code with different levels of success and a lot of failures. I have tried:

If one side is closer than the other, the closer is priority -listen to that sensor.

If one sensor is looking to the side, let it skootch off the wall and the fwd sensor is the priority for obsticle avoid.

If one side sees something, let a bit=1 (to remember something WAS there) and then figure that in as to what the other side is seeing.

Average all numbers on one side and compare to the average of all numbers on the other side.

Find a wall, keep that side's sensor fixed on the wall and sweep the other side's sensor for obsticles

Simple avoid whatever either sensor sees no matter what unless both are triggered. Then stop, scan R to L, find the furthest distance, turn to that heading, rescan to see if there is a wide enough path for the bot to fit, turn to the middle of this "path" and go.

etc. etc. etc. etc. etc.

I'm looking for BIG ideas here, folks. No code, just a 30,000' view of the problem. Brainstorm, brainstorm, brainstorm...

****Update*****

I think I got it, guys... Check my work, would ya?

We have this:

Breaks down like this. 3 distance rings with 8 segments each left and right. Each point on the arc gets a bit --6 bytes total, 48 bits total. The danger limit is proportionate to the servo posistion so as the sonar swings more to the side the danger limit is decreased and as it swings to the fwd posistion, it is increased. At each point, if the range is > danger that bit turns into a one.

Find the start and end of each "pack" of ones. Each pack of ones is then checked for width. Each pack of ones that makes the cut must be wide enough for the bot to get through and also should be able to be narrower if it is farther out. From there, I have all the available paths I could use. Choosing a path could be anything -for instance, widest pack that is farthest out wins -or- lets say I am in my docking mode... I have found the beacon, is there a path wide enough in that direction to travel that way? Or maybe I get one of those pyro-thingies that detect body heat... Same thing: There is a human and I should follow it, is the direction of the sensor within one of the "paths" available. Etc. Etc.

Comment viewing options

Welll, when I wrote down all 16 bits in a row and things seemed much better. I also had a great idea about the 2 bytes issue. Now I know you guys have this bit shifting thing going but really, wouldn't it be easier just to lower the resolution? It should be easy to take the 3 readings at each distance point and then simply convet to a byte for the total arc. Sure, 8 points for the whole arc, but that should be enough to navigate by. Actually, I would rather do a little "robot proofing" around the house (covering sensor "holes") and work with a nice simple byte. Also, I am still working with the idea of using the numbers from the farthest ring that has a wide enough path.

You could always use some bit trickery to condense your 16 bits down to 8, that way you can control the compression method, rather than just hoping that the lack of resolution doesn't cause you any trouble =)

If we take the 16 original bits coming from the sensors:
Left Map     [l7, l6, l5, l4, l3, l2, l1, l0] where bit l7 is on the far left
Right Map   [r7, r6, r5, r4, r3, r2, r1, r0] where bit r0 is on the far right
We can condense them into a new byte, using a couple of different methods:

• Cautious Map [l7&l6, l5&l4, l3&l2, l1&l0, r7&r6, r5&r4, r3&r2, r1&r0]
Using the AND/& operator means that both adjacent bits have to be safe/'1' for the corresponding new bit to also be 1. In other words, Walter will flag that area as unsafe if he detects anything in that 8th of his view.

• Reckless Map [l7|l6, l5|l4, l3|l2, l1|l0, r7|r6, r5|r4, r3|r2, r1|r0]
This time the OR/| operator is used, so if at least one bit in that area of Walter's view is safe, then Walter will determine that the entire 8th of the view is safe.

Now, you might think that there would only ever be reason to use the Cautious Map, but the Reckless Map may be useful for determining possible paths from further away. Or maybe if Walter can't find any paths using the Cautious Map he'll get impatient and move towards potential paths as dictated by the Reckless Map for a closer look, just in case they turn out to be wider than they seemed from a distance.

You know, rik I went back and reread the may i go post, a lot of it still applies.

I would love some psuedo code on this whole bit shifting thing. And yes, it is about packing and unpacking bytes into bits just so I can see them on the screen. And yes again, even a picaxe x2 only allows 4 bytes to be broken down to individual bits. If I want to tell an individual bit to be on or off, I can only do it up to bit31.

Converting to bits to work with them... yeah, I know, I was referring again to me, the human, seeing them.

I just attached code to my RuBo (http://letsmakerobots.com/node/14115). There's some bit shifting and toggling in player-v*.bas files at least. I used bit packing to store/read beats to/from EPPROM and also to store state information (at least in player-v2.bas; search the code for PL_STATE_ string).

Here's some bit shifting, toggling, setting and clearing code you can try in Pixace's simulator. It's getting really late so I'm not so sure I got right. Hope it helps :-)

`w0                           = %1100110010101010 ' Some test bitssymbol SOME_1_BIT_VALUE_MASK = %0000000000000001 ' bit 0symbol SOME_3_BIT_VALUE_MASK = %0000000000001110 ' bits 1, 2 and 3symbol ANOTHER_1_BIT_MASK    = %0000000000010000 ' bit 4' Toggle a bitw0 = w0 ^ SOME_1_BIT_VALUE_MASK ' bit 0 of w0 is set after this' Toggle it againw0 = w0 ^ SOME_1_BIT_VALUE_MASK ' bit 0 of w0 is cleared after this' Set a bitw0 = w0 | SOME_1_BIT_VALUE_MASK ' bit 0 of w0 is set after this' Set it againw0 = w0 | SOME_1_BIT_VALUE_MASK ' bit 0 of w0 is still set after this' Clear a bitw0 = w0 andnot SOME_1_BIT_VALUE_MASK ' bit 0 of w0 is cleared after this' Clear it againw0 = w0 andnot SOME_1_BIT_VALUE_MASK ' bit 0 of w0 is still cleared after this' Read 3 bit value from w0b2 = w0 & SOME_3_BIT_VALUE_MASK ' Now b2 has bits 1, 2 and 3' and shift b2 bits to "correct" position (to bits 0, 1 and 3)b2 = b2 >> 1                    ' Now b2 has "correct" value (5)' Set a new value to that "3 bit value"b2 = 2                          ' We want to store this value (max value = 7)' Shift bits to "corrent" position (to bits 1, 2 and 3)b2 = b2 << 1' Clear previous valuew0 = w0 andnot SOME_3_BIT_VALUE_MASK ' bits 1, 2 and 3 of w0 are cleared' Set new valuew0 = w0 | b2' Let's check that we got it rightb2 = 0                          ' Just to make sure we aren't cheating :-)' Read 3 bit value from w0b2 = w0 & SOME_3_BIT_VALUE_MASK ' Now b2 has bits 1, 2 and 3' and shift b2 bits to "correct" position (to bits 0, 1 and 3)b2 = b2 >> 1                    ' Now b2 has the value set earlier (2)' Let's toggle another bit just for funw0 = w0 ^ ANOTHER_1_BIT_MASK' If the ANOTHER_1_BIT_MASK is set then .. else ..b2 = w0 & ANOTHER_1_BIT_MASKif b2 > 0 then    ' ANOTHER_1_BIT_MASK was set    sertxd ("ANOTHER_1_BIT_MASK was set", cr,lf)else    ' ANOTHER_1_BIT_MASK was clear    sertxd ("ANOTHER_1_BIT_MASK was clear", cr,lf)endif' Let's toggle another bit againw0 = w0 ^ ANOTHER_1_BIT_MASK' If the ANOTHER_1_BIT_MASK is set then .. else ..b2 = w0 & ANOTHER_1_BIT_MASKif b2 > 0 then    ' ANOTHER_1_BIT_MASK was set    sertxd ("ANOTHER_1_BIT_MASK was set", cr,lf)else    ' ANOTHER_1_BIT_MASK was clear    sertxd ("ANOTHER_1_BIT_MASK was clear", cr,lf)endif`

Well boys, we got a problem...

I just started coding the simple procedure of taking readings, set 1's and 0's and spit out some data via LCD. Well, guess what? I need more than 32 bits...   ...and 32 is all that this little chip has to offer. Not to mention, I need a few to run my encoders. Even if I reuse bits, I still don't have enough. My far, middle, close ring idea still stands but each has to be done individually and stored as bytes. Upon decision making, these bytes have to be converted back to binary to be worked with, or the look-up table thing will have to work in real numbers. I guess the picaxe doesn't care if it is thinking in bin. dec. or whatever, but for me (the human) it would have been nice to see all the readings in nice little packs of 8 on-off switches.

I am stiil in the KISS mode right now, so I am going to continue to code one ring right now and play around with the data I get. I figure that in the long run this is not a huge problem. During the main routine, I want to look for a path that is farthest out first. I will just have to check each ring individually. I.e. "is there a path @ fathest out?" -no?- "is there a path @ the middle ring?" -no?- "is there a path @ closest?" -no?- "you probably have close walls on 3 sides and need to turn 180 or back out"

This is what I got so far. I am still trying to wrap my head around this whole bit shifting thing, but one step at a time, I guess.

Thanks, guys.

Not sure if you asked for this, but I am almost certain it is welcome:

Your coding problem with the 32 bits sounds like a limit from either a very small picaxe (08M?) or you are talking about bytes and storing each bit in a byte for representation purposes. I am guessing the second.

This can indeed be solved using the bit shifting (alt: AND'ing et OR'ing). Pack all bits into one (or more) bytes. When representing them (on your LCD) you don't want to read "255" when in your mind it should be " 11111111". Even I would not tolerate such a debug method. So that means unpacking in a clever way. Probably displaying bits "on the go" - while unpacking the byte(s). Don't store anything that's ready to ship out.

I wrote a poc before that demonstrates the packing (was it also in the "may I go"-node?) using << et >>.  On picaxes without the X-suffix, you'd have to user powers of 2 and simple adding. Let me know if you're in the mood for some pseudo code. Just to get your mind a bit tiny amount expanded and prepared for your own experiments.

One more note about different types of variables: Only you (and all humans, with very few exceptions) care about them. The processor only "senses" zeroes and ones. Check the picaxe debug window. It displays "values" of variables in different ways. The actual "data" are in binary.

You mention that "... these bytes have to be converted back to binary to be worked with ...".
I'd prefer: "... iterate over the individual bits inside a byte ...". No conversion as such. No storing individual bits if you can help it. Consider your byte as a string of bits, like a sentence can be a string of characters.

I gotta be honest, telefox, I am just starting to learn this whole bit shifting thing. But in terms of picaxe doing it, does this help?

The X1 and X2 parts also support
<< ; shift left
>> ; shift right
*/ ; multiply (returns middle word of result)
DIG ; return the digit value
REV ; reverse a number of bits

Hey, those are the ones, I know I'd seen >> used as a shift operator somewhere =)
Fun fact, if you shift a binary number to the right one place, it's the same as dividing it by two. Likewise, shift it to the left to multiply by two.

I'm all about minimalist/procedural programming, so I'm somewhat opiniated on the matter, but lookup tables are awful =P
With the exception of trigonometric or other unusual functions, I really can't stand allocating such huge resources to some task that can be performed using sequential analysis. So, on that note, I had a bit of a think regarding your sensor mapping issue...

Firstly there's that problem of having the map split in two, making it hard to decide what's going on straight ahead. One quite simple solution would be to take the right half of the left map, and the left half of the right map, and stick them together, i.e.
Left Map     [l7, l6, l5, l4, l3, l2, l1, l0] where bit l7 is on the far left
Right Map   [r7, r6, r5, r4, r3, r2, r1, r0] where bit r0 is on the far right
Middle Map [l3, l2, l1, l0, r7, r6, r5, r4]
If you're looking for 4bit wide spaces, the 'Middle Map' byte only yields 3 important patterns: [x1111xxx], [xx1111xx] and [xxx1111x]. All other 4bit or wider openings that can appear in the middle map will also appear on the side maps, so you don't need to consider them.

Instead of looking for an unbroken string of 4 '1's, I thought of a little trick that can reduce the complexity of the vector maps. Let's say you've detected a pattern of [01011111] from the left sensor. It's easy to see that there's a 5bit (or maybe more) space to the right of the sensor, but how does Walter determine that? Also, where is the center of this space?
• First, we duplicate the bit pattern into another memory location, so we now have two identical copies. Then we shift the bits (or rotate them, as it is often called) and recombine them. Here's and example using the bit pattern I mentioned above:
Original Bits            [01011111]
Duplicated Bits        [01011111]
• Now comes the shift, we move the original bits left once, and the duplicated bits right once, padding with '0's if needed. I think PICBASIC uses a command along the lines of >> for this.
Left'd Bits               [10111110]<-0
Right'd Bits        0->[00101111]
• Recombine the two modified bit patterns with the unmodified version using AND.
Original Bits            [01011111]
Left'd Bits               [10111110]
AND'd Left'd Bits      [00011110]

Original Bits            [01011111]
Right'd Bits             [00101111]
AND'd Right'd Bits    [00001111]
• Finally, AND the results of the last step together again.
AND'd Left'd Bits      [00011110]
AND'd Right'd Bits    [00001111]
AND'd AND'd Bits      [00001110]
Original Bits            [01011111] (for comparison)

As you can see, only spaces with 3 or more bits of consecutive clear space 'survive' this process, and further more the center of the detected space remains the same. 3 consecutive bits yields only a single bit at the center in the final map, 4 consecutive bits yield two bits, 5 yields 3, etc etc. All these bit operations are extremely fast in terms of processing time, so even on a PICAXE I can't imagine a routine like this would take very long to execute.
You can sorta think of this routine as a digital filter, since it removes the 'noise' of isolated single/double space bits, and consolidates the relevant data. If you do decide to use the lookup table approach, you can still lead in with this method, which will reduce your memory requirements by at least half (the larget byte goes from [11111111] = D'256' to [01111110] = D'126').

You're right. The tables suck. From a procedural point of view. And they may very well be unnecessary. But they can be loads of fun.

From a meta physical point of view (?) I could see them as "instinct tables". Walter is not an intelligent beast. It is pre-programmed. Every single decision it is gonna make is fixed by its creator. Walter has no choice but to do as is written "in the big book". It kinda fits.

Ohw: neat bit shifting trick there. I learnt that only Picaxe X-parts (28X1 - 40X2) can do that.