What I've been trying to suggest is that you're confusing the algorithm with the code. There are very likely more efficient ways to write the code, but the real problem is figuring out how to use the information the sensors are giving you and the code follows from that. Assuming that you were wearing a blacked out motorcycle helmet with the compass, gps and five ping sensor data on a heads up display, what would you do to navigate between your waypoints using that info?
Having said that, what scale is your course? You mention walls and enclosed spaces, so I'm guessing not very big, and likely has no gps reception, so my previous suggestion seems moot. You're down to dead reckoning now, so encoders on the wheels would be nice. Without that, how do you plan to tell where you are and where your destination is? How nasty will the obstacle course be - will you need to navigate a little maze or is it just avoiding a few small cushions strewn along the way?
Depending on the answers to the above, this could turn out to be very hard and likely require additional storage for mapping data, or reasonably easy but with no guarantee that the robot will always be able to find its way.