Index: colonet_dragonfly.c
===================================================================
--- colonet_dragonfly.c	(revision 530)
+++ colonet_dragonfly.c	(revision 531)
@@ -29,7 +29,7 @@
 
 static int robot_x, robot_y;
 static volatile int updated_robot_pos_ready;
-#define ERROR_MAG_TO_MOTOR_DIFFENTIAL_CONST (10)
+#define ERROR_MAG_TO_MOTOR_DIFFENTIAL_CONST (80)
 
 /* Internal function prototypes */
 static unsigned int two_bytes_to_int(char high, char low);
@@ -51,7 +51,7 @@
 
   robot_x = 0;
   robot_y = 0;
-  updated_robot_pos_ready = 1;
+  updated_robot_pos_ready = 0;
 
   return 0;
 }
@@ -77,7 +77,7 @@
 }
 
 void request_abs_position() {
-  //usb_puts("requesting_abs_position\n");
+  usb_puts("requesting_abs_position\n");
 
   ColonetRobotServerPacket pkt;
   pkt.client_id = -1;
@@ -124,21 +124,37 @@
 }
 
 static void move_to_position_routine(int target_x, int target_y) {
- int last_x = robot_x, last_y = robot_y;
+  int last_x = robot_x, last_y = robot_y;
 
- while (distsquared(robot_x, robot_y, target_x, target_y) >
+  char buf[40];
+  sprintf(buf, "cur dist is %d\n", distsquared(robot_x, robot_y, target_x, target_y));
+  usb_puts(buf);
+  sprintf(buf, "radius squared is %d\n", TARGET_POSITION_STOP_DISTANCE_THRESHOLD*TARGET_POSITION_STOP_DISTANCE_THRESHOLD);
+  usb_puts(buf);
+
+  while (distsquared(robot_x, robot_y, target_x, target_y) >
         TARGET_POSITION_STOP_DISTANCE_THRESHOLD*TARGET_POSITION_STOP_DISTANCE_THRESHOLD) {
+    request_abs_position(); // While we're doing this computation, server can be reporting next position.
+
+    int count = 0;
     while (!updated_robot_pos_ready) { // ghetto condition variable
       wl_do();
+
+      if (count++ == 10000) { // in case the server missed it...
+        request_abs_position();
+        count = 0;
+      }
     }
 
     int cur_robot_x = robot_x;
     int cur_robot_y = robot_y;
 
-    char buf[40];
-    sprintf(buf, "Position: %d %d\n", cur_robot_x, cur_robot_y);
+    sprintf(buf, "Current position: %d %d\n", cur_robot_x, cur_robot_y);
     usb_puts(buf);
 
+    sprintf(buf, "Target position: %d %d\n", target_x, target_y);
+    usb_puts(buf);
+
     updated_robot_pos_ready = 0;
     request_abs_position(); // While we're doing this computation, server can be reporting next position.
 
@@ -158,6 +174,9 @@
     float e_y = r_y - v_y;
     float e_mag = sqrt_approx(e_x * e_x + e_y * e_y);
 
+    sprintf(buf, "Error: <%f,%f>; mag: %f\n", e_x, e_y, e_mag);
+    usb_puts(buf);
+
     /* Motor differential proportional to magnitude of directional error. */
     int motor_differential = (int)(e_mag * ERROR_MAG_TO_MOTOR_DIFFENTIAL_CONST);
 
@@ -181,15 +200,16 @@
       motor_differential = -motor_differential;
     }
 
+    sprintf(buf, "motor_diff: %d\n", motor_differential);
+    usb_puts(buf);
+
     last_x = cur_robot_x;
     last_y = cur_robot_y;
 
-
-    sprintf(buf, "motor_diff: %d\n", motor_differential);
-    usb_puts(buf);
-
     set_motors_with_differential(motor_differential);
   }
+  
+  usb_puts("reached destination!\n");
 }
 
 /* Private functions */
@@ -238,7 +258,8 @@
   int_args[2] = two_bytes_to_int(args[4], args[5]);
 
   if (type == COLONET_REQUEST) {
-    usb_puts("received a COLONET_REQUEST\n");
+    sprintf(buf, "received colonet request: pkt msgcode is %d\n", pkt->msg_code);
+    usb_puts(buf);
 
     switch (pkt->msg_code) {
       //Sharp
@@ -279,7 +300,8 @@
       break;
     }
   } else if (type == COLONET_COMMAND) {
-    usb_puts("colonet command ... %d\n", pkt->msg_code);
+    sprintf(buf, "colonet command ... pkt->msg_code=%d\n", pkt->msg_code);
+    usb_puts(buf);
 
 /* TODO uncomment this stuff */
 /*     if (pkt.msg_code >= USER_DEFINED_MSG_ID_START && */
@@ -301,7 +323,7 @@
       robot_x = int_args[0];
       robot_y = int_args[1];
 
-      updated_robot_pos_ready;
+      updated_robot_pos_ready = 1;
 
       sprintf(buf, "pos is: %d %d\n", robot_x, robot_y);
       usb_puts(buf);
@@ -308,7 +330,7 @@
       break;
 
     case MOVE_TO_ABSOLUTE_POSITION:
-      usb_puts("move to abs position!\n");
+      usb_puts("move to abs position2!\n");
       move_to_position_routine(int_args[0], int_args[1]);
       break;
 
