diff --git a/robots/mtp/GNUmakefile.in b/robots/mtp/GNUmakefile.in
index 3d9117058cfc1a8c173fa7f42f40809eea8bc9b2..acb3fe24293f9e79f49434b1b2fd5f31b52dc970 100644
--- a/robots/mtp/GNUmakefile.in
+++ b/robots/mtp/GNUmakefile.in
@@ -44,16 +44,16 @@ mtp_dump.o:	mtp.h
 test_mtp.sh: mtp_send mtp_recv
 
 mtp_test: mtp_test.o $(MTPLIBS)
-	$(CC) $(CFLAGS) $(LDFLAGS) -o $@ mtp_test.o -L. -lmtp
+	$(CC) $(CFLAGS) $(LDFLAGS) -o $@ mtp_test.o -L. -lmtp -lm
 
 mtp_send: mtp_send.o $(MTPLIBS)
-	$(CC) $(CFLAGS) $(LDFLAGS) -o $@ mtp_send.o -L. -lmtp
+	$(CC) $(CFLAGS) $(LDFLAGS) -o $@ mtp_send.o -L. -lmtp -lm
 
 mtp_recv: mtp_recv.o $(MTPLIBS)
-	$(CC) $(CFLAGS) $(LDFLAGS) -o $@ mtp_recv.o -L. -lmtp
+	$(CC) $(CFLAGS) $(LDFLAGS) -o $@ mtp_recv.o -L. -lmtp -lm
 
 mtp_dump: mtp_dump.o $(MTPLIBS)
-	$(CC) $(CFLAGS) $(LDFLAGS) -o $@ mtp_dump.o -L. -lmtp
+	$(CC) $(CFLAGS) $(LDFLAGS) -o $@ mtp_dump.o -L. -lmtp -lm
 
 CLASSES_SRC = $(wildcard $(SRCDIR)/*.java)
 
diff --git a/robots/mtp/mtp.c b/robots/mtp/mtp.c
index bc4a914d399a6484bd5550bec8746fd4d06c9c58..9995b19da2aca27f2ad6b4a402fb36f34b4b5aa1 100755
--- a/robots/mtp/mtp.c
+++ b/robots/mtp/mtp.c
@@ -59,8 +59,10 @@ static int mtp_read(char *handle, char *data, int len)
 
     assert(handle != NULL);
     assert(data != NULL);
-    
-    retval = read(mh->mh_fd, data, len);
+
+    do {
+	retval = read(mh->mh_fd, data, len);
+    } while ((retval == -1) && (errno == EINTR));
 
     if (retval == 0) {
 	mh->mh_flags |= MHF_EOF;
@@ -69,6 +71,9 @@ static int mtp_read(char *handle, char *data, int len)
     else if (retval > 0) {
 	mh->mh_remaining += retval;
     }
+    else {
+	printf(" mtp_read %d %s\n", retval, strerror(errno));
+    }
     
     // printf("%d read(%p(%d), %p, %d)\n", retval, mh, mh->mh_fd, data, len);
 
@@ -621,6 +626,55 @@ float mtp_theta(float theta)
     return retval;
 }
 
+void mtp_polar(struct robot_position *current,
+	       struct robot_position *dest,
+	       float *r_out,
+	       float *theta_out)
+{
+    assert(current != NULL);
+    assert(dest != NULL);
+    assert(r_out != NULL);
+    assert(theta_out != NULL);
+
+    *r_out = hypotf(current->x - dest->x, current->y - dest->y);
+    *theta_out = atan2f(current->y - dest->y, dest->x - current->x);
+}
+
+void mtp_cartesian(struct robot_position *current,
+		   float r,
+		   float theta,
+		   struct robot_position *dest_out)
+{
+    assert(current != NULL);
+    assert(dest_out != NULL);
+
+    dest_out->x = current->x + cos(theta) * r;
+    dest_out->y = current->y - sin(theta) * r;
+}
+
+int mtp_compass(float theta)
+{
+    int retval = 0;
+
+#define DEGREES_30 (0.52f)
+    
+    theta = mtp_theta(theta);
+
+    if ((theta >= DEGREES_30) && (theta <= (M_PI - DEGREES_30)))
+	retval |= MCF_NORTH;
+    
+    if ((theta <= (M_PI_2 - DEGREES_30)) && (theta >= (-M_PI_2 + DEGREES_30)))
+	retval |= MCF_EAST;
+    
+    if ((theta <= -DEGREES_30) && (theta >= (-M_PI + DEGREES_30)))
+	retval |= MCF_SOUTH;
+    
+    if ((theta >= (M_PI_2 + DEGREES_30) || (theta <= (-M_PI + DEGREES_30))))
+	retval |= MCF_WEST;
+    
+    return retval;
+}
+
 void mtp_print_packet(FILE *file, struct mtp_packet *mp)
 {
     struct mtp_garcia_telemetry *mgt;
diff --git a/robots/mtp/mtp.h b/robots/mtp/mtp.h
index 96f2b7f5d6624b3d5729410eb096d659db657f59..79491ec93d91aeecf19ea581332adaf109545428 100755
--- a/robots/mtp/mtp.h
+++ b/robots/mtp/mtp.h
@@ -128,8 +128,6 @@ typedef enum {
     MA_ID,	 	 /*< (int) */
     MA_Code,	 	 /*< (int) */
     MA_Message,	 	 /*< (char *) */
-    MA_Horizontal,	 /*< (double) */
-    MA_Vertical,	 /*< (double) */
     MA_RobotLen,	 /*< (int) */
     MA_RobotVal,	 /*< (robot_config *) */
     MA_CameraLen,	 /*< (int) */
@@ -184,6 +182,42 @@ void mtp_free_packet(struct mtp_packet *mp);
 
 float mtp_theta(float theta);
 
+void mtp_polar(struct robot_position *current,
+	       struct robot_position *dest,
+	       float *r_out,
+	       float *theta_out);
+
+void mtp_cartesian(struct robot_position *current,
+		   float r,
+		   float theta,
+		   struct robot_position *dest_out);
+
+enum {
+    MCB_NORTH,
+    MCB_EAST,
+    MCB_WEST,
+    MCB_SOUTH
+};
+
+enum {
+    MCF_NORTH = (1L << MCB_NORTH),
+    MCF_EAST = (1L << MCB_EAST),
+    MCF_WEST = (1L << MCB_WEST),
+    MCF_SOUTH = (1L << MCB_SOUTH),
+};
+
+#define MTP_COMPASS_STRING(x) ( \
+    (x) == (MCF_NORTH|MCF_WEST) ? "nw" : \
+    (x) == (MCF_NORTH) ? "n" : \
+    (x) == (MCF_NORTH|MCF_EAST) ? "ne" : \
+    (x) == (MCF_EAST) ? "e" : \
+    (x) == (MCF_SOUTH|MCF_EAST) ? "se" : \
+    (x) == (MCF_SOUTH) ? "s" : \
+    (x) == (MCF_SOUTH|MCF_WEST) ? "sw" : \
+    (x) == (MCF_WEST) ? "w" : "u")
+
+int mtp_compass(float theta);
+
 /**
  * Print the contents of the given packet to the given FILE object.
  *
diff --git a/robots/rmcd/pilotConnection.c b/robots/rmcd/pilotConnection.c
index 0fd17d14f7d0eb4cac70624e7e3c03f9d25625d3..142966dcc98fa8ac531afdce80db09b52a8b924d 100644
--- a/robots/rmcd/pilotConnection.c
+++ b/robots/rmcd/pilotConnection.c
@@ -34,6 +34,9 @@ struct pilot_connection_data pc_data;
 #define cmp_fuzzy(x1, x2, tol) \
     ((((x1) - (tol)) < (x2)) && (x2 < ((x1) + (tol))))
 
+#define min(x, y) ((x) < (y)) ? (x) : (y)
+#define max(x, y) ((x) > (y)) ? (x) : (y)
+
 /**
  * Compare two position objects to see if they are the "same", where "same" is
  * within some tolerance.
@@ -179,6 +182,44 @@ struct pilot_connection *pc_add_robot(struct robot_config *rc)
     return retval;
 }
 
+void pc_dump_info(void)
+{
+    int lpc;
+
+    info("info: pilot list\n");
+    for (lpc = 0; lpc < pc_data.pcd_connection_count; lpc++) {
+	struct pilot_connection *pc;
+	int lpc2;
+	
+	pc = &pc_data.pcd_connections[lpc];
+	info("  %s: flags=0x%x; state=%s; tries=%d\n"
+	     "    actual: %.2f %.2f %.2f\tlast:  %.2f %.2f %.2f\n"
+	     "    waypt:  %.2f %.2f %.2f %s\n"
+	     "    goal:   %.2f %.2f %.2f\n",
+	     pc->pc_robot->hostname,
+	     pc->pc_flags,
+	     state_strings[pc->pc_state],
+	     pc->pc_tries_remaining,
+	     pc->pc_last_pos.x, pc->pc_last_pos.y, pc->pc_last_pos.theta,
+	     pc->pc_actual_pos.x, pc->pc_actual_pos.y, pc->pc_actual_pos.theta,
+	     pc->pc_waypoint.x, pc->pc_waypoint.y, pc->pc_waypoint.theta,
+	     (pc->pc_flags & PCF_WAYPOINT) ? "(active)" : "(inactive)",
+	     pc->pc_goal_pos.x, pc->pc_goal_pos.y, pc->pc_goal_pos.theta);
+	for (lpc2 = 0; lpc2 < pc->pc_obstacle_count; lpc2++) {
+	    struct obstacle_config *oc;
+
+	    oc = &pc->pc_obstacles[lpc];
+	    info("   obstacle[%d] = %d -- %.2f %.2f %.2f %.2f\n",
+		 lpc,
+		 oc->id,
+		 oc->xmin,
+		 oc->ymin,
+		 oc->xmax,
+		 oc->ymax);
+	}
+    }
+}
+
 struct pilot_connection *pc_find_pilot(int robot_id)
 {
     struct pilot_connection *retval = NULL;
@@ -219,6 +260,7 @@ void pc_plot_waypoint(struct pilot_connection *pc)
 	
 	pc->pc_flags &= ~PCF_WAYPOINT;
 	pc->pc_obstacle_count = 0;
+	pc->pc_waypoint_tries = 0;
     }
     else {
 	int lpc;
@@ -239,11 +281,35 @@ void pc_plot_waypoint(struct pilot_connection *pc)
 	    rl.y1 = pc->pc_goal_pos.y;
 
 	    pc->pc_tries_remaining = MAX_REFINE_RETRIES;
+
+#if 0
+	    if ((pc->pc_waypoint_tries > 0) &&
+		(pc->pc_waypoint_tries % 3) == 0) {
+		info("*** expanding dynamic obstacle!");
+		oc->xmin -= 0.20;
+		oc->ymin -= 0.20;
+		oc->xmax += 0.20;
+		oc->ymax += 0.20;
+	    }
+#endif
 	    
-	    if ((rc_clip_line(&rl, oc) == 0) ||
-		(hypotf(rl.x0 - rl.x1, rl.y0 - rl.y1) < 0.20)) {
+	    if (rc_compute_code(pc->pc_goal_pos.x,
+				pc->pc_goal_pos.y,
+				oc) == 0) {
 		if (debug) {
-		    info("debug: %s cleared obstacle\n", \
+		    info("debug: %s has a goal in an obstacle\n",
+			 pc->pc_robot->hostname);
+		}
+
+		pc->pc_tries_remaining = 0;
+		pc->pc_obstacle_count = 0;
+	    }
+	    else if ((rc_clip_line(&rl, oc) == 0) ||
+		     (hypotf(rl.x0 - rl.x1, rl.y0 - rl.y1) < 0.20) ||
+		     ((oc->id == -1000) && (pc->pc_waypoint_tries > 20))) {
+		
+		if (debug) {
+		    info("debug: %s cleared obstacle\n",
 			 pc->pc_robot->hostname);
 		}
 		
@@ -252,18 +318,18 @@ void pc_plot_waypoint(struct pilot_connection *pc)
 			sizeof(struct obstacle_config) * (32 - lpc));
 		pc->pc_obstacle_count -= 1;
 		lpc -= 1;
+		pc->pc_waypoint_tries = 0;
 	    }
 	    else {
 		float bearing;
 		rc_code_t rc;
 		int in_vision = 0;
 		int flipped_bearing = 0;
-
-		pc->pc_flags |= PCF_WAYPOINT;
+		int compass;
 
 		printf("clip %f %f %f %f\n",
 		       rl.x0, rl.y0, rl.x1, rl.y1);
-		switch (rc_compute_closest(rl.x0, rl.y0, oc)) {
+		switch ((rc = rc_compute_closest(rl.x0, rl.y0, oc))) {
 		case RCF_LEFT:
 		    rl.x0 = oc->xmin;
 		    break;
@@ -293,10 +359,32 @@ void pc_plot_waypoint(struct pilot_connection *pc)
 		    rl.y0 = oc->ymax;
 		    break;
 		}
+
+		if (rc_clip_line(&rl, oc) == 0 ||
+		    (hypotf(rl.x0 - rl.x1, rl.y0 - rl.y1) < 0.05)) {
+		    info("debug: %s cleared obstacle 2\n", \
+			 pc->pc_robot->hostname);
+		    
+		    memmove(&pc->pc_obstacles[lpc],
+			    &pc->pc_obstacles[lpc + 1],
+			    sizeof(struct obstacle_config) * (32 - lpc));
+		    pc->pc_obstacle_count -= 1;
+		    lpc -= 1;
+		    pc->pc_waypoint_tries = 0;
+		    continue;
+		}
+		
+		pc->pc_flags |= PCF_WAYPOINT;
+		pc->pc_waypoint_tries += 1;
+
 		bearing = atan2f(rl.y0 - rl.y1, rl.x1 - rl.x0);
+		compass = mtp_compass(bearing);
 
 		if (debug) {
 		    info("debug: waypoint bearing %f\n", bearing);
+		    info("debug: side=%s; compass=%s\n",
+			 RC_CODE_STRING(rc),
+			 MTP_COMPASS_STRING(compass));
 		}
 
 		/*
@@ -317,103 +405,95 @@ void pc_plot_waypoint(struct pilot_connection *pc)
 		 * control if we've tried stalling.
 		 */
 		while (!in_vision) {
+		    int new_rc = 0;
+		    
+		    switch (rc) {
+		    case RCF_TOP|RCF_LEFT:
+			if (compass & MCF_EAST)
+			    new_rc = RCF_TOP|RCF_RIGHT;
+			else if (compass & MCF_SOUTH)
+			    new_rc = RCF_BOTTOM|RCF_LEFT;
+			else
+			    assert(0);
+			break;
+		    case RCF_BOTTOM|RCF_LEFT:
+			if (compass & MCF_EAST)
+			    new_rc = RCF_BOTTOM|RCF_RIGHT;
+			else if (compass & MCF_NORTH)
+			    new_rc = RCF_TOP|RCF_LEFT;
+			else
+			    assert(0);
+			break;
+		    case RCF_BOTTOM|RCF_RIGHT:
+			if (compass & MCF_NORTH)
+			    new_rc = RCF_TOP|RCF_RIGHT;
+			else if (compass & MCF_WEST)
+			    new_rc = RCF_BOTTOM|RCF_LEFT;
+			else
+			    assert(0);
+			break;
+		    case RCF_TOP|RCF_RIGHT:
+			if (compass & MCF_SOUTH)
+			    new_rc = RCF_BOTTOM|RCF_RIGHT;
+			else if (compass & MCF_WEST)
+			    new_rc = RCF_TOP|RCF_LEFT;
+			else
+			    assert(0);
+			break;
+		    case RCF_LEFT:
+			if (compass & MCF_EAST)
+			    new_rc = rc_closest_corner(rl.x0, rl.y0, oc);
+			else if (compass & MCF_NORTH)
+			    new_rc = RCF_TOP|RCF_LEFT;
+			else if (compass & MCF_SOUTH)
+			    new_rc = RCF_BOTTOM|RCF_LEFT;
+			else
+			    assert(0);
+			break;
+		    case RCF_TOP:
+			if (compass & MCF_SOUTH)
+			    new_rc = rc_closest_corner(rl.x0, rl.y0, oc);
+			else if (compass & MCF_EAST)
+			    new_rc = RCF_TOP|RCF_RIGHT;
+			else if (compass & MCF_WEST)
+			    new_rc = RCF_TOP|RCF_LEFT;
+			else
+			    assert(0);
+			break;
+		    case RCF_RIGHT:
+			if (compass & MCF_WEST)
+			    new_rc = rc_closest_corner(rl.x0, rl.y0, oc);
+			else if (compass & MCF_NORTH)
+			    new_rc = RCF_TOP|RCF_RIGHT;
+			else if (compass & MCF_SOUTH)
+			    new_rc = RCF_BOTTOM|RCF_RIGHT;
+			else
+			    assert(0);
+			break;
+		    case RCF_BOTTOM:
+			if (compass & MCF_NORTH)
+			    new_rc = rc_closest_corner(rl.x0, rl.y0, oc);
+			else if (compass & MCF_EAST)
+			    new_rc = RCF_BOTTOM|RCF_RIGHT;
+			else if (compass & MCF_WEST)
+			    new_rc = RCF_BOTTOM|RCF_LEFT;
+			else
+			    assert(0);
+			break;
 
-		    if (rl.x0 == oc->xmin && rl.y0 == oc->ymin) {
-			printf("  top left\n");
-			if (bearing > -M_PI_4) {
-			    pc->pc_waypoint.x = oc->xmax;
-			    pc->pc_waypoint.y = oc->ymin;
-			}
-			else {
-			    pc->pc_waypoint.x = oc->xmin;
-			    pc->pc_waypoint.y = oc->ymax;
-			}
-		    }
-		    else if (rl.x0 == oc->xmin && rl.y0 == oc->ymax) {
-			printf("  bottom left\n");
-			if (bearing > M_PI_4) {
-			    pc->pc_waypoint.x = oc->xmin;
-			    pc->pc_waypoint.y = oc->ymin;
-			}
-			else {
-			    pc->pc_waypoint.x = oc->xmax;
-			    pc->pc_waypoint.y = oc->ymax;
-			}
-		    }
-		    else if (rl.x0 == oc->xmax && rl.y0 == oc->ymax) {
-			printf("  bottom right\n");
-			if (bearing > (M_PI_2 + M_PI_4)) {
-			    pc->pc_waypoint.x = oc->xmin;
-			    pc->pc_waypoint.y = oc->ymax;
-			}
-			else {
-			    pc->pc_waypoint.x = oc->xmax;
-			    pc->pc_waypoint.y = oc->ymin;
-			}
-		    }
-		    else if (rl.x0 == oc->xmax && rl.y0 == oc->ymin) {
-			printf("  top right\n");
-			if (bearing < -(M_PI_2 + M_PI_4)) {
-			    pc->pc_waypoint.x = oc->xmin;
-			    pc->pc_waypoint.y = oc->ymin;
-			}
-			else {
-			    pc->pc_waypoint.x = oc->xmax;
-			    pc->pc_waypoint.y = oc->ymax;
-			}
-		    }
-		    else if (rl.x0 == oc->xmin) {
-			printf("  left\n");
-			if (bearing > 0.0) {
-			    pc->pc_waypoint.x = oc->xmin;
-			    pc->pc_waypoint.y = oc->ymin;
-			}
-			else {
-			    pc->pc_waypoint.x = oc->xmin;
-			    pc->pc_waypoint.y = oc->ymax;
-			}
-		    }
-		    else if (rl.x0 == oc->xmax) {
-			printf("  right\n");
-			if (bearing > M_PI_2) {
-			    pc->pc_waypoint.x = oc->xmax;
-			    pc->pc_waypoint.y = oc->ymin;
-			}
-			else {
-			    pc->pc_waypoint.x = oc->xmax;
-			    pc->pc_waypoint.y = oc->ymax;
-			}
-		    }
-		    else if (rl.y0 == oc->ymin) {
-			printf("  top\n");
-			if (bearing < -M_PI_2) {
-			    pc->pc_waypoint.x = oc->xmin;
-			    pc->pc_waypoint.y = oc->ymin;
-			}
-			else {
-			    pc->pc_waypoint.x = oc->xmax;
-			    pc->pc_waypoint.y = oc->ymin;
-			}
-		    }
-		    else if (rl.y0 == oc->ymax) {
-			printf("  bottom\n");
-			if (bearing < M_PI_2) {
-			    pc->pc_waypoint.x = oc->xmax;
-			    pc->pc_waypoint.y = oc->ymax;
-			}
-			else {
-			    pc->pc_waypoint.x = oc->xmin;
-			    pc->pc_waypoint.y = oc->ymax;
-			}
-		    }
-		    else {
+		    default:
 			assert(0);
+			break;
+		    }
+
+		    if (debug) {
+			info("debug: new corner -- %s\n", RC_CODE_STRING(new_rc));
 		    }
+		    
+		    rc_corner(new_rc, &pc->pc_waypoint, oc);
 
-		    if (pc_point_in_bounds(&pc_data,
-					   pc->pc_waypoint.x,
-					   pc->pc_waypoint.y
-					   )) {
+		    if (pc_point_in_bounds(pc->pc_waypoint.x,
+					   pc->pc_waypoint.y)) {
 			in_vision = 1;
 			info("waypoint in vision\n");
 		    }
@@ -449,6 +529,16 @@ void pc_plot_waypoint(struct pilot_connection *pc)
 	    }
 	}
     }
+
+    if (!(pc->pc_flags & PCF_WAYPOINT)) {
+	float distance, theta;
+
+	mtp_polar(&pc->pc_actual_pos, &pc->pc_goal_pos, &distance, &theta);
+	if (distance > 1.5) {
+	    pc->pc_flags |= PCF_WAYPOINT;
+	    mtp_cartesian(&pc->pc_actual_pos, 1.5, theta, &pc->pc_waypoint);
+	}
+    }
 }
 
 void pc_set_goal(struct pilot_connection *pc, struct robot_position *rp)
@@ -480,7 +570,8 @@ void pc_set_goal(struct pilot_connection *pc, struct robot_position *rp)
     }
     else {
 	pc->pc_goal_pos = *rp;
-	pc_change_state(pc, PS_PENDING_POSITION);
+	if (pc->pc_state != PS_START_WIGGLING && pc->pc_state != PS_WIGGLING)
+	    pc_change_state(pc, PS_PENDING_POSITION);
     }
 }
 
@@ -626,7 +717,6 @@ void pc_change_state(struct pilot_connection *pc, pilot_state_t ps)
 	break;
 	
     case PS_WIGGLING:
-	printf("try wiggle %d\n", pc->pc_flags);
 	if (pc->pc_flags & PCF_WIGGLE_REVERSE) {
 	    mtp_init_packet(&mp,
 			    MA_Opcode, MTP_WIGGLE_STATUS,
@@ -640,13 +730,13 @@ void pc_change_state(struct pilot_connection *pc, pilot_state_t ps)
 	    ps = PS_ARRIVED;
 	}
 	else {
-	    printf("** REVERSE %d\n", pc->pc_flags & PCF_CONTACT);
 	    mtp_init_packet(&pmp,
 			    MA_Opcode, MTP_COMMAND_GOTO,
 			    MA_Role, MTP_ROLE_RMC,
 			    MA_RobotID, pc->pc_robot->id,
 			    MA_CommandID, 2,
-			    MA_Theta, pc->pc_flags & PCF_CONTACT ? -M_PI : M_PI,
+			    MA_Theta, (pc->pc_flags & PCF_CONTACT ?
+				       -M_PI : M_PI),
 			    MA_TAG_DONE);
 	    send_pmp = 1;
 
@@ -793,7 +883,6 @@ static void pc_handle_update(struct pilot_connection *pc,
     case MTP_POSITION_STATUS_CONTACT:
 	pc->pc_flags &= ~PCF_VISION_POSITION;
 	pc->pc_flags |= PCF_CONTACT;
-	printf(" %d\n", pc->pc_flags);
 	pc_change_state(pc, pc->pc_state);
 	break;
     case MTP_POSITION_STATUS_COMPLETE:
@@ -876,7 +965,7 @@ static void pc_handle_report(struct pilot_connection *pc,
 	    }
 
 	    oc = &oc_fake;
-	    oc->id = 1000; // XXX
+	    oc->id = -1000; // XXX
 
 	    if (bearing < M_PI_4 && bearing > -M_PI_4) {
 		mcr->points[lpc].x = 0.15;
@@ -884,7 +973,8 @@ static void pc_handle_report(struct pilot_connection *pc,
 	    }
 	    else if (bearing >= -M_PI_4 && bearing < (M_PI_2 + M_PI_4)) {
 	    }
-	    else if (bearing >= (M_PI_2 + M_PI_4) || bearing < -(M_PI_2 + M_PI_4)) {
+	    else if (bearing >= (M_PI_2 + M_PI_4) ||
+		     bearing < -(M_PI_2 + M_PI_4)) {
 	    }
 	    else if (bearing < (M_PI_2 + M_PI_4) && (bearing > -M_PI_4)) {
 		mcr->points[lpc].x = -0.15;
@@ -903,9 +993,27 @@ static void pc_handle_report(struct pilot_connection *pc,
 	    oc->xmax = cp.x + OBSTACLE_BUFFER + 0.10;
 	    oc->ymin = cp.y - OBSTACLE_BUFFER - 0.10;
 	    oc->ymax = cp.y + OBSTACLE_BUFFER + 0.10;
-	    
-	    info("debug: dynamic obstacle %f %f %f %f\n",
-		 oc->xmin, oc->ymin, oc->xmax, oc->ymax);
+
+	    if (pc->pc_obstacles[pc->pc_obstacle_count - 1].id == -1000) {
+		struct obstacle_config *oc_old;
+
+		oc_old = &pc->pc_obstacles[pc->pc_obstacle_count - 1];
+		oc_old->xmin = min(oc->xmin, oc_old->xmin);
+		oc_old->ymin = min(oc->ymin, oc_old->ymin);
+		oc_old->xmax = max(oc->xmax, oc_old->xmax);
+		oc_old->ymax = max(oc->ymax, oc_old->ymax);
+		oc = NULL;
+		
+		info("debug: expanding dynamic obstacle %f %f %f %f\n",
+		     oc_old->xmin,
+		     oc_old->ymin,
+		     oc_old->xmax,
+		     oc_old->ymax);
+	    }
+	    else {
+		info("debug: dynamic obstacle %f %f %f %f\n",
+		     oc->xmin, oc->ymin, oc->xmax, oc->ymax);
+	    }
 	}
 
 	if (oc != NULL) {
@@ -968,14 +1076,22 @@ void pc_handle_signal(fd_set *rready, fd_set *wready)
     }
 }
 
-int pc_point_in_bounds(struct pilot_connection_data *pcd,float x,float y) {
+void pc_handle_timeout(struct timeval *current_time)
+{
+    assert(current_time != NULL);
+
+    
+}
+
+int pc_point_in_bounds(float x, float y)
+{
     int i;
     struct box *boxes;
     int boxes_len;
     int retval = 0;
 
-    boxes = pcd->pcd_config->bounds.bounds_val;
-    boxes_len = pcd->pcd_config->bounds.bounds_len;
+    boxes = pc_data.pcd_config->bounds.bounds_val;
+    boxes_len = pc_data.pcd_config->bounds.bounds_len;
 
     for (i = 0; i < boxes_len; ++i) {
 	if (x >= boxes[i].x && y >= boxes[i].y &&
@@ -990,14 +1106,14 @@ int pc_point_in_bounds(struct pilot_connection_data *pcd,float x,float y) {
     return retval;
 }
 
-int pc_point_in_obstacle(struct pilot_connection_data *pcd,float x,float y) {
+int pc_point_in_obstacle(float x, float y) {
     int i;
     struct obstacle_config *oc;
     int oc_len;
     int retval = 0;
 
-    oc = pcd->pcd_config->obstacles.obstacles_val;
-    oc_len = pcd->pcd_config->obstacles.obstacles_len;
+    oc = pc_data.pcd_config->obstacles.obstacles_val;
+    oc_len = pc_data.pcd_config->obstacles.obstacles_len;
 
     for (i = 0; i < oc_len; ++i) {
 	if (x >= oc[i].xmin && y >= oc[i].ymin &&
diff --git a/robots/rmcd/pilotConnection.h b/robots/rmcd/pilotConnection.h
index 81df3223b885f66a0ebba1ebf8a56d57cddbac8d..4ec28aef5093f53a7572913772da6dc3de123258 100644
--- a/robots/rmcd/pilotConnection.h
+++ b/robots/rmcd/pilotConnection.h
@@ -51,6 +51,7 @@ struct pilot_connection {
 
     struct obstacle_config pc_obstacles[32];
     unsigned int pc_obstacle_count;
+    unsigned int pc_waypoint_tries;
 };
 
 #define REL2ABS(_dst, _theta, _rpoint, _apoint) { \
@@ -64,6 +65,8 @@ struct pilot_connection {
 
 struct pilot_connection *pc_add_robot(struct robot_config *rc);
 
+void pc_dump_info(void);
+
 /**
  * Map the robot ID to a gorobot_conn object.
  *
@@ -81,7 +84,7 @@ void pc_override_state(struct pilot_connection *pc, pilot_state_t ps);
 void pc_change_state(struct pilot_connection *pc, pilot_state_t ps);
 void pc_handle_packet(struct pilot_connection *pc, struct mtp_packet *mp);
 void pc_handle_signal(fd_set *rready, fd_set *wready);
-
+void pc_handle_timeout(struct timeval *current_time);
 
 /**
  * How close does the robot have to be before it is considered at the intended
@@ -102,7 +105,7 @@ void pc_handle_signal(fd_set *rready, fd_set *wready);
 /**
  * Maximum number of times to try and refine the position before giving up.
  */
-#define MAX_REFINE_RETRIES 5
+#define MAX_REFINE_RETRIES 4
 
 #define MAX_PILOT_CONNECTIONS 128
 
@@ -117,7 +120,7 @@ struct pilot_connection_data {
 
 extern struct pilot_connection_data pc_data;
 
-int pc_point_in_bounds(struct pilot_connection_data *pcd,float x,float y);
-int pc_point_in_obstacle(struct pilot_connection_data *pcd,float x,float y);
+int pc_point_in_bounds(float x,float y);
+int pc_point_in_obstacle(float x,float y);
 
 #endif
diff --git a/robots/rmcd/rclip.c b/robots/rmcd/rclip.c
index 7eee1ac624da71134c3076138b741f222ad462c4..1a67908b7a71a0d100b530a3dc52a9233bae8b1c 100644
--- a/robots/rmcd/rclip.c
+++ b/robots/rmcd/rclip.c
@@ -1,6 +1,7 @@
 
 #include "config.h"
 
+#include <math.h>
 #include <stdio.h>
 #include <assert.h>
 
@@ -63,6 +64,61 @@ rc_code_t rc_compute_closest(float x, float y, rc_rectangle_t r)
     return retval;
 }
 
+rc_code_t rc_closest_corner(float x, float y, rc_rectangle_t r)
+{
+    float tl, tr, bl, br, closest;
+    rc_code_t retval = 0;
+    
+    assert(r != NULL);
+
+    tl = hypotf(x - r->xmin, y - r->ymin);
+    tr = hypotf(x - r->xmax, y - r->ymin);
+    bl = hypotf(x - r->xmin, y - r->ymax);
+    br = hypotf(x - r->xmax, y - r->ymax);
+
+    closest = min(tl, min(tr, min(bl, br)));
+
+    if (tl == closest)
+	retval = RCF_TOP|RCF_LEFT;
+    else if (tr == closest)
+	retval = RCF_TOP|RCF_RIGHT;
+    else if (bl == closest)
+	retval = RCF_BOTTOM|RCF_LEFT;
+    else if (br == closest)
+	retval = RCF_BOTTOM|RCF_RIGHT;
+    else {
+	assert(0);
+    }
+    
+    return retval;
+}
+
+void rc_corner(rc_code_t rc, struct robot_position *rp, rc_rectangle_t r)
+{
+    assert(rc != 0);
+    assert(rp != NULL);
+    assert(r != NULL);
+
+    switch (rc) {
+    case RCF_TOP|RCF_LEFT:
+	rp->x = r->xmin;
+	rp->y = r->ymin;
+	break;
+    case RCF_TOP|RCF_RIGHT:
+	rp->x = r->xmax;
+	rp->y = r->ymin;
+	break;
+    case RCF_BOTTOM|RCF_LEFT:
+	rp->x = r->xmin;
+	rp->y = r->ymax;
+	break;
+    case RCF_BOTTOM|RCF_RIGHT:
+	rp->x = r->xmax;
+	rp->y = r->ymax;
+	break;
+    }
+}
+
 int rc_clip_line(rc_line_t line, rc_rectangle_t clip)
 {
     rc_code_t C0, C1, C;
diff --git a/robots/rmcd/rclip.h b/robots/rmcd/rclip.h
index 032a9962cb38d9588d0c3bb93a1a7cb0ee9b9a24..94b4725c08a7ac2004abd923d31924232531cf39 100644
--- a/robots/rmcd/rclip.h
+++ b/robots/rmcd/rclip.h
@@ -35,8 +35,20 @@ typedef struct rc_line {
     float y1;
 } *rc_line_t;
 
+#define RC_CODE_STRING(x) ( \
+    (x) == (RCF_TOP|RCF_LEFT) ? "tl" : \
+    (x) == (RCF_TOP) ? "t" : \
+    (x) == (RCF_TOP|RCF_RIGHT) ? "tr" : \
+    (x) == (RCF_RIGHT) ? "r" : \
+    (x) == (RCF_BOTTOM|RCF_RIGHT) ? "br" : \
+    (x) == (RCF_BOTTOM) ? "b" : \
+    (x) == (RCF_BOTTOM|RCF_LEFT) ? "bl" : \
+    (x) == (RCF_LEFT) ? "l" : "u")
+
+void rc_corner(rc_code_t rc, struct robot_position *rp, rc_rectangle_t r);
 rc_code_t rc_compute_code(float x, float y, rc_rectangle_t r);
 rc_code_t rc_compute_closest(float x, float y, rc_rectangle_t r);
+rc_code_t rc_closest_corner(float x, float y, rc_rectangle_t r);
 int rc_clip_line(rc_line_t line, rc_rectangle_t clip);
 
 #endif
diff --git a/robots/rmcd/rmcd.c b/robots/rmcd/rmcd.c
index c8dd027d5481bb643d625ae661ed3315cd6045a5..3556f769b6e1a500f4f7adb873be5f67ee12c661 100644
--- a/robots/rmcd/rmcd.c
+++ b/robots/rmcd/rmcd.c
@@ -72,6 +72,26 @@ static void usage(void)
 	    "Usage: rmcd [-hd] [-l logfile] [-i pidfile] [-e emchost] [-p emcport]\n");
 }
 
+#if defined(SIGINFO)
+/* SIGINFO-related stuff */
+
+/**
+ * Variable used to tell the main loop that we received a SIGINFO.
+ */
+static int got_siginfo = 0;
+
+/**
+ * Handler for SIGINFO that sets the got_siginfo variable and breaks the main
+ * loop so we can really handle the signal.
+ *
+ * @param sig The actual signal number received.
+ */
+static void siginfo(int sig)
+{
+    got_siginfo = 1;
+}
+#endif
+
 /**
  * Handle an mtp packet from emcd.
  *
@@ -154,14 +174,19 @@ int main(int argc, char *argv[])
     int c, emc_port = 0, retval = EXIT_SUCCESS;
     char *logfile = NULL, *pidfile = NULL;
     mtp_handle_t emc_handle = NULL;
+    struct timeval last_time;
     fd_set readfds;
 
 #if 0
     {
 	struct robot_config rc = { 1, "fakegarcia" };
 	struct pilot_connection *pc;
+	struct mtp_config_rmc rmc;
 
 	debug = 3;
+
+	memset(&rmc, 0, sizeof(rmc));
+	pc_data.pcd_config = &rmc;
 	
 	pc = pc_add_robot(&rc);
 
@@ -270,6 +295,10 @@ int main(int argc, char *argv[])
 	}
     }
 
+#if defined(SIGINFO)
+    signal(SIGINFO, siginfo);
+#endif
+
     if (emc_hostname != NULL || emc_path != NULL) {
 	struct mtp_packet mp, rmp;
 	
@@ -301,7 +330,7 @@ int main(int argc, char *argv[])
 	    pc_data.pcd_config = rmc_config;
 	    
 	    /*
-	     * Walk through the robot list and connect to the gorobots daemons
+	     * Walk through the robot list and connect to the pilot daemons
 	     * running on the robots.
 	     */
 	    for (lpc = 0; lpc < rmc_config->robots.robots_len; lpc++) {
@@ -332,10 +361,21 @@ int main(int argc, char *argv[])
 
     while (looping) {
 	fd_set rreadyfds = readfds;
+	struct timeval tv, diff;
 	int rc;
 
-	rc = select(FD_SETSIZE, &rreadyfds, NULL, NULL, NULL);
+#if defined(SIGINFO)
+	if (got_siginfo) {
+	    pc_dump_info();
+	    got_siginfo = 0;
+	}
+#endif
 
+	tv.tv_sec = 1;
+	tv.tv_usec = 0;
+	rc = select(FD_SETSIZE, &rreadyfds, NULL, NULL, &tv);
+	gettimeofday(&tv, NULL);
+	
 	if (rc > 0) {
 	    if (FD_ISSET(emc_handle->mh_fd, &rreadyfds)) {
 		do {
@@ -345,8 +385,14 @@ int main(int argc, char *argv[])
 
 	    pc_handle_signal(&rreadyfds, NULL);
 	}
-	else {
-	    errorc("accept\n");
+	else if (rc == -1 && errno != EINTR) {
+	    errorc("select\n");
+	}
+
+	timersub(&tv, &last_time, &diff);
+	if (diff.tv_sec > 1) {
+	    pc_handle_timeout(&tv);
+	    last_time = tv;
 	}
     }