Pokey's Software: Part 2

Most of the entrants used dead reckoning to navigate the firefighting maze, using Lego NXTwheel encoders built into the motor modules. The dimensions and layout of the maze are known (approximately) beforehand. Note that if you build the maze it doesn't quite look like the drawing.
Pokey was designed to run through the maze without using dead reckoning. Instead, the little robot looks for what I call "events" such as the appearance of a wall to the left or right. The disappearance of the wall on the right or left. Detecting a floor mark. Of course these events are based on lower level sensor code that we'll get to in future posts.

Most importantly, navigation relies very heavily on good wall following because staying oriented to a wall means Pokey knows where he is in his predictable little world. And, in fact, poor wall following performance resulted in his humiliating defeat. :) But more on the low level stuff later. Here's a rundown on the high level event based navigation functions in nav.c

First off, here's the code that gets Pokey from home circle to the floating room. This code is in the main module, sparky.c

//////////////////////////////////////////////////////////////////////
// Home to Room A //////////////////////////////////////////////////////////////////////
go_straight(NAV_EVENT_WALL_HERE, LEFT);
wall_follow_left(NAV_EVENT_WALL_GONE);
myevent = corner_turn(NAV_EVENT_FLOOR|NAV_EVENT_WALL_FOUND, LEFT, LEFT, TURN_FACTOR * 0.80);
if ((myevent & NAV_EVENT_FLOOR) == 0) {
wall_follow_left(NAV_EVENT_FLOOR|NAV_EVENT_FRONT);
}
// Only way to align to the wall, for now
wall_follow_left(NAV_EVENT_FRONT);
stop_moving(); // Don't delete this you goon!

Pokey wants to follow the wall for the floating room to his left. But that wall doesn't appear right away, so Pokey drives forward looking for the wall to the left. His ranger array points left (second parameter below) and he takes off...

go_straight(NAV_EVENT_WALL_HERE, LEFT);

This routine drops out as soon as the wall is found. Now all he has to do is follow the wall until it disappears again.

wall_follow_left(NAV_EVENT_WALL_GONE);

Now he knows that he's in the middle of the maze and all he has to do is turn left into the first room using a constant radius turn (yes, we could use wall following but I found my way was easier to predictably achieve the proper distance once the robot has turned through 180°).

myevent = corner_turn(NAV_EVENT_FLOOR|NAV_EVENT_WALL_FOUND, LEFT, LEFT, TURN_FACTOR * 0.80);

Once Pokey either (a) crosses the floor marker at the door threshold or (b) finds the wall to the left after having made his 180° turn, we move onto the next step. If we still haven't found the floor marker, well, let's find it:

if ((myevent & NAV_EVENT_FLOOR) == 0) {
wall_follow_left(NAV_EVENT_FLOOR|NAV_EVENT_FRONT);
}

Ok, now Pokey knows he's in the room and that the wall is to the left. Pokey can then orient himself square to the room by relying on wall following that left wall until he gets to the corner-- when a wall appears dead ahead.

wall_follow_left(NAV_EVENT_FRONT);

Now Pokey can scan the room in a predictable fashion (more on that later), winding up either pointed at the candle if there is one, or pointed 180° opposite of how he came in--almost perfectly to leave the room. And since he's in the corner, he has a good stretch of wall to follow to reorient himself perfectly on the way out of the room. This is the same approach I used for every room.

That's the theory, but if you look at the video you'll notice he never makes it to the corner. Pokey doesn't straighten out fast enough after detecting the wall to the left and ends up continuing his turn until he is pointing at a sharp angle into the left wall. He sees the left wall in front of him and mistakes that for being in the corner. Oops.

Fortunately, the scanning routine works so well at orienting him that he's still able to navigate out of the room to the next one at least some of the time.

stop_moving(); // Don't delete this you goon!

During testing I would add stop_moving() calls to pause him at various places in the code so I could visibly troubleshoot what was happening. I'd forget and take out this call, too, and so he'd enter the room and run smack into the corner wall. So I wrote myself a little reminder.

From event.h here are the nav events

#define NAV_EVENT_NONE 0x00 // disables the event check
#define NAV_EVENT_WALL_GONE 0x01 // wall disappears
#define NAV_EVENT_FRONT 0x02 // wall appears in front
#define NAV_EVENT_FLOOR 0x04 // floor mark detected
#define NAV_EVENT_WALL_FOUND 0x08 // wall appears
#define NAV_EVENT_THIN_WALL 0x10 // unused, unneeded
#define NAV_EVENT_SHORT_FRONT 0x20 // wall close in front
#define NAV_EVENT_WALL_HERE 0x40 // wall close
#define NAV_EVENT_ALL 0xFF // all events, unused for now

The navigation event routines take one of these events to "watch for" and once found they drop out. The event type is a bit field, each bit corresponds to an event. So your routines can report multiple events at the same time. Each one of those nav event routines calls this one to watch for events. I threw in some addition comments:

event event_check(void)
{
event myevent = NAV_EVENT_NONE;

get_distances();

// Ranger pointing right?
if ( Pointing == RIGHT ) {
// Is the wall within wall following range?
// do we need to add a little fudge factor to distance?
if (distance_right <>
myevent |= NAV_EVENT_WALL_FOUND;

// Has the wall gone away?
else if (distance_right > 550.0)
myevent |= NAV_EVENT_WALL_GONE;
}

// if ranger is pointing left
else if ( Pointing == LEFT ) {
// Is the wall within wall following range?
// do we need to add a little fudge factor to distance?
if (distance_left <>
myevent |= NAV_EVENT_WALL_HERE;

if (distance_left <>
myevent |= NAV_EVENT_WALL_FOUND;

// Has the wall gone away?
if (distance_left > 550.0)
myevent |= NAV_EVENT_WALL_GONE;
}

// Object ahead, probably at end of hall or something
if (distance_front <>
myevent |= NAV_EVENT_SHORT_FRONT;
}

// this number is kind of fudged, trial and error
if (distance_front <> myevent |= NAV_EVENT_FRONT;
}

// Crossed over something white
if (floor_detected()) {
myevent |= NAV_EVENT_FLOOR;
}

return myevent;
}

You can see there's some dependency on the lower level sensor routines (ranger, floor sensor). Anyway, here's an example of one of the nav event routines:

event wall_follow_right(event wanted)
{
event myevent = NAV_EVENT_NONE;

look_right(); // point the ranger right

// drops out when the desired event is found
while ((myevent & wanted) == 0) {

// find out the distances
range_error = target_distance - distance_right;
range_error_rate = last_range_error - range_error;

// wall correction is a PID type routine based on
// current distance error and rate of change of error
steer = -(WALL_CORRECT());

// limit steering, or if we get too far away the bot will just spin
if (steer < -30) steer = -30;
if (steer > 30) steer = 30;

// record error statistics
last_range_error = range_error;
move_forward(MAX_SPEED, steer);

// Add discovered events to myevent
myevent |= event_check();

} // while

return myevent;
} // wall_follow_right

There's plenty more I could talk about but I don't want a 50 page post, either. For example, momentum and stopping distance: the threshold for detecting the front wall has to depend on speed.

The code has plenty of opportunity for improvement and generalizing to account for different speeds, battery voltages, etc. Reworking the wall following to incorporate constant radius turns would be swell.

Hopefully you get the gist, and I hope you found this useful.