ndirect.c

卡耐基.梅隆大学的机器人仿真软件（Redhat linux 9下安装）

  int i, length = 0, chk_sum = 0;
  unsigned char ichar, ichar2;

  if (!(serial_ready (fd, wait_time))) {
    errorp = SERIAL_TIMEOUT_ERROR;
    return(FALSE);
  }

  errorp = 0;
  
  /* read the begin packet character, it should be 1 */
  ichar = (unsigned char)GETC(fd, conn_type);
  if (!errorp) {
    if (ichar != 1) {
      printf("start byte error: %u\n", ichar);
      errorp = SERIAL_PKG_ERROR;
    }
    else {
      chk_sum = 1;
    }
  }
  
  if (!errorp) {
    /* read length, it should be >0 */
    ichar = GETC(fd, conn_type); 
    if (!errorp) {
      chk_sum = chk_sum + (int)ichar;
    }
    ichar2 = GETC(fd, conn_type);
    if (!errorp) {
      length = two_bytes_to_unsigned_int (ichar, ichar2);
      if (length < 1) {
	printf("length byte error\n");
	errorp = SERIAL_PKG_ERROR;
      }
      else {
	chk_sum = chk_sum + (int)ichar2;
      }
    }
  }
  
  /* get the data portion of the message */
  i = 0;
  while ((!errorp) && (i<=length)) {
    ichar = GETC(fd, conn_type);
    if (!errorp) {
      inbuf[i] = ichar;
      /* printf("%u\n",  ichar); */
      chk_sum = chk_sum + (int)(ichar);
    }
    i++;
  }
  
  if (!errorp) {
    /* check chk_sum and end_pkg */
    if (((chk_sum - inbuf[length-1] - 92) % 256) != inbuf[length-1]) {
      printf("check sum error\n");
      errorp = SERIAL_PKG_ERROR;
    }
    
    if (inbuf[length] != 92) {
      printf("packet end error\n");
      errorp = SERIAL_PKG_ERROR;
    }
  }
  
  if ((errorp) && (errorp != SERIAL_TIMEOUT_ERROR)) {
    printf("emptying buffer\n");
    buf_fill(Fd, conn_type);  /* read everything else in the serial line into
		    buffer */
    bufp = bufe; /* and flush it */
  }
  
  if (errorp)
    return(FALSE);
  else
    return (TRUE);
}

/*********************************************************
 *
 *   Laser Calibration Stuff 
 *
 *********************************************************/

/* Transformation function accordingly to the calibration */
/* xi1 = pixel; yi1 = scanline */
static void ProjectPhy(double xi1, double yi1, double *x, double *y)
{
  double xi,yi;
  double den;		
  
  xi = xi1 - 254.5;
  yi = yi1 - 240.5; 
  
  den = (LASER_CALIBRATION[0]*xi + LASER_CALIBRATION[1]*yi + 1);
  
  *x = (LASER_CALIBRATION[2]*xi + LASER_CALIBRATION[3]*yi +
	LASER_CALIBRATION[4])/den + LASER_OFFSET[0];
  *y = (LASER_CALIBRATION[5]*xi + LASER_CALIBRATION[6]*yi +
	LASER_CALIBRATION[7])/den + LASER_OFFSET[1];
}  

static void convert_laser(int *laser)
{
  int i, num_points, offset, interval;
  double line_num;
  double laserx[483], lasery[483];
  
  num_points = laser[0];
  interval = NUM_LASER/(num_points-1);
  offset = 3 + (NUM_LASER-(num_points * interval))/2;
  for (i=1; i<=num_points; i++) {
    line_num = (double)(offset+(i-1)*interval);
    ProjectPhy((double)laser[i], line_num, &laserx[i], &lasery[i]); 
  }
  for (i=1; i<=num_points; i++) {
    laser[2*i-1] = (int)(laserx[i]*10.0);
    laser[2*i] = (int)(lasery[i]*10.0);
  }
  return;
}

/*********************************************************
 *
 *   Processing different types of packages received from the robot 
 *
 *********************************************************/

/* process the response received from the robot which
   encodes the state of the robot according to the mask */

static void Process_State_Pkg(unsigned char inbuf[BUFSIZE])
{
  int i, byte_count = 1;
  int low_half_used = FALSE;
  
  /* infrared */
  for (i = STATE_IR_0 ; i <= STATE_IR_15; i++)
    if (usedSmask[i] > 0) 
      {
	if (low_half_used == FALSE)
	  {
	    State[i] = low_half(inbuf[byte_count]);
	    low_half_used = TRUE;
	  }
	else
	  {
	    State[i] = high_half(inbuf[byte_count]);
	    byte_count++;
	    low_half_used = FALSE;
	  }
      }
  if (low_half_used == TRUE)
    byte_count++;
  
  /*
   * if the pos attachment was required we read it
   */
  
  if (POS_INFRARED_PI(usedSmask[SMASK_POS_DATA]))
    for (i = 0; i < INFRAREDS; i++)
      if (usedSmask[SMASK_IR_1 + i] > 0) 
	byte_count += posPackageProcess(&inbuf[byte_count], 
					&(posDataAll.infrared[i]));
  
  /* sonars */
  for (i = STATE_SONAR_0; i <= STATE_SONAR_15; i++) {
    if ( usedSmask[i] > 0 ) 
      {
	State[i] = inbuf[byte_count];
	byte_count++;
      }
  }
  
  /*
   * if the pos attachment was required we read it
   */
  
  if (POS_SONAR_PI(usedSmask[SMASK_POS_DATA]))
    for (i = 0; i < SONARS; i++) 
      if (usedSmask[SMASK_SONAR_1 + i] > 0)
	byte_count += posPackageProcess(&inbuf[byte_count], 
					&(posDataAll.sonar[i]));

  if (usedSmask[ SMASK_BUMPER ] > 0)
  {
    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 = 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));
  }
  
  /* the position data */
  
  if (usedSmask[SMASK_CONF_X] > 0)
  {
    State[STATE_CONF_X] =  two_bytes_to_signed_int(inbuf[byte_count],
						   inbuf[byte_count+1]);
    byte_count = byte_count + 2;
  }
  
  if (usedSmask[SMASK_CONF_Y] > 0)
  {
    State[STATE_CONF_Y] = two_bytes_to_signed_int(inbuf[byte_count],
						  inbuf[byte_count+1]);
    byte_count = byte_count + 2;
  }
  
  if (usedSmask[SMASK_CONF_STEER] > 0)
  {
    State[STATE_CONF_STEER] = two_bytes_to_signed_int(inbuf[byte_count], 
						      inbuf[byte_count+1]);
    byte_count = byte_count + 2;
  }
  
  if (usedSmask[SMASK_CONF_TURRET] > 0)
  {
    State[STATE_CONF_TURRET] = two_bytes_to_signed_int(inbuf[byte_count], 
						       inbuf[byte_count+1]);
    byte_count = byte_count + 2;
  }
  
  /* the velocities */
  
  if (usedSmask[SMASK_VEL_TRANS] > 0)
  {
    State[STATE_VEL_TRANS] = two_bytes_to_signed_int(inbuf[byte_count], 
						     inbuf[byte_count+1]);
    byte_count = byte_count + 2;
  }
  
  if (usedSmask[SMASK_VEL_STEER] > 0)
  {
    State[SMASK_VEL_STEER] = two_bytes_to_signed_int(inbuf[byte_count],
						     inbuf[byte_count+1]);
    byte_count = byte_count + 2;
  }
  
  if (usedSmask[SMASK_VEL_TURRET] > 0)
  {
    State[STATE_VEL_TURRET] = two_bytes_to_signed_int(inbuf[byte_count],
						      inbuf[byte_count+1]);
    byte_count = byte_count + 2;
  }
  
  /* the compass value */

  if (usedSmask[SMASK_COMPASS] > 0)
  {
    State[STATE_COMPASS] = 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 compass
     * we have to unpack the package. 
     */
    
    if (POS_COMPASS_PI(usedSmask[SMASK_POS_DATA]))
      byte_count += posPackageProcess(&inbuf[byte_count], 
				      &(posDataAll.compass));
  }

  /* laser */
  if (usedSmask[SMASK_LASER] > 0)
  {
    /* the number of points */
    Laser[0] = two_bytes_to_unsigned_int(inbuf[byte_count],
					 inbuf[byte_count+1]);
    byte_count = byte_count + 2;
    
    /* check the laser mode */
    if ((laser_mode&0x1e) == 0) /* Line mode */
    {
      if (Laser[0] > NUM_LASER/2)
      {
	printf("error in processing laser reply (1).\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;
      }
    }
    else /* Points of some kind */
    {
      if (Laser[0] > NUM_LASER) 
      {
	printf("error in processing laser reply (2).\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&0x1e) == 19)
      convert_laser(Laser);
    
    /*
     * if the position attachment was requested for the laser
     * we have to get it from somewhere else 
     */
    
    if (POS_LASER_PI(usedSmask[SMASK_POS_DATA]))
      byte_count += posPackageProcess(&inbuf[byte_count], 
				      &(posDataAll.laser));
  }
  /* motor active */
  State[STATE_MOTOR_STATUS] = (long)inbuf[byte_count++];  

  /* process the 6811 time */
  byte_count += timePackageProcess(&inbuf[byte_count], &posDataTime);

  /* process the voltages of motor/CPU */
  byte_count += voltPackageProcess(&inbuf[byte_count], 
				   &voltageCPU, &voltageMotor);
}

/* process the response from the robot which encodes the
   active infrared reading */

static void Process_Infrared_Pkg(unsigned char inbuf[BUFSIZE])
{
  int i, byte_count = 1;
  int low_half_used = FALSE;

  /* 
   * the ir datum from one sensor is only a nibble, 
   * two of them are merged into one byte 
   */
  
  for (i = STATE_IR_0 ; i <=  STATE_IR_15; i++)
    if (low_half_used == FALSE) 
      {
	State[i] = low_half(inbuf[byte_count]);
	low_half_used = TRUE;
      }
    else 
      {
	State[i] = high_half(inbuf[byte_count]);
	byte_count++;
	low_half_used = FALSE;
      }

  /* align with next byte */
  if ( low_half_used )
    byte_count++;

  /*
   * if the pos attachment was required we read it
   */

  if ( POS_INFRARED_PI ( usedSmask[ SMASK_POS_DATA ] ) )
  {
    for (i=0; i<16; i++)
      byte_count += posPackageProcess ( &inbuf[byte_count], 
				        &( posDataAll.infrared[i] ) );
  }

  /* extract the time data for the 6811 */
  byte_count += timePackageProcess ( &inbuf[byte_count], &posDataTime );
}

/* process the response from the robot which encodes the
   active sonar reading */

static void Process_Sonar_Pkg(unsigned char inbuf[BUFSIZE])
{
  int i, byte_count = 1;

  /*
   * read the sensory data from the buffer
   */

  for (i = STATE_SONAR_0; i <= STATE_SONAR_15; i++) 
    {
      State[i] = inbuf[byte_count];
      byte_count++;
    }

  /*
   * if the pos attachment was required we read it
   */

  if ( POS_SONAR_PI ( usedSmask[ SMASK_POS_DATA ]) )
    for (i=0; i<16; i++) 
      byte_count += posPackageProcess ( &inbuf[byte_count], 
				        &( posDataAll.sonar[i] ) );
    
  /* extract the time data for the 6811 */
  byte_count += timePackageProcess ( &inbuf[byte_count], &posDataTime );
}

/* process the response from the robot which encodes the
   configuration of the robot */

static void Process_Configuration_Pkg(unsigned char inbuf[BUFSIZE])
{
  int byte_count = 1;
  
  State[ STATE_CONF_X ] = two_bytes_to_signed_int(inbuf[byte_count],
						  inbuf[byte_count+1]);
  byte_count = byte_count + 2;
  
  State[ STATE_CONF_Y ] = two_bytes_to_signed_int(inbuf[byte_count],
						  inbuf[byte_count+1]);
  byte_count = byte_count + 2;
  
  State[ STATE_CONF_STEER ] = two_bytes_to_signed_int(inbuf[byte_count],
						      inbuf[byte_count+1]);
  byte_count = byte_count + 2;
  
  State[ STATE_CONF_TURRET ] = two_bytes_to_signed_int(inbuf[byte_count],
						       inbuf[byte_count+1]);
}

static void Process_Velocity_Pkg(unsigned char inbuf[BUFSIZE])
{
  int byte_count = 1;
  
  State[ STATE_VEL_TRANS ] = two_bytes_to_signed_int(inbuf[byte_count],
						     inbuf[byte_count+1]);
  byte_count = byte_count + 2;
  
  State[ STATE_VEL_STEER ] = two_bytes_to_signed_int(inbuf[byte_count],
						     inbuf[byte_count+1]);
  byte_count = byte_count + 2;
  
  State[ STATE_VEL_TURRET ] = two_bytes_to_signed_int(inbuf[byte_count],
						      inbuf[byte_count+1]);
}

static void Process_Acceleration_Pkg(unsigned char inbuf[BUFSIZE] __attribute__ ((unused)))
{
}

/* process the response from the robot which encodes the
   compass reading of the robot */

static void Process_Compass_Pkg(unsigned char inbuf[BUFSIZE])
{
  int byte_count = 1;
  
  State[ STATE_COMPASS ] = two_bytes_to_unsigned_int(inbuf[byte_count],
						     inbuf[byte_count+1]);
  byte_count +=2;

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

  if ( POS_COMPASS_PI ( usedSmask[ SMASK_POS_DATA ] ) )
    byte_count += posPackageProcess ( &inbuf[byte_count], 
  				      &( posDataAll.compass ) );