* 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 * *                           * *****************************//* * dummy function to maintain compatibility with Nclient * * create_robot - requests the server to create a robot with *                id = robot_id and establishes a connection with *                the robot. This function is disabled in this *                version of the software. *  * parameters: *    long robot_id -- id of the robot to be created. The robot *                     will be referred to by this id. If a process *                     wants to talk (connect) to a robot, it must *                     know its id. */int create_robot ( long robot_id ){  Robot_id = robot_id;  return ( TRUE );}/* Helper function for connect_robot */static char *convertAddr ( char *name, char *addr ){  int addrInt[10];  sscanf(name, "%d.%d.%d.%d", 	 &(addrInt[0]), &(addrInt[1]), &(addrInt[2]), &(addrInt[3]));  addr[0] = addrInt[0];  addr[1] = addrInt[1];  addr[2] = addrInt[2];  addr[3] = addrInt[3];  return ( addr );}int open_serial(char *port, unsigned short baud){  struct termios info;    if (Fd != -1)    close(Fd);  if ((Fd=open(port, O_RDWR|O_NONBLOCK)) < 0)    {      perror("Error opening serial port");      return 0;    }    if (tcgetattr(Fd, &info) < 0)     {      perror("Error using TCGETS in ioctl.");      close(Fd);      Fd = -1;      return 0;    }    /* restore old values to unhang the bastard, if hung */  info.c_iflag=1280;  info.c_oflag=5;  info.c_cflag=3261;  info.c_lflag=35387;    if (tcsetattr(Fd, TCSANOW, &info) < 0)     {       perror("Error using TCSETS in ioctl.");      close(Fd);      Fd = -1;      return 0;    }   close(Fd);    if ((Fd = open(port, O_RDWR)) == -1) {    perror("Error opening serial port");    errorp = SERIAL_OPEN_ERROR;    return(FALSE);  }    if (tcgetattr(Fd,&info) < 0) {    perror("Error using TCGETS in ioctl.");    errorp = SERIAL_OPEN_ERROR;    close(Fd);    Fd = -1;    return(FALSE);  }    if (baud != 4800 && baud != 9600 && baud != 19200 && baud != 38400)  {    if (baud != 0)    {      fprintf(stderr, "Invalid baud rate %d, using %d\n", baud,              DEFAULT_SERIAL_BAUD);    }    baud = DEFAULT_SERIAL_BAUD;  }    info.c_iflag = 0;  info.c_oflag = 0;  info.c_lflag = 0;  switch (baud) { /* serial port rate */  case 4800:    info.c_cflag = B4800 | CS8 | CREAD | CLOCAL;    break;  case 9600:    info.c_cflag = B9600 | CS8 | CREAD | CLOCAL;    break;  case 19200:    info.c_cflag = B19200 | CS8 | CREAD | CLOCAL;    break;  case 38400:    info.c_cflag = B38400 | CS8 | CREAD | CLOCAL;    break;  default:    break;  }  /* set time out on serial read */#if 1  info.c_cc[VMIN] = 0;  info.c_cc[VTIME] = 10;#endif   wait_time = NORMAL_TIMEOUT;  if (tcsetattr(Fd,TCSANOW,&info) < 0) {     perror("Error using TCSETS in ioctl.");    errorp = SERIAL_OPEN_ERROR;    close(Fd);    Fd = -1;    return(FALSE);  }    printf("Robot <-> Host serial communication setup\n");  printf("(%d baud using %s)\n", baud, port);  return(TRUE);}/* * connect_robot - requests the server to connect to the robot *                 with id = robot_id. In order to talk to the server, *                 the SERVER_MACHINE_NAME and SERV_TCP_PORT must be *                 set properly. If a robot with robot_id exists, *                 a connection is established with that robot. If *                 no robot exists with robot_id, no connection is *                 established. In this single robot version, the robot_id *                 is unimportant. You can call connect_robot with any *                 robot_id, it will connect to the robot. * * parameters: *    long robot_id -- robot's id. In this multiple robot version, in order *                     to connect to a robot, you must know it's id. *         model    -- robot type: 0 = Nomad 200, 1 = Nomad 150, 2 = Scout *         *dev     -- hostname for TCP, device file for serial ("/dev/" prefix *                     or ":" suffix means serial) *         conn     -- TCP port for TCP, baud rate for serial */int connect_robot(long robot_id, ...){  static char first = 1;  struct hostent *hp;  struct sockaddr_in serv_addr;  int ret, retlen, i;  unsigned char ir_mask[16],sn_mask[16],cf_mask[4],vl_mask[3];  unsigned char cp_mask,bp_mask,ls_mask,pos_mask, byte;  char addr[10];  va_list args;    if (first)  {    fprintf(stderr, "Ndirect version 2.6.13\n");    fprintf(stderr, "Copyright 1991-1998, Nomadic Technologies, Inc.\n");    first = 0;  }    va_start(args, robot_id);  model = va_arg(args, int);  device = va_arg(args, char *);  conn_value = va_arg(args, int);  if (strncmp(device, "/dev", 4) != 0 && device[strlen(device)-1] != ':')  {    connect_type = 2;  }  va_end(args);    if (connect_type == 1)   {    open_serial(device, conn_value);        /* Send init_sensors to make sure that server and robot are synchronized */    if (model == MODEL_N200)    {      init_sensors();    }    else    {      usedSmask[0] = 0;      /* IR */      for (i = 1; i < 17; i++)	usedSmask[i] = 0;      /* Sonar */      for (i = 17; i < 33; i++)	usedSmask[i] = 1;      /* Bumper */      usedSmask[33] = 1;      /* Conf */      for (i = 34; i < 38; i++)	usedSmask[i] = 1;      /* Velocity */      for (i = 38; i < 41; i++)	usedSmask[i] = 1;      /* Motor */      usedSmask[41] = 1;      /* Laser */      usedSmask[42] = 0;      /* Compass */      usedSmask[43] = 1;    }  } else {    if (device[0] == 0)      device = "localhost";    if ( ((hp = gethostbyname(device)) == NULL))    {      convertAddr(device, addr);      if (addr[0] != 0 || addr[1] != 0 || addr[2] != 0 || addr[3] != 0)      {	memset((char *) &serv_addr, 0, sizeof(serv_addr));	memcpy((char *) &(serv_addr.sin_addr), addr, 4);      }      else      {	fprintf(stderr, "Machine %s not valid.\n", device);	return FALSE;      }    }    else    {      memset((char *) &serv_addr, 0, sizeof(serv_addr));      memcpy((char *) &(serv_addr.sin_addr), hp->h_addr, hp->h_length);    }        serv_addr.sin_family = AF_INET;            /* address family */        /* TCP port number */    if (conn_value == 0)    {      conn_value = DEFAULT_ROBOT_TCP_PORT;    }    serv_addr.sin_port = htons(conn_value);        if ((Fd = socket(AF_INET, SOCK_STREAM, 0)) < 0) {      fprintf(stderr, "Error: in open_socket_to_send_data, socket failed.\n");      return FALSE;    }    fcntl(Fd, F_SETFL, O_NDELAY);    if (connect(Fd, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0) {      if (errno == EINPROGRESS) {        fd_set          lfdvar;        struct timeval  timeout;                FD_ZERO(&lfdvar);        FD_SET(Fd, &lfdvar);                timeout.tv_sec = (long)CONNECT_TIMEOUT;        timeout.tv_usec = (long)0;                if (select(Fd+1, NULL, &lfdvar, NULL, &timeout) == 0) {          fprintf(stderr, "Error: connect timed out.\n");          close(Fd);	  Fd = -1;          return FALSE;        } else {	  errno = 0;	  retlen = 4;	  if (getsockopt(Fd, SOL_SOCKET, SO_ERROR, (char *)&ret, (unsigned int *) &retlen) == 0)	  {	    if (ret != 0)	      errno = ret;	    if (errno != 0)	    {	      perror("Error: connect failed");	      close(Fd);	      Fd = -1;	      return FALSE;	    }	  }	}      } else {        perror("Error: connect failed");        close(Fd);	Fd = -1;        return FALSE;      }    }        wait_time = NORMAL_TIMEOUT;        printf("Robot <-> Host TCP/IP communication setup\n");    printf("(machine %s on port %d)\n", device, conn_value);        /* Read configuration data */    if (model == MODEL_N200)    {      byte = GETC(Fd, connect_type);      ir_mask[ 0] = byte & (1 << 0) ? 1 : 0;      ir_mask[ 1] = byte & (1 << 1) ? 1 : 0;      ir_mask[ 2] = byte & (1 << 2) ? 1 : 0;      ir_mask[ 3] = byte & (1 << 3) ? 1 : 0;      ir_mask[ 4] = byte & (1 << 4) ? 1 : 0;      ir_mask[ 5] = byte & (1 << 5) ? 1 : 0;      ir_mask[ 6] = byte & (1 << 6) ? 1 : 0;      ir_mask[ 7] = byte & (1 << 7) ? 1 : 0;      byte = GETC(Fd, connect_type);      ir_mask[ 8] = byte & (1 << 0) ? 1 : 0;      ir_mask[ 9] = byte & (1 << 1) ? 1 : 0;      ir_mask[10] = byte & (1 << 2) ? 1 : 0;      ir_mask[11] = byte & (1 << 3) ? 1 : 0;      ir_mask[12] = byte & (1 << 4) ? 1 : 0;      ir_mask[13] = byte & (1 << 5) ? 1 : 0;      ir_mask[14] = byte & (1 << 6) ? 1 : 0;      ir_mask[15] = byte & (1 << 7) ? 1 : 0;      byte = GETC(Fd, connect_type);      sn_mask[ 0] = byte & (1 << 0) ? 1 : 0;      sn_mask[ 1] = byte & (1 << 1) ? 1 : 0;      sn_mask[ 2] = byte & (1 << 2) ? 1 : 0;      sn_mask[ 3] = byte & (1 << 3) ? 1 : 0;      sn_mask[ 4] = byte & (1 << 4) ? 1 : 0;      sn_mask[ 5] = byte & (1 << 5) ? 1 : 0;      sn_mask[ 6] = byte & (1 << 6) ? 1 : 0;      sn_mask[ 7] = byte & (1 << 7) ? 1 : 0;      byte = GETC(Fd, connect_type);      sn_mask[ 8] = byte & (1 << 0) ? 1 : 0;      sn_mask[ 9] = byte & (1 << 1) ? 1 : 0;      sn_mask[10] = byte & (1 << 2) ? 1 : 0;      sn_mask[11] = byte & (1 << 3) ? 1 : 0;      sn_mask[12] = byte & (1 << 4) ? 1 : 0;      sn_mask[13] = byte & (1 << 5) ? 1 : 0;      sn_mask[14] = byte & (1 << 6) ? 1 : 0;      sn_mask[15] = byte & (1 << 7) ? 1 : 0;      byte = GETC(Fd, connect_type);      bp_mask    = byte & (1 << 0) ? 1 : 0;      cf_mask[0] = byte & (1 << 1) ? 1 : 0;      cf_mask[1] = byte & (1 << 2) ? 1 : 0;      cf_mask[2] = byte & (1 << 3) ? 1 : 0;      cf_mask[3] = byte & (1 << 4) ? 1 : 0;      vl_mask[0] = byte & (1 << 5) ? 1 : 0;      vl_mask[1] = byte & (1 << 6) ? 1 : 0;      vl_mask[2] = byte & (1 << 7) ? 1 : 0;      byte = GETC(Fd, connect_type);      cp_mask = byte & 1;      byte = GETC(Fd, connect_type);      ls_mask = byte & 1;      pos_mask = byte >> 1;            usedSmask[0] = pos_mask;      for (i = 0; i < 16; i++)	usedSmask[i+1] = ir_mask[i];      for (i = 0; i < 16; i++)	usedSmask[i+17] = sn_mask[i];      usedSmask[33] = bp_mask;      for (i = 0; i < 4; i++)	usedSmask[i+34] = cf_mask[i];      for (i = 0; i < 3; i++)	usedSmask[i+38] = vl_mask[i];      usedSmask[42] = ls_mask;      usedSmask[43] = cp_mask;            /* get laser mode, num_points, processing */      byte = GETC(Fd, connect_type);      laser_mode = byte;      byte = GETC(Fd, connect_type);      byte = GETC(Fd, connect_type);      byte = GETC(Fd, connect_type);    }    else    {      usedSmask[0] = 0;      /* IR */      for (i = 1; i < 17; i++)	usedSmask[i] = 0;      /* Sonar */      for (i = 17; i < 33; i++)	usedSmask[i] = 1;      /* Bumper */      usedSmask[33] = 1;      /* Conf */      for (i = 34; i < 38; i++)	usedSmask[i] = 1;      /* Velocity */      for (i = 38; i < 41; i++)	usedSmask[i] = 1;      /* Motor */      usedSmask[41] = 1;      /* Laser */      usedSmask[42] = 0;      /* Compass */      usedSmask[43] = 1;    }  }    return TRUE;}/* * dummy function to maintain compatibility with Nclient * * disconnect_robot - requests the server to close the connect with robot *                    with id = robot_id.  * * parameters: *    long robot_id -- robot's id. In order to disconnect a robot, you *                     must know it's id. */int disconnect_robot(long robot_id __attribute__ ((unused))){  Robot_id = -1;  return ( TRUE );}/*  * ac - sets accelerations of the robot. Currently it has no effect in  *      simulation mode. * * parameters: *    int t_ac, s_ac, r_ac -- the translation, steering, and turret *                            accelerations. t_ac is in 1/10 inch/sec^2 *                            s_ac and r_ac are in 1/10 degree/sec^2. */int ac(int t_ac, int s_ac, int r_ac){  unsigned char outbuf[BUFSIZE];    stuff_three_unsigned_int(8, AC, t_ac, s_ac, r_ac, outbuf);  return(Comm_Robot(Fd, outbuf));  }/* * sp - sets speeds of the robot, this function will not cause the robot to *      move. However, the set speed will be used when executing a pr() *      or a pa(). * * parameters: *    int t_sp, s_sp, r_sp -- the translation, steering, and turret *                            speeds. t_sp is in 1/10 inch/sec and *                            s_sp and r_sp are in 1/10 degree/sec. */int sp(int t_sp, int s_sp, int r_sp){  unsigned char outbuf[BUFSIZE];    stuff_three_unsigned_int(8, SP, t_sp, s_sp, r_sp, outbuf);  return(Comm_Robot(Fd, outbuf));}/* * pr - moves the motors of the robot by a relative distance, using the speeds *      set by sp(). The three parameters specify the relative distances for *      the three motors: translation, steering, and turret. All the three *      motors move concurrently if the speeds are not set to zero and the  *      distances to be traveled are non-zero. Depending on the timeout  *      period set (by function conf_tm(timeout)), the motion may  *      terminate before the robot has moved the specified distances * * parameters: