orc_arm_lib.c

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

    right_delta_tick = right_delta_tick - 2*SHRT_MAX;
  if (right_delta_tick < -SHRT_MAX/2)
    right_delta_tick = right_delta_tick + 2*SHRT_MAX;

  left_last_tick = left_tick;
  right_last_tick = right_tick;

  right_delta_tick = -right_delta_tick;

  left_displacement = delta_tick_to_metres(left_delta_tick);
  right_displacement = delta_tick_to_metres(right_delta_tick);


  // velezj: For RssII Arm
  shoulder_pwm = buffer[ORC_SHOULDER_MOTOR_ACTUAL_PWM];
  elbow_pwm = buffer[ORC_ELBOW_MOTOR_ACTUAL_PWM];

  shoulder_dir = ((buffer[ORC_SHOULDER_MOTOR_DIR] & 0x0f) ) & 0x03;
  elbow_dir = ((buffer[ORC_ELBOW_MOTOR_DIR] & 0x0f) ) & 0x03;

  if (shoulder_dir == ORC_BACKWARD)
    shoulder_pwm = -shoulder_pwm;
  if (elbow_dir == ORC_BACKWARD)
    elbow_pwm = -elbow_pwm;

  shoulder_tick = ((buffer[ORC_SHOULDER_MOTOR_ENCODER]&0x00ff)<<8)+
    (buffer[ORC_SHOULDER_MOTOR_ENCODER+1]&0x00ff);

  shoulder_delta_tick = shoulder_tick - shoulder_last_tick;
  if (shoulder_delta_tick > SHRT_MAX/2)
    shoulder_delta_tick = shoulder_delta_tick - 2*SHRT_MAX;
  if (shoulder_delta_tick < -SHRT_MAX/2)
    shoulder_delta_tick = shoulder_delta_tick + 2*SHRT_MAX;

  elbow_tick = ((buffer[ORC_ELBOW_MOTOR_ENCODER]&0x00ff)<<8)+
    (buffer[ORC_ELBOW_MOTOR_ENCODER+1]&0x00ff);
  elbow_delta_tick = elbow_tick - elbow_last_tick;
  if (elbow_delta_tick > SHRT_MAX/2)
    elbow_delta_tick = elbow_delta_tick - 2*SHRT_MAX;
  if (elbow_delta_tick < -SHRT_MAX/2)
    elbow_delta_tick = elbow_delta_tick + 2*SHRT_MAX;

  shoulder_last_tick = shoulder_tick;
  elbow_last_tick = elbow_tick;

  //elbow_delta_tick = -elbow_delta_tick;

  shoulder_angle_change = delta_ticks_to_angle(shoulder_delta_tick);
  elbow_angle_change = delta_ticks_to_angle(elbow_delta_tick);
  if( fabs( shoulder_desired_theta - shoulder_theta ) > 0.01 ) {
    printf( "shoulder_delta_theta: %f  (%d)\n", shoulder_angle_change, shoulder_delta_tick );
  }
  if( fabs( elbow_desired_theta - elbow_theta ) > 0.01 ) {
    printf( "elbow_delta_theta: %f  (%d)\n", elbow_angle_change, elbow_delta_tick );
  }



  time_ticks = ((buffer[ORC_SLAVE_TIME]&0x00ff)<<8)+
    (buffer[ORC_SLAVE_TIME+1]&0x00ff);

  delta_slave_ticks = time_ticks - last_slave_ticks;
  last_slave_ticks = time_ticks;

  if (delta_slave_ticks > SHRT_MAX/2)
    delta_slave_ticks -= 2*SHRT_MAX;
  if (delta_slave_ticks < -SHRT_MAX/2)
    delta_slave_ticks += 2*SHRT_MAX;

  delta_slave_time = delta_slave_ticks/4000.0;

  displacement = (left_displacement+right_displacement)/2;
  rotation = atan2(right_displacement-left_displacement, ORC_WHEEL_BASE);

  left_velocity = left_displacement/delta_slave_time;
  right_velocity = right_displacement/delta_slave_time;

  x = x+cos(theta);
  y = y+sin(theta);
  theta = carmen_normalize_theta(theta+rotation);

  // velezj: For RssII Arm
  shoulder_theta = shoulder_theta + shoulder_angle_change;
  elbow_theta = elbow_theta + elbow_angle_change;
  // since elbow relative to shoulder and when the shoulder moves to elbow angle
  // in fact does NOT change with it, so we must update our elbow angle even when
  // only the shoulder moves
  //elbow_theta -= shoulder_angle_change; 
  shoulder_angular_velocity = shoulder_angle_change / delta_slave_time;
  elbow_angular_velocity = elbow_angle_change / delta_slave_time;
  if( fabs( shoulder_desired_theta - shoulder_theta ) > 0.01 || fabs( elbow_desired_theta - elbow_theta ) > 0.01 ) {
    printf( "shoulder_theta: %f  (av = %f)\n", shoulder_theta, shoulder_angular_velocity );
    printf( "elbow_theta:    %f  (av = %f)\n", elbow_theta, elbow_angular_velocity  );
  }

  time_since_last_command = carmen_get_time() - last_command_time;
  last_command_time = carmen_get_time();
  command_velocity(left_desired_velocity, left_velocity, left_pwm,
       ORC_LEFT_MOTOR);
  command_velocity(right_desired_velocity, right_velocity, right_pwm,
       ORC_RIGHT_MOTOR);
}

int carmen_arm_direct_reset(void)
{
  printf("carmen_arm_direct_reset\n");
  initialized = 0;
  return 0;
}


int carmen_arm_direct_initialize(char *model __attribute__ ((unused)), char *dev)
{
  int result;
  unsigned char buffer[5];

  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_arm_direct_update_status();
  carmen_arm_direct_update_status();
  carmen_serial_ClearInputBuffer(serial_fd);
  fprintf(stderr, " ok.\n");

  buffer[0] = 'C';
  buffer[1] = 0; // Motor Encoder 0
  buffer[2] = 14; // Quad Phase Fast

  fprintf(stderr, "Setting motor encoder 0 into quad phase fast mode... ");
  if (send_packet_and_ack(buffer, 3, ORC_SLAVE) < 0) {
    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);
    return -1;
  }
  fprintf(stderr, "ok.\n");

  buffer[1] = 1; // Motor Encoder 0
  fprintf(stderr, "Setting motor encoder 1 into quad phase fast mode... ");
  if (send_packet_and_ack(buffer, 3, ORC_SLAVE) < 0) {
    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);
    return -1;
  }
  fprintf(stderr, "ok\n");

  buffer[1] = 2; // Motor Encoder 1
  fprintf(stderr, "Setting motor encoder 2 into quad phase fast mode... ");
  if (send_packet_and_ack(buffer, 3, ORC_SLAVE) < 0) {
    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);
    return -1;
  }
  fprintf(stderr, "ok\n");

  buffer[1] = 3; // Motor Encoder 1
  fprintf(stderr, "Setting motor encoder 3 into quad phase fast mode... ");
  if (send_packet_and_ack(buffer, 3, ORC_SLAVE) < 0) {
    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);
    return -1;
  }
  fprintf(stderr, "ok\n");

  buffer[0] = 'W';
  buffer[1] = ORC_LEFT_MOTOR; 
  buffer[2] = 10;             // Quad Phase Fast
  fprintf(stderr, "Setting left motor slew to %d... ", buffer[2]);
  if (send_packet_and_ack(buffer, 3, ORC_SLAVE) < 0) {
    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);
    return -1;
  }
  fprintf(stderr, "ok\n");

  buffer[1] = ORC_RIGHT_MOTOR; 
  fprintf(stderr, "Setting right motor slew to %d... ", buffer[2]);
  if (send_packet_and_ack(buffer, 3, ORC_SLAVE) < 0) {
    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);
    return -1;
  }
  fprintf(stderr, "ok\n");  



  //Edsinger: added servo 
  buffer[0] = 'C';
  buffer[1] = 0; // Servo0
  buffer[2] = ORC_SERVO_PIN; //Servo
  fprintf(stderr, "Setting servo pin 0 into servo mode... ");
  if (send_packet_and_ack(buffer, 3, ORC_MASTER) < 0) {
    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);
    return -1;
  }
  fprintf(stderr, "ok\n");

  buffer[1] = 1; // Servo1
  buffer[2] = ORC_SERVO_PIN; //Servo
  fprintf(stderr, "Setting servo pin 1 into servo mode... ");
  if (send_packet_and_ack(buffer, 3, ORC_MASTER) < 0) {
    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);
    return -1;
  }
  fprintf(stderr, "ok\n");

  buffer[1] = 2; // Servo2
  buffer[2] = ORC_SERVO_PIN; //Servo
  fprintf(stderr, "Setting servo pin 2 into servo mode... ");
  if (send_packet_and_ack(buffer, 3, ORC_MASTER) < 0) {
    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);
    return -1;
  }
  fprintf(stderr, "ok\n");

  buffer[1] = 3; // Servo3
  buffer[2] = ORC_SERVO_PIN; //Servo
  fprintf(stderr, "Setting servo pin 3 into servo mode... ");
  if (send_packet_and_ack(buffer, 3, ORC_MASTER) < 0) {
    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);
    return -1;
  }
  fprintf(stderr, "ok\n");

  buffer[1] = 10; // Bumper
  buffer[2] = ORC_DIGITAL_IN_PULL_UP;
  fprintf(stderr, "Setting bumper 0 into digital pull-up mode... ");
  if (send_packet_and_ack(buffer, 3, ORC_MASTER) < 0) {
    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);
    return -1;
  }
  fprintf(stderr, "ok\n");

  buffer[1] = 11; // Bumper
  buffer[2] = ORC_DIGITAL_IN_PULL_UP;
  fprintf(stderr, "Setting bumper 1 into digital pull-up mode... ");
  if (send_packet_and_ack(buffer, 3, ORC_MASTER) < 0) {
    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);
    return -1;
  }
  fprintf(stderr, "ok\n");

  buffer[1] = 12; // Bumper
  buffer[2] = ORC_DIGITAL_IN_PULL_UP;
  fprintf(stderr, "Setting bumper 2 into digital pull-up mode... ");
  if (send_packet_and_ack(buffer, 3, ORC_MASTER) < 0) {
    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);
    return -1;
  }
  fprintf(stderr, "ok\n");

  buffer[1] = 13; // Bumper
  buffer[2] = ORC_DIGITAL_IN_PULL_UP;
  fprintf(stderr, "Setting bumper 3 into digital pull-up mode... ");
  if (send_packet_and_ack(buffer, 3, ORC_MASTER) < 0) {
    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);
    return -1;
  }
  fprintf(stderr, "ok\n");

  fprintf(stderr, "Setting gripper into digital pull-up mode... ");
  buffer[1] = 12; // Gripper
  buffer[2] = ORC_DIGITAL_IN_PULL_UP;
  if (send_packet_and_ack(buffer, 3, ORC_MASTER) < 0) {
    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);
    return -1;
  }
  fprintf(stderr, "ok\n");

  buffer[0] = 0x4D;
  buffer[1] = ORC_LEFT_MOTOR;
  buffer[2] = 0;
  buffer[3] = 0;
  fprintf(stderr, "Zeroing left motor velocity ... ");
  if (send_packet_and_ack(buffer, 4, ORC_SLAVE) < 0) {
    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);
    return -1;
  }
  fprintf(stderr, "ok\n");

  buffer[1] = ORC_RIGHT_MOTOR;
  fprintf(stderr, "Zeroing right motor velocity... ");
  if (send_packet_and_ack(buffer, 4, ORC_SLAVE) < 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_arm_direct_update_status() < 0) {
    fprintf(stderr, "%sfailed%s.\n", carmen_red_code, carmen_normal_code);
    return -1;
  }
  fprintf(stderr, "ok\n");

  return 0;
}

int carmen_arm_direct_shutdown(void)
{
  unsigned char buffer[5];

  buffer[0] = 0x4D;
  buffer[1] = ORC_LEFT_MOTOR;
  buffer[2] = ORC_FORWARD;
  buffer[3] = 0;
  send_packet_and_ack(buffer, 4, ORC_SLAVE);  buffer[0] = 0x4D;

  buffer[1] = ORC_RIGHT_MOTOR;
  buffer[2] = ORC_FORWARD;
  buffer[3] = 0;
  send_packet_and_ack(buffer, 4, ORC_SLAVE);

  close(serial_fd);

  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;
      }
    }
    if (!buffer || buffer[0] != 0xED || buffer[3] != 0)
      count++;
    //    carmen_warn("Count %d\n", count);
  } while (count < 10);
  
  return buffer;

}

int carmen_arm_direct_update_status(void)
{
  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;
}

/* void carmen_arm_direct_set(double servos[], int num_servos) */
/* { */
/*   unsigned char buffer[4]; */
/*   unsigned short pwm; */
/*   int i; */

  
/* if (num_servos > 4) { */
/*     carmen_warn("orc_lib.c only supports 4 servos (%d sent)\n" */
/*                 "Returning only 4\n", num_servos); */
/*     num_servos = 4; */
/*   } */

/*  for (i=0;i<num_servos;i++) */
/*    {  */
/*      int nservo=(int)servos[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(double servos[], int num_servos, */
/* 			   double *currents, int *gripper) */
/* { */
/*   int i; */

  
/*   if (num_servos > 4) { */
/*     carmen_warn("orc_lib.c only supports 4 servos (%d requested)\n" */
/*                 "Returning only 4\n", num_servos); */
/*     num_servos = 4; */
/*   } */
 
/*   for (i = 0; i < num_servos; i++) { */
/*     servos[i] = servo_state[i]; */
/*   } */
/*   if (currents && num_servos > 0) { */
/*     currents[0] = servo_current[0]; */
/*     if (num_servos > 1) */
/*       currents[1] = servo_current[1]; */
/*   } */
/*   if (gripper) */
/*     *gripper = gripper_state; */
/* } */