diff --git a/robots/emc/emcd.c b/robots/emc/emcd.c
index f05155ec8aa1fefecf7d50f463f07ae54437ba2d..b2da127ab5c4fe4cd9aafada229f40c771231215 100755
--- a/robots/emc/emcd.c
+++ b/robots/emc/emcd.c
@@ -936,9 +936,13 @@ void ev_callback(event_handle_t handle,
 			MA_Speed, speed,
 			MA_TAG_DONE);
 
+	match->flags |= ERF_HAS_GOAL;
+	match->last_goal_pos = mp.data.mtp_payload_u.command_goto.position;
+
 	if (rmc_data.handle != NULL) {
 	    mtp_send_packet(rmc_data.handle, &mp);
 	}
+#if 0
 	else {
 	    mtp_print_packet(stdout, &mp);
 	    
@@ -953,6 +957,7 @@ void ev_callback(event_handle_t handle,
 		     EA_TAG_DONE);
 #endif
 	}
+#endif
 	
 	mtp_free_packet(&mp);
     }
@@ -1077,15 +1082,26 @@ int unknown_client_callback(elvin_io_handler_t handler,
 	  struct emc_robot_config *erc = rli->data;
 	  struct mtp_packet gmp;
 
-	  mtp_init_packet(&gmp,
-			  MA_Opcode, MTP_COMMAND_GOTO,
-			  MA_Role, MTP_ROLE_EMC,
-			  MA_CommandID, 1,
-			  MA_RobotID, erc->id,
-			  MA_X, (double)erc->init_x,
-			  MA_Y, (double)erc->init_y,
-			  MA_Theta, (double)erc->init_theta,
-			  MA_TAG_DONE);
+	  if (erc->flags & ERF_HAS_GOAL) {
+	    mtp_init_packet(&gmp,
+			    MA_Opcode, MTP_COMMAND_GOTO,
+			    MA_Role, MTP_ROLE_EMC,
+			    MA_CommandID, 1,
+			    MA_RobotID, erc->id,
+			    MA_Position, &erc->last_goal_pos,
+			    MA_TAG_DONE);
+	  }
+	  else {
+	    mtp_init_packet(&gmp,
+			    MA_Opcode, MTP_COMMAND_GOTO,
+			    MA_Role, MTP_ROLE_EMC,
+			    MA_CommandID, 1,
+			    MA_RobotID, erc->id,
+			    MA_X, (double)erc->init_x,
+			    MA_Y, (double)erc->init_y,
+			    MA_Theta, (double)erc->init_theta,
+			    MA_TAG_DONE);
+	  }
 	  mtp_send_packet(mh, &gmp);
 	}
 	
@@ -1377,7 +1393,7 @@ int emulab_callback(elvin_io_handler_t handler,
 				  mp->data.mtp_payload_u.command_goto.position.x,
 				  mp->data.mtp_payload_u.command_goto.position.y)
 	    ) {
-
+	  
 	    /* forward the packet on to rmc... */
 	    if (rmc_data.handle == NULL) {
 		error("no rmcd yet\n");
diff --git a/robots/emc/emcd.h b/robots/emc/emcd.h
index 9063f14d3e8fbab3214f2458364705e9d78d81b8..9fd0c4f9715622647ecbc454e44c7e5d19c86693 100644
--- a/robots/emc/emcd.h
+++ b/robots/emc/emcd.h
@@ -4,6 +4,14 @@
 
 #include "mtp.h"
 
+enum {
+  ERB_HAS_GOAL,
+};
+
+enum {
+  ERF_HAS_GOAL = (1L << ERB_HAS_GOAL),
+};
+
 struct emc_robot_config {
   int id;
   char *hostname;
@@ -14,6 +22,8 @@ struct emc_robot_config {
   float init_y;
   float init_theta;
   struct robot_position last_update_pos;
+  struct robot_position last_goal_pos;
+  unsigned long flags;
 };
 
 struct rmc_client {