nclient.c

机器人仿真软件

/*
 * 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])
{
  int i;
  
  the_request.type = CONF_SN_MSG;
  the_request.size = 17;
  the_request.mesg[0] = rate;
  for (i=0; i<16; i++)
  {
    the_request.mesg[i+1] = order[i];
  }
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_state_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*
 * 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)
{
  the_request.type = CONF_CP_MSG;
  the_request.mesg[0] = mode;
  the_request.size = 1;
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_state_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*
 * 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)
{
  the_request.type = CONF_LS_MSG;
  the_request.size = 5;
  the_request.mesg[0] = mode;
  the_request.mesg[1] = threshold;
  the_request.mesg[2] = width;
  the_request.mesg[3] = num_data;
  the_request.mesg[4] = processing;
  
  if (ipc_comm(&the_request, &the_reply))
  {
    laser_mode = mode;
    return(process_state_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*
 * 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)
{
  the_request.type = CONF_TM_MSG;
  the_request.size = 1;
  the_request.mesg[0] = timeout;
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_state_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*
 * 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(void)
{
  the_request.type = GET_IR_MSG;
  the_request.size = 0;
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_infrared_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*
 * 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(void)
{
  the_request.type = GET_SN_MSG;
  the_request.size = 0;
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_sonar_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*
 * get_rc - get robot configuration data (x, y, th, tu), independent of 
 *          mask. It updates the State vector.
 */
int get_rc(void)
{
  the_request.type = GET_RC_MSG;
  the_request.size = 0;
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_configuration_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*
 * get_rv - get robot velocities (translation, steering, and turret) data,
 *          independent of mask. It updates the State vector.
 */
int get_rv(void)
{
  the_request.type = GET_RV_MSG;
  the_request.size = 0;
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_velocity_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*
 * get_ra - get robot acceleration (translation, steering, and turret) data,
 *          independent of mask. It updates the State vector.
 */
int get_ra(void)
{
  the_request.type = GET_RA_MSG;
  the_request.size = 0;
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_velocity_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*
 * 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(void)
{
  the_request.type = GET_CP_MSG;
  the_request.size = 0;
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_compass_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*
 * 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(void)
{
  int temp_laser_mode;
  
  the_request.type = GET_LS_MSG;
  the_request.size = 0;
  
  temp_laser_mode = laser_mode;
  if ((laser_mode == 33) || (laser_mode == 1))
    laser_mode = 51;
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_laser_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
  
  laser_mode = temp_laser_mode;
}

/* ##### setup_ls() is obsolete */

/*
 * get_bp - get bumper data, independent of mask. It updates the State
 *          vector.
 */
int get_bp(void)
{
  the_request.type = GET_BP_MSG;
  the_request.size = 0;
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_bumper_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*
 * conf_sg - configure laser sensor system line segment processing mode:
 *
 * parameters:
 *    unsigned int threshold -- specifies the threshold value for least-square
 *                             fitting. When the error term grows above the 
 *                             threshold, the line segment will be broken
 *    unsigned int min_points -- specifies the acceptable number of points
 *                              to form a line segment.
 *    unsigned int gap -- specifies the acceptable "gap" between two segments
 *                        while they can still be treated as one (in 1/10 inch)
 *
 * If you don't understand the above, try this one:
 *    conf_sg(50, 4, 30)
 */
int conf_sg(unsigned int threshold, unsigned int min_points, unsigned int gap)
{
  the_request.type = CONF_SG_MSG;
  the_request.size = 3;
  the_request.mesg[0] = threshold;
  the_request.mesg[1] = min_points;
  the_request.mesg[2] = gap;
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_state_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*
 * get_sg - get laser data line mode, independent of mask. It updates
 *          the laser vector.
 */
int get_sg(void)
{
  int temp_laser_mode;
  
  the_request.type = GET_SG_MSG;
  the_request.size = 0;
  
  temp_laser_mode = laser_mode;
  laser_mode = 33;
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_laser_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
  
  laser_mode = temp_laser_mode;
}

/*
 * da - define the current steering angle of the robot to be th
 *      and the current turret angle of the robot to be tu.
 * 
 * parameters:
 *    int th, tu -- the steering and turret orientations to set the
 *                  robot to.
 */
int da(int th, int tu)
{
  the_request.type = DA_MSG;
  the_request.size = 2;
  the_request.mesg[0] = th;
  the_request.mesg[1] = tu;
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_state_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*
 * ws - waits for stop of motors of the robot. This function is intended  
 *      to be used in conjunction with pr() and pa() to detect the desired
 *      motion has finished
 *
 * parameters:
 *    unsigned char t_ws, s_ws, r_ws -- These three parameters specify 
 *                                      which axis or combination of axis 
 *                                      (translation, steering, and turret) 
 *                                      to wait. 
 *    unsigned char timeout -- specifies how long to wait before timing out 
 *                             (return without stopping the robot).
 */
int ws(unsigned char t_ws, unsigned char s_ws,
       unsigned char r_ws, unsigned char timeout)
{
  the_request.type = WS_MSG;
  the_request.size = 4;
  the_request.mesg[0] = t_ws;
  the_request.mesg[1] = s_ws;
  the_request.mesg[2] = r_ws;
  the_request.mesg[3] = timeout;
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_state_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*
 * get_rpx - get the position of all nearby robots
 */
int get_rpx(long *robot_pos)
{
  the_request.type = GET_RPX_MSG;
  the_request.size = 0;
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_rpx_reply(&the_reply, robot_pos));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*****************************
 *                           *
 * World Interface Functions *
 *                           *
 *****************************/

/*
 * add_obstacle - creates an obstacle and adds it to the obstacle list
 *                of the robot environment. 
 * 
 * parameters:
 *    long obs[2*MAX_VERTICES+1] -- 
 *                The first element of obs specifies the number of 
 *                vertices of the polygonal obstacle (must be no greater 
 *                than MAX_VERTICES). The subsequent elements of obs 
 *                specifies the x and y coordinates of the vertices, 
 *                in counter-clockwise direction.
 */
int add_obstacle(long obs[2*MAX_VERTICES+1])
{
  int i;
  
  the_request.type = ADDOBS_MSG;
  the_request.size = obs[0]*2+1;
  for (i=0; i<obs[0]*2+1; i++)
  {
    the_request.mesg[i] = obs[i];
  }
  
  if (ipc_comm(&the_request, &the_reply))
  {
    return(process_simple_reply(&the_reply));
  }
  else
  {
    State[ STATE_ERROR ] = IPC_ERROR;  /* indicate IPC_ERROR */
    return(FALSE);
  }
}

/*
 * add_Obs - is the same as add_obstacle, for backward compatibility
 */
int add_Obs(long obs[2*MAX_VERTICES+1])
{
  return(add_obstacle(obs));
}

/*