Index: trunk/code/projects/colonet/robot/robot_slave/robot_slave.c
===================================================================
--- trunk/code/projects/colonet/robot/robot_slave/robot_slave.c	(revision 679)
+++ trunk/code/projects/colonet/robot/robot_slave/robot_slave.c	(revision 680)
@@ -14,6 +14,8 @@
 
 int behavior_status;
 
+static void step() {}
+
 int main(void) {
   dragonfly_init(ALL_ON);
 
@@ -28,13 +30,13 @@
   if (wl_token_ring_join() == 0) {
     orb_set_color(GREEN);
 
-    while(1) {
-      wl_do();
-    }
+    colonet_run(step);
   } else {
     orb_set_color(RED);
   }
 
+  usb_puts("Failed to join token ring.\n");
+
   while(1);
 
   return 0;
Index: trunk/code/projects/colonet/robot/robot_slave/Makefile
===================================================================
--- trunk/code/projects/colonet/robot/robot_slave/Makefile	(revision 679)
+++ trunk/code/projects/colonet/robot/robot_slave/Makefile	(revision 680)
@@ -15,8 +15,8 @@
 
 # com1 = serial port. Use lpt1 to connect to parallel port.
 #AVRDUDE_PORT = /dev/ttyUSB1
-AVRDUDE_PORT = /dev/ttyUSB0
-#AVRDUDE_PORT = /dev/ttyUSB2
+#AVRDUDE_PORT = /dev/ttyUSB0
+AVRDUDE_PORT = /dev/ttyUSB2
 
 #
 #
Index: trunk/code/projects/colonet/robot/colonet_dragonfly/colonet_dragonfly.c
===================================================================
--- trunk/code/projects/colonet/robot/colonet_dragonfly/colonet_dragonfly.c	(revision 679)
+++ trunk/code/projects/colonet/robot/colonet_dragonfly/colonet_dragonfly.c	(revision 680)
@@ -36,13 +36,16 @@
 
 static volatile unsigned robot_x, robot_y;
 static volatile unsigned last_x, last_y;
+static volatile unsigned target_x, target_y;
 static volatile int updated_robot_pos_ready;
-static volatile int new_movement_command_received;
 static volatile int robot_lost; //set to 1 if robot's position is not known by the server, else 0.
 static volatile VirtualWall virtual_wall;
 
-static int virtual_wall_available;
+typedef enum {MOVING, MOVING_TO_POSITION, NO_COMMAND} ColonetRobotCommandState;
+static volatile ColonetRobotCommandState robot_state;
 
+static volatile int virtual_wall_up_available, virtual_wall_low_available;
+
 #define ERROR_MAG_TO_MOTOR_DIFFENTIAL_CONST (1)
 
 /* Internal function prototypes */
@@ -61,6 +64,10 @@
   return (high<<8) | low;
 }
 
+static int virtual_wall_available() {
+  return virtual_wall_up_available && virtual_wall_low_available;
+}
+
 /* Public functions */
 int colonet_init() {
   colonet_pgh.groupCode = COLONET_PACKET_GROUP_ID;
@@ -75,15 +82,18 @@
   robot_x = last_x = 0;
   robot_y = last_y = 0;
   updated_robot_pos_ready = 0;
-  new_movement_command_received = 0;
   robot_lost = 1;
 
-  virtual_wall_available = 0;
+  virtual_wall_up_available = 0;
+  virtual_wall_low_available = 0;
+
   virtual_wall.up_x = 0;
   virtual_wall.up_y = 0;
   virtual_wall.low_x = 0;
   virtual_wall.low_y = 0;
 
+  robot_state = NO_COMMAND;
+
   return 0;
 }
 
@@ -162,124 +172,126 @@
 
 }
 
-static void move_routine() {
-  do {    
-    int count = 0;
-    while (!updated_robot_pos_ready) {
-      wl_do();
-      if (count++ == 5000) { // in case the server missed it...
-        request_abs_position();
-        count = 0;
-      }
+static int inside_virtual_wall(int x, int y) {
+  if (virtual_wall_available()) {
+    if (robot_x < virtual_wall.up_x || robot_x > virtual_wall.low_x || robot_y < virtual_wall.up_y
+        || robot_y > virtual_wall.low_y) {
+      return 0;
+    } else {
+      return 1;
     }
+  } else {
+    return 1;
+  }
+}
 
-    wl_do();
 
-    if (robot_lost) {
-      motors_off();
-    }
+/**
+ * Main loop which responds to commands.
+ */
+void colonet_run(void (*step)(void)) {
+  char buf[80];
 
-    updated_robot_pos_ready = 0;
+  while (1) {
+    wl_do();
     request_abs_position();
-  } while (!new_movement_command_received);
-}
+    step();
 
-static void move_to_position_routine(unsigned target_x, unsigned target_y) {
-  new_movement_command_received = 0;
+    switch (robot_state) {
+    case NO_COMMAND:
+      break;
 
-  usb_puts("move to absolute position routine!\n");
+    case MOVING:
+      if (robot_lost) {
+        motors_off();
+        robot_state = NO_COMMAND;
+      } else {
+        if (updated_robot_pos_ready) {
+          if (!inside_virtual_wall(robot_x, robot_y)) {
+            usb_puts("outside of boundary\n");
+            motors_off();
+            robot_state = NO_COMMAND;
+          }
 
-  updated_robot_pos_ready = 0;
-  request_abs_position(); // While we're doing this computation, server can be reporting next position.
+          updated_robot_pos_ready = 0;
+        }
+      }
+      break;
 
-  int count = 0;
-  while (!updated_robot_pos_ready) {
-    wl_do();
-    if (count++ == 5000) { // in case the server missed it...
-      request_abs_position();
-      count = 0;
-    }
-  }
+    case MOVING_TO_POSITION:
+      usb_puts("move to absolute position\n");
 
-  int dist = 0;
+      if (robot_lost) {
+        motors_off();
+        robot_state = NO_COMMAND;
+      } else {
+        if (updated_robot_pos_ready) {
+          updated_robot_pos_ready = 0;
 
-  while (!new_movement_command_received &&
-         (dist = dist_squared(robot_x, robot_y, target_x, target_y)) > TARGET_POSITION_STOP_DISTANCE_THRESHOLD) {
-    char buf[80];
-    //sprintf(buf, "dist:%d\n", dist);
-    //usb_puts(buf);
+          if (!inside_virtual_wall(robot_x, robot_y)) {
+            usb_puts("outside of boundary\n");
+            motors_off();
+            robot_state = NO_COMMAND;
+          } else {
+            int dist = 0;
+            if ((dist = dist_squared(robot_x, robot_y, target_x, target_y)) < TARGET_POSITION_STOP_DISTANCE_THRESHOLD) {
+              // Reached destination.
+              motors_off();
+              robot_state = NO_COMMAND;
+            } else {
+              // e is the error vector (where we want to go)
+              //char buf[80];
+              //		sprintf(buf, "target_x:%d target_y:%d robot_x:%d robot_y:%d\n", target_x, target_y, robot_x, robot_y);
+              //		usb_puts(buf);
 
-    updated_robot_pos_ready = 0;
-    request_abs_position();   // While we're doing this computation, server can be reporting next position.
+              int e_x = (int)target_x - (int)robot_x;
+              int e_y = (int)target_y - (int)robot_y;
 
-    if (robot_lost) {
-      usb_puts("lost known\n");
-      motors_off();
-    } else {
-      // e is the error vector (where we want to go)
-      //char buf[80];
-      //		sprintf(buf, "target_x:%d target_y:%d robot_x:%d robot_y:%d\n", target_x, target_y, robot_x, robot_y);
-      //		usb_puts(buf);
+              //		sprintf(buf, "e: (x:%d,y:%d)\n", e_x, e_y);
+              //		usb_puts(buf);
 
-      int e_x = (int)target_x - (int)robot_x;
-      int e_y = (int)target_y - (int)robot_y;
+              // v is the velocity vector (where we just went)
+              int v_x = (int)robot_x - (int)last_x;
+              int v_y = (int)robot_y - (int)last_y;
 
-      //		sprintf(buf, "e: (x:%d,y:%d)\n", e_x, e_y);
-      //		usb_puts(buf);
+              //sprintf(buf, "v: (x:%d,y:%d)\n", v_x, v_y);
+              //usb_puts(buf);
 
-      // v is the velocity vector (where we just went)
-      int v_x = (int)robot_x - (int)last_x;
-      int v_y = (int)robot_y - (int)last_y;
+              if (abs(v_x) < 2 && abs(v_x) < 2) {
+                // if we didn't move since the last check, just go forward.
+                motor1_set(1, 180);
+                motor2_set(1, 180);
+              } else {
+                int e_dot_v = e_x * v_x + e_y * v_y;
 
-      //sprintf(buf, "v: (x:%d,y:%d)\n", v_x, v_y);
-      //usb_puts(buf);
+                // move according to algortihm
+                int e_mag = e_x*e_x + e_y*e_y;
+                //       int motor_differential = 180;//e_mag >> 7;
+                int motor_differential = get_motor_differential(e_x, e_y, v_x, v_y);
 
-      if (abs(v_x) < 2 && abs(v_x) < 2) {
-        // if we didn't move since the last check, just go forward.
-        motor1_set(1, 180);
-        motor2_set(1, 180);
-      } else {
-        int e_dot_v = e_x * v_x + e_y * v_y;
+                // p is perpendicular to v, directed to the "left" of the robot.
+                int p_x = v_y;
+                int p_y = -v_x;
+                int dot_product_pe = p_x * e_x + p_y * e_y;
 
-        // move according to algortihm
-        int e_mag = e_x*e_x + e_y*e_y;
-        //       int motor_differential = 180;//e_mag >> 7;
-        int motor_differential = get_motor_differential(e_x, e_y, v_x, v_y);
+                // if the dot product of p and e is negative, we should turn right. otherwise turn left.
+                if (dot_product_pe < 0) {
+                  motor_differential = -motor_differential;
+                }
 
-        // p is perpendicular to v, directed to the "left" of the robot.
-        int p_x = v_y;
-        int p_y = -v_x;
-        int dot_product_pe = p_x * e_x + p_y * e_y;
-
-        // if the dot product of p and e is negative, we should turn right. otherwise turn left.
-        if (dot_product_pe < 0) {
-          motor_differential = -motor_differential;
+                set_motors_with_differential(motor_differential);
+              }
+            }
+          }
         }
-
-        set_motors_with_differential(motor_differential);
       }
-    }
 
-    if (robot_lost) {
-      motors_off();
+      break;
     }
-
-    // once we move, ask for the position again.
-    count = 0;
-    while (!updated_robot_pos_ready) {
-      wl_do();
-      if (count++ == 5000) { // in case the server missed it...
-        request_abs_position();
-        count = 0;
-      }
-    }
   }
+}
 
-  motors_off();
 
-  //  Once this function terminates, the robot be at its destination.
-}
-
 /* Private functions */
 
 /** @brief Handles colonet packets.  Should be called by parse_buffer
@@ -378,26 +390,39 @@
       last_x = robot_x;
       last_y = robot_y;
 
+      robot_lost = 1;
+
       usb_puts("lost\n");
 
-      robot_lost = 1;
-
       motors_off();
       break;
 
+    case SERVER_CLEAR_VIRTUAL_WALL:
+      virtual_wall_up_available = 0;
+      virtual_wall_low_available = 0;
+      break;
+
     case SERVER_REPORT_VIRTUAL_WALL_UPPER:
       virtual_wall.up_x = int_args[0];
       virtual_wall.up_y = int_args[1];
+
+      virtual_wall_up_available = 1;
+
+      sprintf(buf, "%d %d\n", virtual_wall.up_x, virtual_wall.up_y);
+      usb_puts(buf);
       break;
 
     case SERVER_REPORT_VIRTUAL_WALL_LOWER:
       virtual_wall.low_x = int_args[0];
       virtual_wall.low_y = int_args[1];
+
+      sprintf(buf, "%d %d\n", virtual_wall.low_x, virtual_wall.low_y);
+      usb_puts(buf);
+
+      virtual_wall_low_available = 1;
       break;
 
     case SERVER_REPORT_POSITION_TO_ROBOT:
-      robot_lost = 0;
-
       last_x = robot_x;
       last_y = robot_y;
       robot_x = (unsigned)int_args[0];
@@ -404,26 +429,20 @@
       robot_y = (unsigned)int_args[1];
 
       updated_robot_pos_ready = 1;
+      robot_lost = 0;
 
-      if (virtual_wall_available) {
-        if (robot_x < virtual_wall.up_x || robot_x > virtual_wall.low_x || robot_y < virtual_wall.up_x
-            || robot_y > virtual_wall.up_y) {
-          robot_lost = 1;
-          motors_off();
-        }
-      }
-
-      //    sprintf(buf, "pos is: (x:%d, y:%d)\n", robot_x, robot_y);
+      //      sprintf(buf, "pos is: (x:%d, y:%d)\n", robot_x, robot_y);
       //usb_puts(buf);
       break;
 
     case MOVE_TO_ABSOLUTE_POSITION:
-      new_movement_command_received = 1;
-
 			sprintf(buf, "move to abs (x:%u, y:%u)\n", (unsigned)int_args[0], (unsigned)int_args[1]);
 			usb_puts(buf);
 
-      move_to_position_routine((unsigned)int_args[0], (unsigned)int_args[1]);
+      target_x = (unsigned)int_args[0];
+      target_y = (unsigned)int_args[1];
+
+      robot_state = MOVING_TO_POSITION;
       break;
 
       //Buzzer
@@ -466,24 +485,8 @@
       motors_init();
       break;
 
-    case MOTOR1_SET:
-      new_movement_command_received = 1;
-
-      sprintf(buf, "calling motor1_set [%i] [%i]\n", args[0], args[1]);
-      usb_puts(buf);
-      motor1_set(args[0], args[1]);
-      break;
-
-    case MOTOR2_SET:
-      new_movement_command_received = 1;
-
-      sprintf(buf, "calling motor2_set [%i] [%i]\n", args[0], args[1]);
-      usb_puts(buf);
-      motor2_set(args[0], args[1]);
-      break;
-
     case MOTORS_OFF:
-      new_movement_command_received = 1;
+      robot_state = NO_COMMAND;
 
       usb_puts("calling motors_off\n");
       motors_off();
@@ -490,22 +493,27 @@
       break;
 
     case MOVE:
-      new_movement_command_received = 1;
-      /* format for move: left direction, left velocity, right direction, right velocity */
-      sprintf(buf, "calling move\n");
-      usb_puts(buf);
-      motor1_set(args[0], args[1]);
-      motor2_set(args[2], args[3]);
+      if (args[1] == 0 && args[3] == 0) {
+        motors_off();
+        robot_state = MOVING;
 
-      /* Loop and update position for virtual wall. */
-      move_routine();
+        usb_puts("stopped\n");
+      } else {
+        /* format for move: left direction, left velocity, right direction, right velocity */
+        motor1_set(args[0], args[1]);
+        motor2_set(args[2], args[3]);
+
+        /* Loop and update position for virtual wall. */
+        robot_state = MOVING;
+      }
       break;
 
     case PRINTF:
       usb_puts((char*)pkt->data);
       break;
+
     case KILL_ROBOT:
-      new_movement_command_received = 1;
+      robot_state = NO_COMMAND;
 
       motors_off();
       buzzer_off();
Index: trunk/code/projects/colonet/robot/colonet_dragonfly/colonet_dragonfly.h
===================================================================
--- trunk/code/projects/colonet/robot/colonet_dragonfly/colonet_dragonfly.h	(revision 679)
+++ trunk/code/projects/colonet/robot/colonet_dragonfly/colonet_dragonfly.h	(revision 680)
@@ -13,6 +13,10 @@
  */
 int colonet_init(void);
 
+/**
+ */
+void colonet_run(void (*step)(void));
+
 /** @brief Registers a new colonet message handler function.  If a message
  * with msgId is received, then handler will be called.
  *
Index: trunk/code/projects/colonet/common/colonet_defs.h
===================================================================
--- trunk/code/projects/colonet/common/colonet_defs.h	(revision 679)
+++ trunk/code/projects/colonet/common/colonet_defs.h	(revision 680)
@@ -156,6 +156,8 @@
 #define SERVER_REPORT_VIRTUAL_WALL_UPPER 0x5A
 #define SERVER_REPORT_VIRTUAL_WALL_LOWER 0x5B
 
+#define SERVER_CLEAR_VIRTUAL_WALL 0x5C
+
 ///////////////////////////
 //Colonet specific commands
 //////////////////////////
