orc_lib.c

卡耐基.梅隆大学的机器人仿真软件（Redhat linux 9下安装）

  buffer[1] = motor;
  buffer[2] = dir;
  buffer[3] = pwm;
  if (send_packet_and_ack(buffer, 4, ORC_SLAVE) < 0) {
    return -1;
  }

  return 0;
}


int carmen_base_direct_initialize_robot(char *model __attribute__ ((unused)),
          char *dev)
{
  int result;

  result = carmen_serial_connect(&serial_fd, dev);
  if(result == -1) {
    serial_fd = -1;
    return -1;
  }

  if (strstr(dev, "USB") == NULL) {
    carmen_warn("This doesn't look like a USB port. Setting linespeed to 115200\n");
    carmen_serial_configure(serial_fd, 115200, "8N1");
  } else {
    carmen_warn("This looks like a USB port. Setting linespeed to 500000\n");
    carmen_serial_configure(serial_fd, 500000, "8N1");
  }

  fprintf(stderr, "Clearing input buffer...");
  carmen_serial_ClearInputBuffer(serial_fd);

  carmen_base_direct_update_status(NULL);
  carmen_base_direct_update_status(NULL);
  carmen_serial_ClearInputBuffer(serial_fd);
  fprintf(stderr, " ok.\n");

  // Set pins for Motor Encoder 0 into fast mode

  if (orc_set_encoder_quad_phase_fast(ORC_LEFT_MOTOR) < 0)
    return -1;
  if (orc_set_encoder_quad_phase_fast(ORC_LEFT_MOTOR+1) < 0)
    return -1;

  // Set pins for Motor Encoder 1 into fast mode

  if (orc_set_encoder_quad_phase_fast(ORC_RIGHT_MOTOR) < 0)
    return -1;
  if (orc_set_encoder_quad_phase_fast(ORC_RIGHT_MOTOR+1) < 0)
    return -1;

  // Set motor slew rates 

  if (orc_set_motor_slew(ORC_LEFT_MOTOR, 10) < 0)
    return -1;
  if (orc_set_motor_slew(ORC_RIGHT_MOTOR, 10) < 0)
    return -1;

  // Set servo pins into servo mode
  
  if (orc_set_pin(ORC_SERVO_PIN_0, "servo", ORC_SERVO_MODE) < 0)
    return -1;
  if (orc_set_pin(ORC_SERVO_PIN_1, "servo", ORC_SERVO_MODE) < 0)
    return -1;
  if (orc_set_pin(ORC_SERVO_PIN_2, "servo", ORC_SERVO_MODE) < 0)
    return -1;
  if (orc_set_pin(ORC_SERVO_PIN_3, "servo", ORC_SERVO_MODE) < 0)
    return -1;

  if (orc_set_pin(ORC_LEFT_SONAR_PING_PIN, "left sonar ping",
		  ORC_SONAR_PING_MODE) < 0)
    return -1;
  if (orc_set_pin(ORC_LEFT_SONAR_ECHO_PIN, "left sonar echo",
		  ORC_SONAR_ECHO_MODE) < 0)
    return -1;
  if (orc_set_pin(ORC_RIGHT_SONAR_PING_PIN, "right sonar ping",
		  ORC_SONAR_PING_MODE) < 0)
    return -1;
  if (orc_set_pin(ORC_RIGHT_SONAR_ECHO_PIN, "right sonar echo",
		  ORC_SONAR_ECHO_MODE) < 0)
    return -1;

  if (orc_set_pin(ORC_BUMPER_PIN_0, "bumper", ORC_DIGITAL_IN_PULL_UP) < 0)
    return -1;
  if (orc_set_pin(ORC_BUMPER_PIN_1, "bumper", ORC_DIGITAL_IN_PULL_UP) < 0)
    return -1;
  if (orc_set_pin(ORC_BUMPER_PIN_2, "bumper", ORC_DIGITAL_IN_PULL_UP) < 0)
    return -1;
  if (orc_set_pin(ORC_BUMPER_PIN_3, "bumper", ORC_DIGITAL_IN_PULL_UP) < 0)
    return -1;
  if (orc_set_pin(ORC_GRIPPER_PIN, "gripper sensor", 
		  ORC_DIGITAL_IN_PULL_UP) < 0)
    return -1;

  fprintf(stderr, "Zeroing left motor velocity ... ");
  if (orc_set_pwm(ORC_LEFT_MOTOR, 0, 0) < 0) {
    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);
    return -1;
  }
  fprintf(stderr, "ok.\n");
  fprintf(stderr, "Zeroing right motor velocity ... ");
  if (orc_set_pwm(ORC_RIGHT_MOTOR, 0, 0) < 0) {
    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);
    return -1;
  }
  fprintf(stderr, "ok.\n");

  fprintf(stderr, "Checking board status... ");
  if (carmen_base_direct_update_status(NULL) < 0) {
    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);
    return -1;
  }

  fprintf(stderr, "Orc Board initialization succeeded.\n");

  return 0;
}

int carmen_base_direct_shutdown_robot(void)
{
  carmen_base_direct_set_velocity(0.0, 0.0);

  close(serial_fd);

  return 0;
}

int carmen_base_direct_set_acceleration(double new_acceleration)
{
  acceleration = new_acceleration;
  //  printf("carmen_base_direct_set_acceleration: accel=%f\n", new_acceleration);

  return 0;
}

int carmen_base_direct_set_deceleration(double new_deceleration)
{
  deceleration = new_deceleration;
  //  printf("carmen_base_direct_set_deceleration: decel=%f\n", new_deceleration);

  return 0;
}

int carmen_base_direct_set_velocity(double new_tv, double new_rv)
{
  left_desired_velocity = new_tv;
  right_desired_velocity = new_tv;

  right_desired_velocity += new_rv/2;
  left_desired_velocity -= new_rv/2;

  carmen_base_command_velocity(left_desired_velocity, left_velocity,
       ORC_LEFT_MOTOR);
  carmen_base_command_velocity(right_desired_velocity, right_velocity,
       ORC_RIGHT_MOTOR);

  time_since_last_command = carmen_get_time() - last_command_time;
  last_command_time = carmen_get_time();

  printf("carmen_base_direct_set_velocity: tv=%.2f, rv=%.2f\n", 
	 new_tv, new_rv);
  carmen_base_direct_update_status(NULL);

  return 0;
}

static unsigned char *get_status_packet(unsigned char where)
{
  unsigned char byte;
  unsigned char *buffer = NULL;
  unsigned char routing = 0;
  int count;

  byte = ORC_STATUS;

  count = 0;
  do {
    send_packet(&byte, 1, where);
    buffer = read_packet();
    if (buffer != NULL) {
      byte = buffer[2];
      routing = byte >> 6;
      if (buffer[3] == ORC_STATUS && routing == where) {
	return buffer;
      } else {
	carmen_warn("Out of order packet %c %d %d %d\n", buffer[3],
		    buffer[3], routing, where);
	free(buffer);
	buffer = NULL;
      }
    }
    count++;
    if (0)
    carmen_warn("Count %d\n", count);
  } while (count < 5);
  
  return buffer;

}

int carmen_base_direct_update_status(double* packet_timestamp __attribute__((unused)))
{
  unsigned char *buffer;
  double start_time = carmen_get_time();

  buffer = get_status_packet(ORC_MASTER);
  if (buffer == NULL)
    return -1;
  unpack_master_packet(buffer);
  free(buffer);

  if(0)
    carmen_warn("time to update: %.2f\n", carmen_get_time() -
		start_time);

  buffer = get_status_packet(ORC_SLAVE);
  if (buffer == NULL)
    return -1;
  unpack_slave_packet(buffer);
  free(buffer);

  if(0)
  carmen_warn("time to update: %.2f\n", carmen_get_time() -
	      start_time);
  return 0;
}

int carmen_base_direct_get_state(double *disp_p __attribute__ ((unused)), 
				 double *rot_p __attribute__ ((unused)),
				 double *tv_p __attribute__ ((unused)), 
				 double *rv_p __attribute__ ((unused)))
{

  carmen_die("Turn base_hardware_integrator on\n");

  return 0;
}

int carmen_base_query_encoders(double *disp_p __attribute__ ((unused)),
			       double *rot_p __attribute__ ((unused)),
			       double *tv_p __attribute__ ((unused)),
			       double *rv_p __attribute__ ((unused)))
{
  carmen_die("Turn base_hardware_integrator on\n");

  return 0;
}

int carmen_base_query_low_level(double *left_disp, double *right_disp,
				double *delta_time)
{
  int err;

  err = carmen_base_direct_update_status(NULL);

  if (err < 0)
    return -1;

  *left_disp = left_displacement;
  *right_disp = right_displacement;
  *delta_time = delta_slave_time;

  return 0;
}

int carmen_base_direct_get_integrated_state(double *x_p, double *y_p,
					    double *theta_p, double *tv_p,
					    double *rv_p)
{
  int err;

  err = carmen_base_direct_update_status(NULL);
  if (err < 0)
    return -1;

  if(0)
  carmen_warn("before %f %f %f\n", x, y, theta);

  *x_p = x;
  *y_p = y;
  *theta_p = theta;
  *tv_p = (left_velocity+right_velocity)/2;
  *rv_p = atan2(right_velocity-left_velocity, ORC_WHEEL_BASE);  

  return 0;
}

int carmen_base_direct_get_sonars(double *ranges, carmen_point_t *positions,
          int num_sonars)
{
  if (num_sonars >= 1) {
    ranges[0] = left_range;
    positions[0].x = .05;
    positions[0].y = -.10;
    positions[0].theta = -M_PI/2;
  }
  if (num_sonars >= 2) {
    ranges[1] = right_range;
    positions[1].x = .05;
    positions[1].y = .10;
    positions[1].theta = M_PI/2;
  }

  return 0;
}

int carmen_base_direct_get_bumpers(unsigned char *bumpers_p, int num_bumpers)
{
  if (num_bumpers >= 1)
    bumpers_p[0] = bumpers[0];
  if (num_bumpers >= 2)
    bumpers_p[1] = bumpers[1];
  if (num_bumpers >= 3)
    bumpers_p[2] = bumpers[2];
  if (num_bumpers >= 4)
    bumpers_p[3] = bumpers[3];
  return 4;
}

int carmen_base_direct_send_binary_data(unsigned char *data, int size)
{
  return carmen_serial_writen(serial_fd, data, size);
}

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

  return 0;
}

int carmen_arm_direct_num_velocities(int num_joints __attribute__ ((unused)))
{
  return 0;
}

int carmen_arm_direct_num_currents(int num_joints)
{
  if (num_joints > 2)
    return 2;
  return num_joints;
}

void carmen_arm_direct_set(double *joint_angles, int num_joints)
{
  unsigned char buffer[4];
  unsigned short pwm;
  int i;
  
  if (num_joints > 4) {
    carmen_warn("orc_lib.c only supports 4 joints (%d sent)\n"
                "Returning only 4\n", num_joints);
    num_joints = 4;
  }
  
  for (i=0;i<num_joints;i++) { 
    int nservo=(int)joint_angles[i];
    pwm = nservo;
    buffer[0] = 0x53;
    buffer[1] = i;
    buffer[2] = pwm >> 8;
    buffer[3] = pwm & 0x00ff;
    send_packet(buffer, 4, ORC_MASTER);
  }
}

void carmen_arm_direct_get_state(double *joint_angles, double *joint_currents,
				 double *joint_angular_vels
				 __attribute__ ((unused)), 
				 int *gripper_closed,
				 int num_joints)
{
  int i;
  
  if (num_joints > 4) {
    carmen_warn("orc_lib.c only supports 4 joints (%d requested)\n"
                "Returning only 4\n", num_joints);
    num_joints = 4;
  }
 
  for (i = 0; i < num_joints; i++) {
    joint_angles[i] = servo_state[i];
  }
  if (joint_currents && num_joints > 0) {
    joint_currents[0] = servo_current[0];
    if (num_joints > 1)
      joint_currents[1] = servo_current[1];
  }
  if (gripper_closed)
    *gripper_closed = gripper_state;
}