rflex_lib.c

卡内基梅隆大学（CMU）开发的移动机器人控制开发软件包。可对多种机器人进行控制

carmen_rflex_update_bumpers(int num_bumpers, char *values)
{
  clear_incoming_data();

  if (num_bumpers > status.num_bumpers) 
    {
      carmen_warn("Requested more bumpers than available.\n");
      num_bumpers = status.num_bumpers;
    }

  memcpy(values, status.bumpers, num_bumpers*sizeof(char));
}

void 
carmen_rflex_initialize(int trans_acceleration,
			int rot_acceleration,
			int trans_pos __attribute__ ((unused)),
			int rot_pos __attribute__ ((unused)))
{
  unsigned char data[MAX_COMMAND_LENGTH];
  int i;

  // Translation 

  data[0] = 0;
  convertUInt32((long) 0, &(data[1]) );                 /* velocity */ 
  convertUInt32((long) trans_acceleration, &(data[5])); /* acceleration */ 
  convertUInt32((long) 0, &(data[9]) );                 /* torque */ 
  data[13] = 0;

  cmdSend(MOT_PORT, 0, MOT_AXIS_SET_DIR, 14, data);

  // Rotation

  data[0] = 1;
  convertUInt32((long) 0, &(data[1]) );                  /* velocity */ 
  convertUInt32((long) rot_acceleration, &(data[5]) );   /* acceleration */ 
  convertUInt32((long) 0, &(data[9]) );                  /* torque */ 
  data[13] = 0;

  cmdSend(MOT_PORT, 0, MOT_AXIS_SET_DIR, 14, data);

  //mark all non-existant (or no-data) sonar as such
  //note - this varies from MAX_INT set when sonar fail to get a valid echo.

  for (i=0; i < MAX_NUM_SONARS; i++)
     status.ranges[i] = -1;
}

int carmen_base_direct_sonar_on(void)
{
  unsigned char data[MAX_COMMAND_LENGTH];
  convertUInt32( (long) 30000, &(data[0]) );
  convertUInt32( (long) 0, &(data[4]) );
  convertUInt32( (long) 0, &(data[8]) );
  convertUInt8(  (int) 2, &(data[12]) );
  cmdSend(SONAR_PORT, 4, SONAR_RUN, 13, data);

  sonar_is_on = 1;

  return 0;
}

int carmen_base_direct_sonar_off(void)
{
  unsigned char data[MAX_COMMAND_LENGTH];
  convertUInt32( (long) 0, &(data[0]) );
  convertUInt32( (long) 0, &(data[4]) );
  convertUInt32( (long) 0, &(data[8]) );
  convertUInt8(  (int) 0, &(data[12]) );
  cmdSend(SONAR_PORT, 4, SONAR_RUN, 13, data);

  sonar_is_on = 0;
  return 0;
}

int
carmen_base_direct_initialize_robot(char *model, char *dev)
{
  Device rdev;
  int i;

  rflex_model = -1;
  for (i = 0; carmen_rflex_models[i] != 0; i++) 
    {
      if (strlen(model) == strlen(carmen_rflex_models[i]) &&
	  carmen_strncasecmp(model, carmen_rflex_models[i], 
			     strlen(carmen_rflex_models[i])) == 0)
	{
	  rflex_model = i;
	  break;
	}
  }

  if (rflex_model == -1) 
    {
      carmen_warn("Unknown rflex model %s\nKnown models: ", model);
      for (i = 0; carmen_rflex_models[i] != 0; i++) 
	{
	  carmen_warn("%s ", carmen_rflex_models[i]);
	  if (i % 6 == 5)
	    carmen_warn("\n");
	}
      carmen_warn("To determine your rflex model, read \n"
	      "carmen/src/rflex/README\n");
      return -1;
    }

  distance_conversion = carmen_rflex_params[rflex_model][0];
  angle_conversion = carmen_rflex_params[rflex_model][1];  

  strncpy(rdev.ttyport, dev, MAX_NAME_LENGTH);
  rdev.baud           = 115200;
  rdev.databits       = 8;
  rdev.parity         = N;
  rdev.stopbits       = 1;
  rdev.hwf            = 0;
  rdev.swf            = 0;
  
  if (DEVICE_connect_port(&rdev) < 0) {
    carmen_warn("Can't open device %s\n", rdev.ttyport);
    return -1;
  } 

  dev_fd = rdev.fd;

  carmen_rflex_odometry_on(100000);
  carmen_rflex_digital_io_on(100000);
  carmen_rflex_motion_set_defaults();

  carmen_rflex_brake_off();

  carmen_rflex_initialize(1.0*distance_conversion, 
			  M_PI/4 * angle_conversion, 0, 0);

  return 0;
}

int carmen_base_direct_reset(void)
{
  unsigned char data[MAX_COMMAND_LENGTH];

  convertUInt8((long) 0, &(data[0]));                   /* forward motion */
  convertUInt32((long) 0, &(data[1]));                  /* trans velocity */
  convertUInt32((long) acceleration_state, &(data[5])); /* trans acc */
  convertUInt32((long) STD_TRANS_TORQUE, &(data[9]));   /* trans torque */
  convertUInt32((long) 0, &(data[13]));                 /* trans position */

  cmdSend(MOT_PORT, 0, MOT_AXIS_SET_POS, 17, data);

  convertUInt8((long) 1, &(data[0]));                  /* rotational motion */
  convertUInt32((long) 0, &(data[1]));                 /* rot velocity  */
  convertUInt32((long) STD_ROT_ACC, &(data[5]));       /* rot acc */
  convertUInt32((long) STD_ROT_TORQUE, &(data[9]));    /* rot torque */
  convertUInt32((long) 0, &(data[13]));                /* rot position */

  cmdSend(MOT_PORT, 0, MOT_AXIS_SET_POS, 17, data);

  return 0;
}

int 
carmen_base_direct_shutdown_robot(void)
{
  if (sonar_is_on)
    carmen_base_direct_sonar_off();

  carmen_rflex_odometry_off();
  carmen_rflex_digital_io_off();
  carmen_rflex_brake_on();
  carmen_rflex_motion_set_defaults();

  return 0;
}

int 
carmen_base_direct_set_acceleration(double acceleration)
{
  acceleration_state = acceleration * distance_conversion;

  return 0;
}

int 
carmen_base_direct_set_deceleration(double deceleration)
{
  deceleration_state = deceleration * distance_conversion;

  return 0;
}

int 
carmen_base_direct_set_velocity(double tv, double rv)
{
  unsigned char data[MAX_COMMAND_LENGTH];

  tv = tv * distance_conversion;
  rv = rv * angle_conversion;
  
  convertUInt8( (long) 0, &(data[0]));                   /* forward motion */
  convertUInt32( (long) abs(tv), &(data[1]));        /* abs trans velocity */

  if ((tv > 0 && tv < status.t_vel) || (tv < 0 && tv > status.t_vel))
    convertUInt32( (long) deceleration_state, &(data[5])); /* trans acc */
  else
    convertUInt32( (long) acceleration_state, &(data[5])); /* trans acc */

  convertUInt32( (long) STD_TRANS_TORQUE, &(data[9]));   /* trans torque */
  convertUInt8((long) sgn(tv), &(data[13]));           /* trans direction */

  cmdSend(MOT_PORT, 0, MOT_AXIS_SET_DIR, 14, data);

  convertUInt8((long) 1, &(data[0]));                  /* rotational motion */
  convertUInt32((long) abs(rv), &(data[1]));         /* abs rot velocity  */
  /* 0.275 rad/sec * 10000 */ 
  convertUInt32((long) STD_ROT_ACC, &(data[5]));       /* rot acc */
  convertUInt32((long) STD_ROT_TORQUE, &(data[9]));    /* rot torque */
  convertUInt8((long) sgn(rv), &(data[13]));         /* rot direction */

  cmdSend(MOT_PORT, 0, MOT_AXIS_SET_DIR, 14, data);

  return 0;
}

int 
carmen_base_direct_update_status(double* packet_timestamp __attribute__ ((unused)))
{
  clear_incoming_data();

  return 0;
}

int 
carmen_base_direct_get_state(double *displacement, double *rotation,
			     double *tv, double *rv)
{
  if (displacement)
    *displacement = (status.current_displacement_odometry - 
		     status.last_displacement_odometry) / distance_conversion;
  
  if (rotation)
    *rotation = (status.current_bearing_odometry - 
		 status.last_bearing_odometry) / angle_conversion;
  if (tv)
    *tv = status.t_vel / distance_conversion;
  if (rv)
    *rv = status.r_vel / angle_conversion;

  status.last_displacement_odometry = status.current_displacement_odometry;
  status.last_bearing_odometry = status.current_bearing_odometry;

  return 0;
}

int 
carmen_base_direct_get_integrated_state(double *x __attribute__ ((unused)), 
					double *y __attribute__ ((unused)), 
					double *theta __attribute__ ((unused)),
					double *tv __attribute__ ((unused)), 
					double *rv __attribute__ ((unused)))
{
  carmen_warn("Hardware integration not supported by RFlex.\n");

  return 0;
}

int 
carmen_base_direct_send_binary_data(unsigned char *data 
				    __attribute__ ((unused)), 
				    int size __attribute__ ((unused)))
{

  return 0;
}

int 
carmen_base_direct_get_binary_data(unsigned char **data
				   __attribute__ ((unused)), 
				   int *size __attribute__ ((unused)))
{

  return 0;
}

int carmen_base_direct_get_bumpers(unsigned char *state
				   __attribute__ ((unused)), 
				   int num_bumpers __attribute__ ((unused)))
{

  return 0;
}

void carmen_base_direct_arm_get(double servos[] __attribute__ ((unused)), 
				int num_servos __attribute__ ((unused)), 
				double *currents __attribute__ ((unused)), 
				int *gripper __attribute__ ((unused)))
{
}

void carmen_base_direct_arm_set(double servos[] __attribute__ ((unused)), 
				int num_servos __attribute__ ((unused)))
{
}

int carmen_base_direct_get_sonars(double *ranges, 
				  carmen_point_t *positions 
				  __attribute__ ((unused)),
				  int num_sonars)
{
  int i;

  for(i=0; i < num_sonars && i < status.num_sonars; i++) {
    if (status.ranges[i] < 0)
      continue;
    else if(status.ranges[i]==32766)
      ranges[i]=0;
    else
      ranges[i]=status.ranges[i];
  }
  
  if ( i < num_sonars)
    carmen_warn("Requested more sonars (%d) than available (%d).\n",
		num_sonars, i);      
  
  return 0;
}