*    int t_pr, s_pr, r_pr -- the specified relative distances of the *                            translation, steering, and turret motors. *                            t_pr is in 1/10 inch and s_pr and r_pr are *                            in 1/10 degrees. */int pr(int t_pr, int s_pr, int r_pr){  unsigned char outbuf[BUFSIZE];    stuff_three_signed_int(8, PR, t_pr, s_pr, r_pr, outbuf);  return(Comm_Robot(Fd, outbuf));}/* * pa - moves the motors of the robot to the specified absolute positions  *      using the speeds set by sp().  Depending on the timeout period set  *      (by conf_tm()), the motion may terminate before the robot has  *      moved to the specified positions. * * parameters: *    int t_pa, s_pa, r_pa -- the specified absolute positions of the *                            translation, steering, and turret motors. *                            t_pa is in 1/10 inch and s_pa and r_pa are *                            in 1/10 degrees. */int pa(int t_pa, int s_pa, int r_pa);int pa(int t_pa, int s_pa, int r_pa){  unsigned char outbuf[BUFSIZE];    if (model != MODEL_N200)  {    return FALSE;  }    stuff_three_signed_int(8, PA, t_pa, s_pa, r_pa, outbuf);  return(Comm_Robot(Fd, outbuf));}/* * vm - velocity mode, command the robot to move at translational *      velocity = tv, steering velocity = sv, and rotational velocity = *      rv. The robot will continue to move at these velocities until *      either it receives another command or this command has been *      timeout (in which case it will stop its motion). * * parameters:  *    int t_vm, s_vm, r_vm -- the desired translation, steering, and turret *                            velocities. tv is in 1/10 inch/sec and *                            sv and rv are in 1/10 degree/sec. */int vm(int t_v, int s_v, int r_v){  unsigned char outbuf[BUFSIZE];    stuff_three_signed_int(8, VM, t_v, s_v, r_v, outbuf);  return(Comm_Robot(Fd, outbuf));}/* * mv - move, send a generalized motion command to the robot. *      For each of the three axis (translation, steering, and *      turret) a motion mode (t_mode, s_mode, r_mode) can be  *      specified (using the values MV_IGNORE, MV_AC, MV_SP, *      MV_LP, MV_VM, and MV_PR defined above): * *         MV_IGNORE : the argument for this axis is ignored *                     and the axis's motion will remain  *                     unchanged. *         MV_AC :     the argument for this axis specifies *                     an acceleration value that will be used *                     during motion commands. *         MV_SP :     the argument for this axis specifies *                     a speed value that will be used during *                     position relative (PR) commands. *         MV_LP :     the arguemnt for this axis is ignored *                     but the motor is turned off. *         MV_VM :     the argument for this axis specifies *                     a velocity and the axis will be moved *                     with this velocity until a new motion *                     command is issued (vm,pr,mv) or  *                     recieves a timeout. *         MV_PR :     the argument for this axis specifies *                     a position and the axis will be moved *                     to this position, unless this command *                     is overwritten by another (vm,pr,mv). * * parameters:  *    int t_mode - the desired mode for the tranlation axis *    int t_mv   - the value for that axis, velocity or position, *                 depending on t_mode *    int s_mode - the desired mode for the steering axis *    int s_mv   - the value for that axis, velocity or position, *                 depending on t_mode *    int r_mode - the desired mode for the turret axis *    int r_mv   - the value for that axis, velocity or position, *                 depending on t_mode */int mv(int t_mode, int t_mv, int s_mode,        int s_mv, int r_mode, int r_mv) {  unsigned char outbuf[BUFSIZE];  unsigned char *ptr;  ptr = outbuf;    /* skip the first byte for begin package */  ptr++;  /* 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;  if (model != MODEL_N200)  {    return FALSE;  }    for ( i = 0; i < NUM_MASK; i++ )    usedSmask[i] = Smask[i];    /* 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));}/* * gs - get the current state of the robot according to the mask (of  *      the communication channel) */int gs(){  unsigned char outbuf[BUFSIZE];    stuff_length_type (2, GS, outbuf);  return(Comm_Robot(Fd, outbuf));}/* * 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 tkfd, i, length;    if (model == MODEL_N200)  {    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));  }  else  {    tkfd = open("/dev/dbtk", O_RDWR);    if (tkfd >= 0)    {      write(tkfd, talk_string, strlen(talk_string));      write(tkfd, "\n", 1);      close(tkfd);            return TRUE;    }  }    return FALSE;}/* * 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 =  NORMAL_TIMEOUT;  /* zr() takes maximum of 120 seconds */  stuff_length_type (2, ZR, outbuf);  temp = Comm_Robot(Fd, outbuf);  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;    if (model != MODEL_N200)  {    return FALSE;  }    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;    if (model != MODEL_N200)  {    return FALSE;  }    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;    if (model != MODEL_N200)  {    return FALSE;  }    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));}int conf_ser(unsigned char port, unsigned short baud){  unsigned char *outbuf, buffer[BUFSIZE];    if (model == MODEL_N200)  {    return FALSE;  }    outbuf=buffer;  stuff_length_type(5, CONF_SER, outbuf);  outbuf+=4;  *outbuf=port;  outbuf++;  unsigned_int_to_two_byt