ndirect.c

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

  /* the length into the packet (# of ints * sizeof(int)) */
  unsigned_int_to_two_bytes ( 6 * 2 + 2, ptr ); ptr += 2;

  /* the packet type */
  *(ptr++) = MV;

  /* translational axis - mode and value */
  unsigned_int_to_two_bytes ( t_mode , ptr ); ptr += 2;
  signed_int_to_two_bytes   ( t_mv   , ptr ); ptr += 2;

  /* steering axis - mode and value */
  unsigned_int_to_two_bytes ( s_mode , ptr ); ptr += 2;
  signed_int_to_two_bytes   ( s_mv   , ptr ); ptr += 2;

  /* turret  axis - mode and value */
  unsigned_int_to_two_bytes ( r_mode , ptr ); ptr += 2;
  signed_int_to_two_bytes   ( r_mv   , ptr ); ptr += 2;

  return ( Comm_Robot(Fd, outbuf) );
}

/*
 * ct - send the sensor mask, Smask, to the robot. You must first change
 *      the global variable Smask to the desired communication mask before
 *      calling this function. 
 *
 *      to avoid inconsistencies usedSmask is used in all other function.
 *      once ct is called the user accessible mask Smask is used to 
 *      redefine usedSmask. This avoids that a returning package is encoded
 *      with a different mask than the one it was sent with, in case
 *      the mask has been changed on the client side, but has not been 
 *      updated on the server side.
 */
int ct()
{
  int i;
  unsigned char b0, b1, b2, b3, b4, b5, b6;
  unsigned char outbuf[BUFSIZE], *byte_ptr;

  for ( i = 0; i < NUM_MASK; i++ )
    usedSmask[i] = Smask[i];
  
#if NOMAD150
#else /* rich */
  /* first encode Mask */
  b0 = bits_to_byte (Smask[1], Smask[2], Smask[3], Smask[4],
		     Smask[5], Smask[6], Smask[7], Smask[8]);
  b1 = bits_to_byte (Smask[9], Smask[10], Smask[11], Smask[12],
		     Smask[13], Smask[14], Smask[15], Smask[16]);
  b2 = bits_to_byte (Smask[17], Smask[18], Smask[19], Smask[20],
		     Smask[21], Smask[22], Smask[23], Smask[24]);
  b3 = bits_to_byte (Smask[25], Smask[26], Smask[27], Smask[28],
		     Smask[29], Smask[30], Smask[31], Smask[32]);
  b4 = bits_to_byte (Smask[33], Smask[34], Smask[35], Smask[36],
		     Smask[37], Smask[38], Smask[39], Smask[30]);
  b5 = bits_to_byte (Smask[42], 0, 0, 0, 0, 0, 0, 0);
  /* we pack the pos mask into b6 */
  b6 = bits_to_byte(Smask[43], 
		    POS_INFRARED_PI(Smask[ SMASK_POS_DATA ]),
		    POS_SONAR_PI   (Smask[ SMASK_POS_DATA ]), 
		    POS_BUMPER_PI  (Smask[ SMASK_POS_DATA ]), 
		    POS_LASER_PI   (Smask[ SMASK_POS_DATA ]), 
		    POS_COMPASS_PI (Smask[ SMASK_POS_DATA ]),
		    0,0);
  
  stuff_length_type (9, CT, outbuf);
  byte_ptr = outbuf + 4;
  *byte_ptr = b0;
  byte_ptr++;
  *byte_ptr = b1;
  byte_ptr++;
  *byte_ptr = b2;
  byte_ptr++;
  *byte_ptr = b3;
  byte_ptr++;
  *byte_ptr = b4;
  byte_ptr++;
  *byte_ptr = b5;
  byte_ptr++;
  *byte_ptr = b6;
  return(Comm_Robot(Fd, outbuf));
#endif
}

/*
 * gs - get the current state of the robot according to the mask (of 
 *      the communication channel)
 */
int gs()
{ 
  unsigned char outbuf[BUFSIZE];
  int retval = 0;
  
  //fprintf(stderr,"gs: calling stuff_length_type\n");//trb
  stuff_length_type (2, GS, outbuf);
  //fprintf(stderr,"gs: calling Comm_Robot\n");//trb
  retval = Comm_Robot(Fd, outbuf);
  //fprintf(stderr,"gs: returning\n");//trb
  return(retval);
}

/*
 * st - stops the robot (the robot holds its current position)
 */
int st()
{
  unsigned char outbuf[BUFSIZE];
  
  stuff_length_type (2, ST, outbuf);
  return(Comm_Robot(Fd, outbuf));
}

/*
 * lp - set motor limp (the robot may not hold its position).
 */
int lp ()
{
  unsigned char outbuf[BUFSIZE];
  
  stuff_length_type (2, LP, outbuf);
  return(Comm_Robot(Fd, outbuf));
}

/*
 * tk - sends the character stream, talk_string, to the voice synthesizer
 *      to make the robot talk.
 *
 * parameters:
 *    char *talk_string -- the string to be sent to the synthesizer.
 */
int tk(char *talk_string)
{
  unsigned char outbuf[BUFSIZE], *byte_ptr;
  int i, length;
  
  length = 3 + strlen(talk_string);
  stuff_length_type (length, TK, outbuf);
  byte_ptr = outbuf + 4;
  for (i=3; i<length; i++) {
    *byte_ptr = talk_string[i-3];
    byte_ptr++;
  }
  *byte_ptr = 0; /* null terminate the string */
  return(Comm_Robot(Fd, outbuf));
}

/*
 * dp - define the current position of the robot as (x,y)
 * 
 * parameters:
 *    int x, y -- the position to set the robot to.
 */
int dp(int x, int y)
{
  unsigned char outbuf[BUFSIZE];
  
  stuff_two_signed_int (6, DP, x, y, outbuf);
  return(Comm_Robot(Fd, outbuf));
}

/*
 * zr - zeroing the robot, align steering and turret with bumper zero.
 *      The position, steering and turret angles are all set to zero.
 *      This function returns when the zeroing process has completed.
 */
int zr()
{
  unsigned char outbuf[BUFSIZE];
  int temp;
  
  wait_time =  120;   /* maximum of 2 minutes */
  /* zr() takes maximum of 120 seconds */
  stuff_length_type (2, ZR, outbuf);
  temp = Comm_Robot(Fd, outbuf);
  wait_time = NORMAL_TIMEOUT;
  return(temp);
}

/*
 * conf_ir - configure infrared sensor system.
 *
 * parameters: 
 *    int history -- specifies the percentage dependency of the current 
 *                   returned reading on the previous returned reading.
 *                   It should be set between 0 and 10: 0 = no dependency 
 *                   10 = full dependency, i.e. the reading will not change
 *    int order[16] --  specifies the firing sequence of the infrared 
 *                      (#0 .. #15). You can terminate the order list by a 
 *                      "255". For example, if you want to use only the 
 *                      front three infrared sensors then set order[0]=0,
 *                      order[1]=1, order[2]=15, order[3]=255 (terminator).
 */
int conf_ir(int history, int order[16])
{
  unsigned char outbuf[BUFSIZE], *byte_ptr;
  int i;
  
  stuff_length_type (19, CONF_IR, outbuf);
  byte_ptr = outbuf + 4;
  if (history > 10)
    history = 10;
  *byte_ptr = (unsigned char)history;
  for (i=0; i<16; i++) {
    byte_ptr++;
    *byte_ptr = (unsigned char)order[i];
  }
  return(Comm_Robot(Fd, outbuf));
}

/*
 * conf_sn - configure sonar sensor system.
 *
 * parameters:
 *    int rate -- specifies the firing rate of the sonar in 4 milli-seconds 
 *                interval; 
 *    int order[16] -- specifies the firing sequence of the sonar (#0 .. #15).
 *                     You can terminate the order list by a "255". For 
 *                     example, if you want to use only the front three 
 *                     sensors, then set order[0]=0, order[1]=1, order[2]=15, 
 *                     order[3]=255 (terminator).
 */
int conf_sn(int rate, int order[16])
{
  unsigned char outbuf[BUFSIZE], *byte_ptr;
  int i;
  
  stuff_length_type (19, CONF_SN, outbuf);
    byte_ptr = outbuf + 4;
    *byte_ptr = (unsigned char)rate;
  for (i=0; i<16; i++) {
    byte_ptr++;
    *byte_ptr = (unsigned char)order[i];
  }
  return(Comm_Robot(Fd, outbuf));
}

/*
 * conf_cp - configure compass system.
 * 
 * parameters:
 *    int mode -- specifies compass on/off: 0 = off ; 1 = on; 2 = calibrate.
 *                When you call conf_cp (2), the robot will rotate slowly 360
 *                degrees. You must wake till the robot stop rotating before
 *                issuing another command to the robot (takes ~3 minutes).
 */
int conf_cp(int mode)
{
  unsigned char outbuf[BUFSIZE], *byte_ptr;
  
  stuff_length_type (3, CONF_CP, outbuf);
  byte_ptr = outbuf + 4;
  *byte_ptr = (unsigned char)mode;
  return(Comm_Robot(Fd, outbuf));
}

/*
 * conf_ls - configure laser sensor system:
 *
 * parameters:
 *    unsigned int mode -- specifies the on-board processing mode of the laser 
 *                         sensor data which determines the mode of the data 
 *                         coming back: 
 *                           the first bit specifies the on/off;
 *                           the second bit specifies point/line mode;
 *                           the third to fifth bits specify the 
 *                           returned data types: 
 *                             000 = peak pixel, 
 *                             001 = rise pixel, 
 *                             010 = fall pixel, 
 *                             011 = magnitude,
 *                             100 = distance;
 *                           the sixth bit specifies data integrity checking.
 *
 *   unsigned int threshold -- specifies the inverted acceptable brightness
 *                             of the laser line. 
 *
 *   unsigned int width -- specifies the acceptable width in terms
 *                         of number of pixels that are brighter than the 
 *                         set threshold.
 *  
 *   unsigned int num_data -- specifies the number of sampling points. 
 *   unsigned int processing --  specifies the number of neighboring 
 *                               pixels for averaging
 *
 * If you don't understand the above, try this one:
 *   conf_ls(51, 20, 20, 20, 4)
 */
int conf_ls(unsigned int mode, unsigned int threshold, unsigned int width,
	    unsigned int num_data, unsigned int processing)
{
  unsigned char outbuf[BUFSIZE], *byte_ptr;
  
  laser_mode = mode;
  stuff_length_type (8, CONF_LS, outbuf);
  byte_ptr = outbuf + 4;
  if (mode == 51) 
    *byte_ptr = 35; /* special case */
  else
    *byte_ptr = (unsigned char)mode;
  byte_ptr++;
  *byte_ptr = (unsigned char)threshold;
  byte_ptr++;
  *byte_ptr = (unsigned char)width;
  byte_ptr++;
  unsigned_int_to_two_bytes(num_data, byte_ptr);
  byte_ptr = byte_ptr + 2;
  *byte_ptr = (unsigned char)processing;
  return(Comm_Robot(Fd, outbuf));
}

/*
 * conf_tm - sets the timeout period of the robot in seconds. If the
 *           robot has not received a command from the host computer
 *           for more than the timeout period, it will abort its 
 *           current motion
 * 
 * parameters:
 *    unsigned int timeout -- timeout period in seconds. If it is 0, there
 *                            will be no timeout on the robot.
 */
int conf_tm(unsigned char timeout)
{
  unsigned char outbuf[BUFSIZE], *byte_ptr;
  
  stuff_length_type (3, CONF_TM, outbuf);
  byte_ptr = outbuf + 4;
  *byte_ptr = (unsigned char)timeout;
  return(Comm_Robot(Fd, outbuf));
}

#if NOMAD150

int conf_ser(unsigned char port, unsigned short baud)
{
  unsigned char *outbuf, buf[BUFSIZE];
  
  outbuf=buf;
  stuff_length_type(5, CONF_SER, outbuf);
  outbuf+=4;
  *outbuf=port;
  outbuf++;
  unsigned_int_to_two_bytes(baud, outbuf);
  return(Comm_Robot(Fd, buf));
}

unsigned short poll_serial(unsigned char port)
{
  unsigned short bauds[]={9600, 19200, 38400, 0};
  int old_wait_time, i, result;

  old_wait_time=wait_time;
  wait_time=1;
  
  for (i=0, result=FALSE; (bauds[i] != 0) && (result == FALSE); i++)
    {
      open_serial(port, bauds[i]);
      result=gs();
    }
  
  wait_time=old_wait_time;
  printf("baud %d\n", bauds[i]);

  return bauds[i];
}

  
int conf_serial(unsigned char port, unsigned short baud)
{
  conf_ser(0, baud);
  open_serial(port, baud);
} 

#endif

/*
 * get_ir - get infrared data, independent of mask. However, only 
 *          the active infrared sensor readings are valid. It updates
 *          the State vector.
 */
int get_ir()
{
  unsigned char outbuf[BUFSIZE];
  
  stuff_length_type (2, GET_IR, outbuf);
  return(Comm_Robot(Fd, outbuf));
}

/*
 * get_sn - get sonar data, independent of mask. However, only 
 *          the active sonar sensor readings are valid. It updates
 *          the State vector.
 */
int get_sn()
{
  unsigned char outbuf[BUFSIZE];
  
  stuff_length_type (2, GET_SN, outbuf);
  return(Comm_Robot(Fd, outbuf));
}

/*
 * get_rc - get robot configuration data (x, y, th, tu), independent of 
 *          mask. It updates the State vector.
 */
int get_rc() 
{
  unsigned char outbuf[BUFSIZE];
  
  stuff_length_type (2, GET_RC, outbuf);
  return(Comm_Robot(Fd, outbuf));
}

/*
 * get_rv - get robot velocities (translation, steering, and turret) data,
 *          independent of mask. It updates the State vector.
 */
int get_rv() 
{
  unsigned char outbuf[BUFSIZE];
  
  stuff_length_type (2, GET_RV, outbuf);
  return(Comm_Robot(Fd, outbuf));
}

/*
 * get_ra - get robot acceleration (translation, steering, and turret) data,
 *          independent of mask. It updates the State vector.
 */
int get_ra() 
{
  unsigned char outbuf[BUFSIZE];
  
  stuff_length_type (2, GET_RA, outbuf);
  return(Comm_Robot(Fd, outbuf));
}

/*
 * get_cp - get compass data, independent of mask. However, the
 *          data is valid only if the compass is on. It updates the
 *          State vector.
 */
int get_cp()
{
  unsigned char outbuf[BUFSIZE];
  
  stuff_length_type (2, GET_CP, outbuf);
  return(Comm_Robot(Fd, outbuf));
}

/*
 * get_ls - get laser data point mode, independent of mask. However the
 *          data is valid only of the laser is on. It updates the Laser 
 *          vector.
 */
int get_ls()
{
  unsigned char outbuf[BUFSIZE];
  
  stuff_length_type (2, GET_LS, outbuf);
  return(Comm_Robot(Fd, outbuf));
}

/*
 * get_bp - get bumper data, independent of mask. It updates the State
 *          vector.
 */
int get_bp()
{
  unsigned char outbuf[BUFSIZE];
  
  stuff_length_type (2, GET_BP, outbuf);
  return(Comm_Robot(Fd, outbuf));
}

/*
 * conf_sg - configure laser sensor system line segment processing mode:
 *
 * parameters:
 *    unsigned int threshold -- specifies the threshold value for least-square
 *                             fitting. When the er