/* extract the time data for the 6811 */  byte_count += timePackageProcess ( &inbuf[byte_count], &posDataTime );}/* process the response from the robot which encodes the   compass reading of the robot */static void Process_Compass_Conf_Pkg(unsigned char inbuf[BUFSIZE]){  int byte_count = 1;    printf("compass calibration score x: %d y: %d z: %d\n",	 inbuf[byte_count], 	 inbuf[byte_count+1], 	 inbuf[byte_count+2]);}/* process the response from the robot which encodes the   bumper reading of the robot */static void Process_Bumper_Pkg(unsigned char inbuf[BUFSIZE]){  int byte_count = 1;    if (model == MODEL_SCOUT)  {    State[ STATE_BUMPER ] = combine_bumper_vector(inbuf[byte_count + 0], 						  inbuf[byte_count + 1],						  inbuf[byte_count + 2]);  }  else  {    State[ STATE_BUMPER ] = combine_bumper_vector(inbuf[byte_count + 2], 						  inbuf[byte_count + 1],						  inbuf[byte_count + 0]);  }    byte_count +=3;  /*   * if the position attachment was requested for the bumper   * we have to unpack the package.    */  if ( POS_BUMPER_PI ( usedSmask[ SMASK_POS_DATA ] ) )    byte_count += posPackageProcess ( &inbuf[byte_count],   				      &( posDataAll.bumper ) );  /* extract the time data for the 6811 */  byte_count += timePackageProcess ( &inbuf[byte_count], &posDataTime );}static void Process_Laser_Point_Pkg(unsigned char inbuf[BUFSIZE]){  int i, byte_count = 1;    Laser[0]=two_bytes_to_unsigned_int(inbuf[byte_count],inbuf[byte_count+1]);  byte_count = byte_count+2;  for (i=1; i<=Laser[0]; i++) {    Laser[i] = two_bytes_to_signed_int(inbuf[byte_count], inbuf[byte_count+1]);    byte_count = byte_count+2;  }  convert_laser(Laser);  Laser[0] = inbuf[byte_count] + 256 * inbuf[byte_count+1];  byte_count = byte_count + 2;    if ( Laser[0] > NUM_LASER ) {    printf("error in processing laser point reply\n");    errorp = SERIAL_READ_ERROR;    Laser[0] = 0;    return;  }  for (i=1; i<=Laser[0]; i++) {    Laser[i] = two_bytes_to_unsigned_int(inbuf[byte_count], 					 inbuf[byte_count+1]);    byte_count = byte_count+2;  }  if ((laser_mode == 51) || (laser_mode == 50) || (laser_mode == 19))    convert_laser(Laser);  /*   * if the position attachment was requested for the laser   * we have to unpack the package.    */  if ( POS_LASER_PI ( usedSmask[ SMASK_POS_DATA ] ) )    byte_count += posPackageProcess ( &inbuf[byte_count],   				      &( posDataAll.laser ) );  /* extract the time data for the 6811 */  byte_count += timePackageProcess ( &inbuf[byte_count], &posDataTime );}static void Process_Laser_Line_Pkg(unsigned char inbuf[BUFSIZE]){   int i, byte_count = 1;    Laser[0] = inbuf[byte_count] + 256 * inbuf[byte_count+1];  byte_count = byte_count + 2;  if (Laser[0] > NUM_LASER) {    printf("error in processing laser line reply\n");    errorp = SERIAL_READ_ERROR;    Laser[0] = 0;    return;  }  for (i=1; i<=4*Laser[0]; i++) {    Laser[i] = two_bytes_to_signed_int(inbuf[byte_count], 				       inbuf[byte_count+1]);    byte_count = byte_count+2;  }  /*   * if the position attachment was requested for the laser   * we have to unpack the package.    */  if ( POS_LASER_PI ( usedSmask[ SMASK_POS_DATA ] ) )    byte_count += posPackageProcess ( &inbuf[byte_count],   				      &( posDataAll.laser ) );  /* extract the time data for the 6811 */  byte_count += timePackageProcess ( &inbuf[byte_count], &posDataTime );}/* process the response from the robot which encodes special information */static void Process_Special_Pkg(unsigned char inbuf[BUFSIZE]){  int data_size, i, byte_count = 1;    data_size = two_bytes_to_unsigned_int(inbuf[1],inbuf[2]);  special_answer_size = ( unsigned short ) data_size;  if (data_size > MAX_USER_BUF)     data_size = MAX_USER_BUF;  if ( special_buffer != (unsigned char *)NULL)    {      for (i=0; i<data_size; i++) 	{	  special_buffer[i] = inbuf[i+1];	  byte_count++;	}    }  else    printf("Data buffer for user package is NULL pointer\n");}static int Process_Robot_Resp(unsigned char inbuf[BUFSIZE]){  switch (inbuf[0]) { /* type of the returned package */  case AC:  case SP:  case PR:  case PA:  case VM:  case MV:  case CT:  case GS:  case ST:  case LP:  case DP:  case DA:  case WS:  case ZR:  case TK:  case CONF_IR:  case CONF_SN:  case CONF_LS:  case CONF_TM:  case CONF_SG:  case CONF_SER:  case SETUP_LS:    Process_State_Pkg(inbuf);    break;  case CONF_CP:    Process_Compass_Conf_Pkg(inbuf);    break;  case NAK: /* Nak */    printf("Nak\n");    break;  case GET_IR: /* Infrared */    Process_Infrared_Pkg(inbuf);    break;  case GET_SN: /* Sonar */    Process_Sonar_Pkg(inbuf);    break;  case GET_RC: /* Configuration */    Process_Configuration_Pkg(inbuf);    break;  case GET_RV: /* Velocity */    Process_Velocity_Pkg(inbuf);    break;  case GET_RA: /* Acceleration */    Process_Acceleration_Pkg(inbuf);    break;  case GET_CP: /* Compass */    Process_Compass_Pkg(inbuf);    break;  case GET_LS: /* Laser */    Process_Laser_Point_Pkg(inbuf);    break;  case GET_BP: /* Bumper */    Process_Bumper_Pkg(inbuf);    break;  case GET_SG: /* Laser line mode */    Process_Laser_Line_Pkg(inbuf);    break;  case SPECIAL: /* User */    Process_Special_Pkg(inbuf);    break;  default:    printf("Invalid Robot Response\n");    return(FALSE);    break;  }  return(TRUE);}static int Comm_Robot(int fd, unsigned char command[BUFSIZE]){  unsigned char response[BUFSIZE];  int respondedp;  int re_xmitp, i;  fd_set          lfdvar;  struct timeval  timeout;  if (fd == -1) {    fprintf(stderr,	    "Trying again to reestablish connection with the robot...\n"	    "                          ");    fflush(stderr);    for (i = 0; (i < 2) &&         (connect_robot(Robot_id, model, device, conn_value) == FALSE);         i++)    {      sleep(5);      fprintf(stderr, "Trying again...           ");      fflush(stderr);    }    if (i == 2 &&        connect_robot(Robot_id, model, device, conn_value) == FALSE)    {      fprintf(stderr, "Failed to reestablish connection.  Command aborted.\n");      return FALSE;    }    fprintf(stderr, "Successful!  Continuing with command.\n");  }  re_xmitp = RE_XMIT;  FD_ZERO(&lfdvar);  FD_SET(fd, &lfdvar);    timeout.tv_sec = 0;  timeout.tv_usec = 0;    while (select(fd+1, &lfdvar, NULL, NULL, &timeout) != 0)  {    /* Flush buffer */    respondedp = read(fd, response, BUFSIZE);    /* Check for errors, such as lost connection. */    if (respondedp <= 0 && errno != EWOULDBLOCK)    {      close(fd);      Fd = -1;      fprintf(stderr,	      "Lost communication with robot.\nAttempting to reconnect...");      fflush(stderr);      for (i = 0; (i < 2) &&           (connect_robot (Robot_id, model, device, conn_value) == FALSE);           i++)      {	sleep(5);	fprintf(stderr, "Trying again...           ");	fflush(stderr);      }      if (i == 2 &&          connect_robot (Robot_id, model, device, conn_value) == FALSE)      {	fprintf(stderr, "Unable to reconnect to robot.  Command aborted.\n");	return FALSE;      }      else      {	fprintf(stderr, "Successful!  Continuing with command.\n");      }    }  }    Write_Pkg(fd, command);  while (!(respondedp = Read_Pkg(fd, connect_type, response)) && (RE_XMIT)) {    Write_Pkg(fd, command);  }  if (!respondedp) {    printf("Last command packet transmitted:\n");    for (i = 0; i < command[1]+4; i++)      printf("%2.2x ", command[i]);    printf("\n");    return(FALSE);  }  else {    if (Process_Robot_Resp (response)) {      return(TRUE);    }    else {      printf("error in robot response\n");      return(FALSE);     }  }}/************************************ *                                  * * Robot Serial Interface Functions * *                                  * ************************************//*  * First some helper functions  */void stuff_length_type(int length, int ptype, unsigned char *byte_ptr);void stuff_length_type(int length, int ptype, unsigned char *byte_ptr){     byte_ptr++; /* skip the first byte of the buffer, which is 		    reserved for begin_pkg character */     unsigned_int_to_two_bytes(length, byte_ptr);     byte_ptr++; byte_ptr++;     *byte_ptr = ptype;}void stuff_two_signed_int(int length, int ptype, int num1, int num2,			  unsigned char *byte_ptr);void stuff_two_signed_int(int length, int ptype, int num1, int num2,			  unsigned char *byte_ptr){     byte_ptr++; /* skip the first byte of the buffer, which is 		    reserved for begin_pkg character */     unsigned_int_to_two_bytes(length, byte_ptr);     byte_ptr++; byte_ptr++;     *byte_ptr = ptype;     byte_ptr++;     signed_int_to_two_bytes(num1, byte_ptr);     byte_ptr++; byte_ptr++;     signed_int_to_two_bytes(num2, byte_ptr);}void stuff_three_unsigned_int(int length, int ptype, int num1, int num2,			      int num3, unsigned char *byte_ptr);void stuff_three_unsigned_int(int length, int ptype, int num1, int num2,			      int num3, unsigned char *byte_ptr){     byte_ptr++; /* skip the first byte of the buffer, which is 		    reserved for begin_pkg character */     unsigned_int_to_two_bytes(length, byte_ptr);     byte_ptr++; byte_ptr++;     *byte_ptr = ptype;     byte_ptr++;     unsigned_int_to_two_bytes(num1, byte_ptr);     byte_ptr++; byte_ptr++;     unsigned_int_to_two_bytes(num2, byte_ptr);     byte_ptr++; byte_ptr++;     unsigned_int_to_two_bytes(num3, byte_ptr);}void stuff_three_signed_int(int length, int ptype, int num1, int num2,			    int num3, unsigned char *byte_ptr);void stuff_three_signed_int(int length, int ptype, int num1, int num2,			    int num3, unsigned char *byte_ptr){     byte_ptr++; /* skip the first byte of the buffer, which is 		    reserved for begin_pkg character */     unsigned_int_to_two_bytes(length, byte_ptr);     byte_ptr++; byte_ptr++;     *byte_ptr = ptype;     byte_ptr++;     signed_int_to_two_bytes(num1, byte_ptr);     byte_ptr++; byte_ptr++;     signed_int_to_two_bytes(num2, byte_ptr);     byte_ptr++; byte_ptr++;     signed_int_to_two_bytes(num3, byte_ptr);}/*************** * FUNCTION:     posLongExtract * PURPOSE:      compose a long out of four bytes * ARGUMENTS:    unsigned char *inbuf : the pointer to the four bytes * ALGORITHM:    bit manipulation * RETURN:       long * SIDE EFFECT:   * CALLS:         * CALLED BY:     ***************/static long posLongExtract( unsigned char *inbuf ){  long tmp;    tmp = (long) LONG_B(inbuf[3],inbuf[2],inbuf[1],inbuf[0]);    if ( tmp & (1L << 31) )    return ( -(tmp & ~(1L << 31) ) );  else    return ( tmp );}/*************** * FUNCTION:     posUnsignedLongExtract * PURPOSE:      compose an unsigned long out of four bytes * ARGUMENTS:    unsigned char *inbuf : the pointer to the four bytes * ALGORITHM:    bit manipulation * RETURN:       usigned long * SIDE EFFECT:   * CALLS:         * CALLED BY:     ***************/static unsigned long posUnsignedLongExtract( unsigned char *inbuf ){  return ( (unsigned long) LONG_B(inbuf[3],inbuf[2],inbuf[1],inbuf[0]) );}/*************** * FUNCTION:     posPackageProcess * PURPOSE:      processes the part of the package with pos information * ARGUMENTS:    unsigned char *inbuf : pointer to the data in chars *               PosData *posData : this is were the posData are written to * ALGORITHM:    regroup the bytes and assign variables * RETURN:       int (the number of bytes read from the buffer) * SIDE EFFECT:   * CALLS:         * CALLED BY:     ***************/static int posPackageProcess ( unsigned char *inbuf, PosData *posData ){  int i = 0;  /* copy the stuff from the buffer into the posData for the current robot */  posData->config.configX      = posLongExtract(inbuf + i++ * sizeof(long));  posData->config.configY      = posLongExtract(inbuf + i++ * sizeof(long));  posData->config.configSteer  = posLongExtract(inbuf + i++ * sizeof(long));  posData->config.configTurret = posLongExtract(inbuf + i++ * sizeof(long));  posData->config.velTrans     = posLongExtract(inbuf + i++ * sizeof(long));  posData->config.velSteer     = posLongExtract(inbuf + i++ * sizeof(long));  posData->config.velTurret    = posLongExtract(inbuf + i++ * sizeof(long));  posData->config.timeStamp    = posUnsignedLongExtract(inbuf + i++ * 							sizeof(long));  posData->timeStamp           = posUnsignedLongExtract(inbuf + i++ * 							sizeof(long));    return ( i * sizeof(long) );}/*************** * FUNCTION:     timePackageProcess * PURPOSE:      processes the part of the package with the 6811 time * ARGUMENTS:    unsigned char *inbuf : pointer to the data in chars *               unsigned long *time : this is were the time is written to * ALGORITHM:    --- * RETURN:       static int  * SIDE EFFECT:   * CALLS:         * CALLED BY:     ***************/static int timePackageProcess ( unsigned char *inbuf, unsigned long *timeS ){  *timeS = posUnsignedLongExtract( inbuf );  return ( 4 );}/*************** * FUNCTION:     voltPackageProcess * PURPOSE:      processes the part of the package with the voltages * ARGUMENTS:    unsigned char *inbuf : pointer to the data in chars *               unsigned long *time : this is were the time is written to