Fixes for 'lost' robots: the vision system will now periodically try to

wiggle 'lost' robots (those for whom a wiggle, or subsequent wiggles, were
not detectable).

robots/vmcd/robotObject.h

@@ -18,6 +18,7 @@ struct robot_object {
     unsigned long ro_flags;
     char *ro_name;
     int ro_id;
+    struct timeval ro_lost_timestamp;
 };
 
 struct robot_object *roFindRobot(struct lnMinList *list, int id);

robots/vmcd/visionTrack.c

@@ -35,12 +35,14 @@ static struct vision_track *vtFindMin(struct vision_track *vt,
 
 	distance = hypotf(vt->vt_position.x - curr->vt_position.x,
 			  vt->vt_position.y - curr->vt_position.y);
+#if 0
 	printf("  min %f %f - %f %f = %f\n",
 	       vt->vt_position.x,
 	       vt->vt_position.y,
 	       curr->vt_position.x,
 	       curr->vt_position.y,
 	       distance);
+#endif
 	if (distance < *distance_out) {
 	    retval = curr;
 	    *distance_out = distance;
@@ -97,14 +99,14 @@ int vtUpdate(struct lnMinList *now,
 	    if (mup->position.y > vc->vc_bottom)
 		vc->vc_bottom = mup->position.y;
 
-	    
+#if 0
 	    printf("vc %p %f %f %f %f\n",
 		   vc,
 		   vc->vc_left,
 		   vc->vc_right,
 		   vc->vc_top,
 		   vc->vc_bottom);
-	    
+#endif
 	    retval = (mup->status == MTP_POSITION_STATUS_CYCLE_COMPLETE);
 	}
 	break;

robots/vmcd/vmcd.c

@@ -445,8 +445,12 @@ int main(int argc, char *argv[])
 	     * Send MTP_WIGGLE_STARTs for any unknown robots, this should stop
 	     * them and cause an IDLE wiggle_status to come back.
 	     */
+
 	    ro = (struct robot_object *)unknown_bots.lh_Head;
 	    while (ro->ro_link.ln_Succ != NULL) {
+		info("checking through unknown_bots; id %d\n",
+		     ro->ro_id
+		     );
 		if (!(ro->ro_flags & RF_WIGGLING)) {
 		    struct mtp_packet wmp;
 
@@ -478,6 +482,44 @@ int main(int argc, char *argv[])
 	    current_client = 0;
 	}
 
+	/*
+	 * Check which bots in the lost list have exceeded the timeout;
+	 * if any have, add them back to the unknown_bots list, and they
+	 * will be rewiggled once the next set of updates comes from the
+	 * vision system.
+	 */
+	if (!lnEmptyList(&lost_bots)) {
+	    struct robot_object *ro = NULL;
+	    struct robot_object *ro_next = NULL;
+	    struct timeval current;
+
+	    gettimeofday(&current,NULL);
+
+	    ro = (struct robot_object *)lost_bots.lh_Head;
+	    while (ro->ro_link.ln_Succ != NULL) {
+		if ((current.tv_sec - ro->ro_lost_timestamp.tv_sec) > 10) {
+		    // remove this guy
+		    ro_next = (struct robot_object *)ro->ro_link.ln_Succ;
+		    lnRemove(&ro->ro_link);
+		    // add him to unknown list
+		    lnAddTail(&unknown_bots,&ro->ro_link);
+		    info("robot %d exceeded the lost timeout, trying to "
+			 "reacquire.\n",
+			 ro->ro_id
+			 );
+		    // also have to unset the RF_WIGGLING flag
+		    ro->ro_flags &= ~RF_WIGGLING;
+		    ro = ro_next;
+		}
+		else {
+		    //info("robot %d did not exceed the lost timeout!\n",
+		    // ro->ro_id
+		    // );
+		    ro = (struct robot_object *)ro->ro_link.ln_Succ;
+		}
+	    }
+	}
+
 	/*
 	 * Check if we need to identify any bots, assuming we're not in the
 	 * process of finding one.
@@ -564,6 +606,14 @@ int main(int argc, char *argv[])
 				     */
 				    lnAddTail(&lost_bots,
 					      &wiggle_bot->ro_link);
+
+				    gettimeofday(&(wiggle_bot->ro_lost_timestamp),
+						 NULL);
+				    info("took a lost timestamp for robot"
+					 " %d\n",
+					 wiggle_bot->ro_id
+					 );
+
 				    wiggle_bot = NULL;
 				}
 				else {
@@ -608,9 +658,11 @@ int main(int argc, char *argv[])
 			}
 			
 			if (vtUpdate(&current_frame, vc, &mp, &vt_pool)) {
-			    printf("got frame from client %s:%d\n",
-				   vc->vc_hostname,
-				   vc->vc_port);
+			    if (debug > 1) {
+				printf("got frame from client %s:%d\n",
+				       vc->vc_hostname,
+				       vc->vc_port);
+			    }
 
 			    /*
 			     * Got all of the tracks from this client, clear