{  int i;  int pioneer_model;  int result;  int count;  pioneer_model = -1;  for (i = 0; carmen_pioneer_models[i][0] != 0; i++)   {    if (strlen(model) == strlen(carmen_pioneer_models[i]) &&        carmen_strncasecmp(model, carmen_pioneer_models[i],           strlen(carmen_pioneer_models[i])) == 0)    {      pioneer_model = i;      break;    }  }  if (pioneer_model == -1)     {      carmen_warn("Unknown pioneer model %s\nKnown models: ", model);      for (i = 0; carmen_pioneer_models[i][0] != 0; i++) 	{	  carmen_warn("%s ", carmen_pioneer_models[i]);	  if (i % 6 == 5)	    carmen_warn("\n");	  carmen_warn("To determine your pioneer model, read \n"		      "carmen/src/pioneer/README\n");	  return -1;	}    }  angle_conv_factor = carmen_pioneer_params[pioneer_model][0];  dist_conv_factor = carmen_pioneer_params[pioneer_model][1];  vel_conv_factor = carmen_pioneer_params[pioneer_model][2];  range_conv_factor = carmen_pioneer_params[pioneer_model][3];  diff_conv_factor = carmen_pioneer_params[pioneer_model][4];  vel2_divisor = carmen_pioneer_params[pioneer_model][5];  if (pioneer_model < 17)     pioneer_version = 2;  else    pioneer_version = 1;  if (carmen_serial_connect(&dev_fd, dev) < 0)     return -1;  count = 0;  do     {      result = pioneer_sync0();      if (result < 0 && count == 4)	return -1;      count++;    }  while (result < 0);  if (pioneer_sync1() < 0)    return -1;  if (pioneer_sync2() < 0)    return -1;  pioneer_send_command0(PIONEER_OPEN);  pioneer_sonar(0);  pioneer_send_command0(PIONEER_PULSE);  if (pioneer_version == 2)     pioneer_send_command1(PIONEER_ENABLE, PIONEER_ON);  return 0;}int carmen_base_direct_shutdown_robot(void) {  if (sonar_is_on) {    carmen_base_direct_sonar_off();    usleep(1000000);  }  pioneer_send_command0(PIONEER_CLOSE);  usleep(1000000);  close(dev_fd);  fprintf( stderr, "\nClosed\n" );  return(0);}int carmen_base_direct_set_acceleration(double acceleration){  acceleration = fabs(acceleration) * 1000;  return pioneer_send_command1(PIONEER_SETA, acceleration);}int carmen_base_direct_set_deceleration(double deceleration){  deceleration = -fabs(deceleration) * 1000;  return pioneer_send_command1(PIONEER_SETA, deceleration);}int carmen_base_direct_set_velocity(double tv, double rv){  /** Cyrill, 13.Jan.2004      Pioneer drives much nicer when using VEL2 instead      of VEL and RVEL commands.       ActiveMedia's strage dist_conf_factor comes from:      distBetweemLeftAndRightWheel = 0.002 / dist_conf_factor  */  int result = 0;  double vl = tv  - 0.001 * rv / diff_conv_factor;  double vr = tv  + 0.001 * rv / diff_conv_factor;  if (pioneer_version == 1)   {    vl *= PIONEER1_VEL2_CONVERSION_FACTOR;    vr *= PIONEER1_VEL2_CONVERSION_FACTOR;  }  else   {    vl *= PIONEER2_VEL2_CONVERSION_FACTOR;    vr *= PIONEER2_VEL2_CONVERSION_FACTOR;  }  result =  pioneer_vel2((int) rint(vl), (int) rint(vr));    if (result < 0)    return -1;  return 0;  /* The old method to send the velocities to the robot.     Pioneer Robots (can) drive quite poor if using     VEL and RVEL command, that's why I changed the     code to the  VEL2 command */  /*   int err;  *//*   int arg; *//*   arg = tv * 1000; *//*   err = pioneer_send_command1(PIONEER_VEL, arg); *//*   if (err < 0) *//*     return -1; */  /*   arg = carmen_radians_to_degrees(rv); *//*   err = pioneer_send_command1(PIONEER_RVEL, arg); *//*   if (err < 0) *//*     return -1; *//*   return 0; */}int carmen_base_direct_update_status(double* update_timestamp) {  int read = 0;  double packet_timestamp=0.;  pioneer_send_command0(PIONEER_PULSE);  memset(&raw_state, 0, sizeof(struct pioneer_raw_information_packet));  raw_state.motor_status = 0;  do     {      read = pioneer_read_string((unsigned char*) &raw_state, 				 PIONEER_SERIAL_TIMEOUT, &packet_timestamp);    }  while (carmen_serial_numChars(dev_fd) > 0);  if (raw_state.motor_status != PIONEER_MOTOR_NO_POWER &&      raw_state.motor_status != PIONEER_MOTOR_STOPPED &&      raw_state.motor_status != PIONEER_MOTOR_MOVING)     {      carmen_warn("\nTried to convert non-information packet.\n");      return -1;    }  if (update_timestamp){    *update_timestamp=packet_timestamp;  }  return 0;  //  return read;}intcarmen_base_direct_get_state(double *displacement, double *rotation,			     double *tv, double *rv){  double delta_x, delta_y, delta_theta, vl, vr;  static int initialised = 0;  static int    pioneer_prev_x, pioneer_prev_y;  int           pioneer_x, pioneer_y;  int           pioneer_delta_x, pioneer_delta_y;    static double prev_theta;  double theta;  if (raw_state.motor_status != PIONEER_MOTOR_NO_POWER &&      raw_state.motor_status != PIONEER_MOTOR_STOPPED &&      raw_state.motor_status != PIONEER_MOTOR_MOVING)     {      carmen_warn("\nTried to convert non-information packet.\n");      return -1;    }  pioneer_x = pioneer_buf_to_unsigned_int(raw_state.x);  pioneer_y = pioneer_buf_to_unsigned_int(raw_state.y);  theta =    pioneer_buf_to_unsigned_int(raw_state.orientation) * angle_conv_factor;  theta = carmen_normalize_theta(theta);  if (initialised)     {            pioneer_delta_x = pioneer_x - pioneer_prev_x;      pioneer_delta_y = pioneer_y - pioneer_prev_y;      // x and y roll-over by 0x7fff       if ( pioneer_delta_x > 0x3fff ) {	pioneer_delta_x -= 0x7fff;      } else if ( pioneer_delta_x < -0x3fff ) {	pioneer_delta_x += 0x7fff;      }      if ( pioneer_delta_y > 0x3fff ) {	pioneer_delta_y -= 0x7fff;      } else if ( pioneer_delta_y < -0x3fff ) {	pioneer_delta_y += 0x7fff;      }      delta_x = pioneer_delta_x * dist_conv_factor / 1000.0;      delta_y = pioneer_delta_y * dist_conv_factor / 1000.0;      delta_theta = theta - prev_theta;      if (theta < prev_theta && prev_theta - theta > M_PI)	delta_theta = (theta + M_PI) - (prev_theta - M_PI);      else if (theta > prev_theta && theta - prev_theta > M_PI)	delta_theta = (theta - M_PI) - (prev_theta + M_PI);      if (displacement) {	*displacement = hypot(delta_x, delta_y);      }      if (rotation)	*rotation = delta_theta;    }  else     {      initialised = 1;    }  pioneer_prev_x     = pioneer_x;  pioneer_prev_y     = pioneer_y;  prev_theta         = theta;  vl = pioneer_buf_to_int(raw_state.vl) * vel_conv_factor / 1000;  vr = pioneer_buf_to_int(raw_state.vr) * vel_conv_factor / 1000;  // We do this because the stupid ActivMedia controller returns non-zero  // values for wheel velocity when the robot is not moving.  if (fabs(vl) < .05)    vl = 0;  if (fabs(vr) < .05)    vr = 0;  if (tv)    *tv = (vl+vr)/2;  if (*tv<0.0 && *displacement )    *displacement *= -1;  /*   if (rv) *//*     *rv = (vr-vl) * diff_conv_factor; */  /** Cyrill, 13.Jan.2004      ActivMedia uses this strange diff_conv_factor to compute the       rotational velocity from the individual wheel speeds.      Mathematically it should be:      omega = (vRightWheel - vLeftWheel) / DistanceBetweenRightAndLeftWheel      That means ActivMedia's diff_conv_factor is computed by      diff_conv_factor = 0.002/DistanceBetweenRightAndLeftWheelInM      If we want to use ActivMedia's parameter, we must compute:      omega = (vRightWheel - vLeftWheel) * 500.0 * diff_conv_factor;  */  if (rv)    *rv = (vr-vl) * 500.0 * diff_conv_factor;    return 0;}intcarmen_base_direct_get_integrated_state(double *x, double *y, double *theta,					double *tv, double *rv){  double vl, vr;  if (raw_state.motor_status != PIONEER_MOTOR_NO_POWER &&      raw_state.motor_status != PIONEER_MOTOR_STOPPED &&      raw_state.motor_status != PIONEER_MOTOR_MOVING)     {      carmen_warn("\nTried to convert non-information packet.\n");      return -1;    }  if (x)    *x = pioneer_buf_to_unsigned_int(raw_state.x) * dist_conv_factor/1000;  if (y)    *y = pioneer_buf_to_unsigned_int(raw_state.y) * dist_conv_factor/1000;  if (theta)    {      *theta = pioneer_buf_to_int(raw_state.orientation) * 	angle_conv_factor;      *theta = carmen_normalize_theta(*theta);    }  vl = pioneer_buf_to_int(raw_state.vl) * vel_conv_factor / 1000;  vr = pioneer_buf_to_int(raw_state.vr) * vel_conv_factor / 1000;  // We do this because the stupid ActivMedia controller returns non-zero  // values for wheel velocity when the robot is not moving.  if (fabs(vl) < .05)    vl = 0;  if (fabs(vr) < .05)    vr = 0;  if (tv)    *tv = (vl+vr)/2;  if (rv)    *rv = (vr-vl) * diff_conv_factor;  return 0;}int carmen_base_direct_send_binary_data(unsigned char *data, int size){  return pioneer_send_string(data, size);}int carmen_base_direct_get_binary_data(unsigned char **data, int *size){  if (data == NULL || size == NULL)    return 0;  (*data) = calloc(1, sizeof(struct pioneer_raw_information_packet));    carmen_test_alloc(*data);  *size = sizeof(struct pioneer_raw_information_packet);  memcpy(*data, &raw_state, *size);  return 0;}int carmen_base_direct_get_bumpers(unsigned char *state, int num_bumpers){  int i;  if (state == NULL || num_bumpers == 0)    return PIONEER_MAX_NUM_BUMPERS;  for (i=0; i<7 && i<num_bumpers; i++){    state[i]=((raw_state.bumper[1])&(1<<(i+1)))!=0?1:0;  }  int j;  for (j=0; j<7 && i<num_bumpers; j++){    state[i++]=((raw_state.bumper[0])&(1<<(j+1))) != 0 ? 1 : 0;  }  return PIONEER_MAX_NUM_BUMPERS;}void carmen_base_direct_arm_get(double servos[] __attribute__ ((unused)), 				int num_servos __attribute__ ((unused)), 				double *currents __attribute__ ((unused)), 				int *gripper __attribute__ ((unused))){  carmen_warn("%s not supported by pioneer.\n", __FUNCTION__);}void carmen_base_direct_arm_set(double servos[] __attribute__ ((unused)), 				int num_servos __attribute__ ((unused))){  carmen_warn("%s not supported by pioneer.\n", __FUNCTION__);}int carmen_base_direct_get_sonars(double *ranges, 				  carmen_point_t *positions 				  __attribute__ ((unused)),				  int num_sonars){  int i;  double range;  if (ranges != NULL && sonar_is_on) {    //fprintf(stderr, "sonars %d, readings %d:", num_sonars, raw_state.num_sonar_readings);    for (i = 0; i < num_sonars; i++){      ranges[i]=-1.;    }    for (i = 0; i < raw_state.num_sonar_readings; i++) {      range = pioneer_buf_to_unsigned_int(raw_state.sonar[i].range) *	range_conv_factor / 1000.0;      if (raw_state.sonar[i].n < num_sonars)        ranges[raw_state.sonar[i].n] = range;    } /*     for (i = 0; i < num_sonars; i++){ *//*       fprintf(stderr, "%f " ,ranges[i]); *//*     } *//*     fprintf(stderr, "\n"); */ }  return 0;}