ndirect.c

TeamBots 是一个可移植的多代理机器人仿真器

						inbuf[byte_count + 2]);
  byte_count +=3;

  /*
   * if the position attachment was requested for the bumper
   * we have to unpack the package. 
   */

#if NOMAD150
#else
  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 );
#endif
}

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])
{
  int selectval;
  unsigned char response[BUFSIZE];
  int respondedp;
  int re_xmitp, i;
  fd_set          lfdvar;
  struct timeval  timeout;

  //fprintf(stderr,"Comm_Robot: fd == -1 \n");//trb
  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 ) == FALSE); i++)
    {
      sleep(5);
      fprintf(stderr, "Trying again...           ");
      fflush(stderr);
    }
    if (i == 2 && connect_robot( Robot_id ) == FALSE)
    {
      fprintf(stderr, "Failed to reestablish connection.  Command aborted.\n");
      return FALSE;
    }
    fprintf(stderr, "Successful!  Continuing with command.\n");
  }
  //fprintf(stderr,"Comm_Robot: before FD_ZERO\n");//trb
  re_xmitp = RE_XMIT;
  FD_ZERO(&lfdvar);
  FD_SET(fd, &lfdvar);
  
  timeout.tv_sec = 0;
  timeout.tv_usec = 0;
  
  //fprintf(stderr,"Comm_Robot: before while(select)\n");//trb
  while ((selectval = select(fd+1, &lfdvar, NULL, NULL, &timeout)) != 0)
  {
    /* Flush buffer */
    if (selectval != -1)
	{
	respondedp = read(fd, response, BUFSIZE);
	}
    else
	{
	respondedp = 1;
	fprintf(stderr,"Ndirect.c Comm_Robot: select error %d %d\n",selectval,errno);//trb
	}
    /* 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 ) == FALSE); i++)
      {
	sleep(5);
	fprintf(stderr, "Trying again...           ");
	fflush(stderr);
      }
      if (i == 2 && connect_robot ( Robot_id ) == FALSE)
      {
	fprintf(stderr, "Unable to reconnect to robot.  Command aborted.\n");
	return FALSE;
      }
      else
      {
	fprintf(stderr, "Successful!  Continuing with command.\n");
      }
    }
  }
  
  //fprintf(stderr,"Comm_Robot: calling Write_Pkg \n");//trb
  Write_Pkg(fd, command);
  //fprintf(stderr,"Comm_Robot: while Read_Pkg \n");//trb
  while (!(respondedp = Read_Pkg(fd, CONN_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 {
    //fprintf(stderr,"Comm_Robot: calling Process_Robot_Resp \n");//trb
    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
 * ALGORITHM:    ---
 * RETURN:       static int 
 * SIDE EFFECT:  
 * CALLS:        
 * CALLED BY:    
 ***************/
static int voltPackageProcess ( unsigned char *inbuf, 
			        unsigned char *voltCPU,
			        unsigned char *voltMotor)
{
  int i = 0;

  /* read the raw voltages out of the buffer */
  *voltCPU   = *(inbuf + i++);
  *voltMotor = *(inbuf + i++);
  
  return ( i );
}

/*****************************
 *                           *
 * Robot Interface Functions *
 *                           *