orc_lib.c

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

  if (fabs(desired_angular_velocity) > .001) {
    if (desired_angular_velocity > ORC_MAX_ANGULAR_VEL)
      desired_angular_velocity = ORC_MAX_ANGULAR_VEL;
    if (desired_angular_velocity < -ORC_MAX_ANGULAR_VEL)
      desired_angular_velocity = -ORC_MAX_ANGULAR_VEL;

    velError = (desired_angular_velocity - current_angular_velocity);
    pTerm = velError * pGain;

    if (!*ignore_d_term)
      dTerm = (velError - *velErrorPrevPtr) * ORC_D_GAIN;
    else {
      dTerm = 0;
      *ignore_d_term = 0;
    }

    new_pwm = current_pwm + carmen_round(pTerm + dTerm);

    if(0)
    carmen_warn("%s %f %f : %f %f : %d %d %d\n", 
		(WHICH_MOTOR == ORC_LEFT_MOTOR ? "left" : "right"),
		desired_angular_velocity,
		current_angular_velocity, pTerm, dTerm,
		current_pwm, carmen_round(pTerm+dTerm), new_pwm);

    *velErrorPrevPtr = velError;

    if (abs(new_pwm) > ORC_MAX_PWM)
      new_pwm = (new_pwm > 0 ? ORC_MAX_PWM : -ORC_MAX_PWM);

    if (WHICH_MOTOR == ORC_RIGHT_MOTOR) {
      if (new_pwm < 0) 
	dir = ORC_FORWARD;
      else 
	dir = ORC_BACKWARD;
      //      carmen_warn("left %s\n", (dir == 1) ? "forward" : "backward");
    } else {
      if (new_pwm < 0) 
	dir = ORC_BACKWARD;
      else 
	dir = ORC_FORWARD;
      //      carmen_warn("right %s\n", (dir == 1) ? "forward" : "backward");
    }
    command_pwm = abs(new_pwm);
  } else {
    dir = ORC_FORWARD;
    command_pwm = 0;
  }

  if (0)
  carmen_warn("Tried to send %d %d %f %f %d\n", command_pwm, new_pwm,
	      desired_velocity, current_velocity, WHICH_MOTOR);

  buffer[0] = 0x4D;
  buffer[1] = WHICH_MOTOR;
  buffer[2] = dir;
  buffer[3] = command_pwm;
  send_packet_and_ack(buffer, 4, ORC_SLAVE);
}

static double delta_tick_to_metres(int delta_tick)
{
  double revolutions = (double)delta_tick/
    (double)(ORC_ENCODER_RESOLUTION*ORC_GEAR_RATIO);
  double radians = revolutions*2*M_PI;
  double metres = radians*(ORC_WHEEL_DIAMETER/2.0);

  return metres;
}

static double voltage_to_current(unsigned short voltage)
{
  double current;
  current = voltage*5.0/65536.0;
  // V=IR, R=0.18 ohm
  current = current/0.18;
  return current;
}

static int unpack_short (unsigned char *buffer, int offset)
{
  return ((buffer[offset]&0x00ff)<<8)+(buffer[offset+1]&0x00ff);
}

static void unpack_master_packet(unsigned char *buffer)
{
  int range;
  short time_ticks, delta_master_ticks;
  unsigned char command_buffer[2];
  short digital_io_state;
  unsigned short servo_pwm;
  int i;
  static double last_ping_time = 0;
  static int last_ping = ORC_RIGHT_SONAR_PING;

  //carmen_warn("Got master packet\n");
  //printf("unpack_master_packet\n");

  range = unpack_short(buffer, ORC_LEFT_SONAR_RANGE);
  if (range == 0xffff)
    left_range = 0;
  else
    left_range = range/1000000.0*331.46/2.0;

  range = unpack_short(buffer, ORC_RIGHT_SONAR_RANGE);
  if (range == 0xffff)
    right_range = 0;
  else
    right_range = range/1000000.0*331.46/2.0;

  time_ticks = buffer[ORC_MASTER_TIME];
  delta_master_ticks = time_ticks - last_master_ticks;
  if (delta_master_ticks > SHRT_MAX/2)
    delta_master_ticks -= 2*SHRT_MAX;
  if (delta_master_ticks < -SHRT_MAX/2)
    delta_master_ticks += 2*SHRT_MAX;

  last_master_ticks = time_ticks;

  if (sonar_on && carmen_get_time() - last_ping_time > .05) {
    command_buffer[0] = 'R';
    if (last_ping == ORC_LEFT_SONAR_PING)
      command_buffer[1] = ORC_RIGHT_SONAR_PING;
    else
      command_buffer[1] = ORC_LEFT_SONAR_PING;
    send_packet_and_ack(command_buffer, 2, ORC_MASTER);
    last_ping = command_buffer[1];
    last_ping_time = carmen_get_time();
  }

  digital_io_state = unpack_short(buffer, ORC_DIGITAL_IN);

  irs[0] = digital_io_state >> ORC_LEFT_IR_ECHO & 1;
  irs[1] = digital_io_state >> ORC_RIGHT_IR_ECHO & 1; // 1 means there's a curb (beyond range of IR)

  bumpers[0] = digital_io_state >> 8 & 1;
  bumpers[1] = digital_io_state >> 9 & 1;
  bumpers[2] = digital_io_state >> 10 & 1;
  bumpers[3] = digital_io_state >> 11 & 1;
  gripper_state = digital_io_state >> 12 & 1;
 
  for (i = 0; i < 4; i++) {
    servo_pwm = unpack_short(buffer, ORC_SERVO_PWM_STATE+i*2);
    servo_state[i] = servo_pwm;
  }

}

static void unpack_slave_packet(unsigned char *buffer)
{
  short left_tick, right_tick, time_ticks, delta_slave_ticks;
  int left_delta_tick, right_delta_tick;
  int left_dir, right_dir;
  unsigned char left_pinmode, right_pinmode;
  unsigned short voltage;
  static double start;
  int left_orc_encoder, right_orc_encoder;

  unsigned char left_slew, right_slew;

  //carmen_warn("Got slave packet\n");
  //printf("unpack_slave_packet\n");

  if (!initialized) {
    start = carmen_get_time();
    initialized = 1;

    x = 0;
    y = 0;
    theta = 0;

    left_last_tick = unpack_short(buffer, ORC_LEFT_MOTOR_ENCODER);
    right_last_tick = ((buffer[ORC_RIGHT_MOTOR_ENCODER]&0x00ff)<<8)+
      (buffer[ORC_RIGHT_MOTOR_ENCODER+1]&0x00ff);

    last_slave_ticks = ((buffer[ORC_SLAVE_TIME]&0x00ff)<<8)+
      (buffer[ORC_SLAVE_TIME+1]&0x00ff);
    left_pwm = buffer[ORC_LEFT_MOTOR_ACTUAL_PWM];
    right_pwm = buffer[ORC_RIGHT_MOTOR_ACTUAL_PWM];

    left_dir = ((buffer[ORC_LEFT_MOTOR_DIR] & 0x0f) >> 2) & 0x03;
    right_dir = ((buffer[ORC_RIGHT_MOTOR_DIR] & 0x0f) >> 2) & 0x03;

    if (left_dir == ORC_BACKWARD)
      left_pwm = -left_pwm;
    if (right_dir == ORC_FORWARD)
      right_pwm = -right_pwm;

    return;
  }

  left_orc_encoder = buffer[ORC_LEFT_ENCODER_STATE];
  if (left_orc_encoder != ((0x0e << 4) + 0x0e))
    carmen_warn("left encoder is in a bad state: %d %d\n", 
		(left_orc_encoder >> 4) & 0xf, (left_orc_encoder & 0xf));
  //  else
  //    carmen_warn("left encoder is in a good state: %d %d\n", 
  //		(left_orc_encoder >> 4) & 0xf, (left_orc_encoder & 0xf));
  right_orc_encoder = buffer[ORC_RIGHT_ENCODER_STATE];
  if (right_orc_encoder != ((0x0e << 4) + 0x0e))
    carmen_warn("right encoder is in a bad state: %d %d\n", 
		(right_orc_encoder >> 4) & 0xf, right_orc_encoder & 0xf);
  //  else
  //    carmen_warn("right encoder is in a good state: %d %d\n", 
  //		(right_orc_encoder >> 4) & 0xf, right_orc_encoder & 0xf);

  voltage = unpack_short(buffer, ORC_SERVO_CURRENT);
  servo_current[0] = voltage_to_current(voltage);
  voltage = unpack_short(buffer, ORC_SERVO_CURRENT+2);
  servo_current[1] = voltage_to_current(voltage);

  left_pinmode = buffer[ORC_LEFT_PINMODE];
  right_pinmode = buffer[ORC_RIGHT_PINMODE];

  left_pwm = buffer[ORC_LEFT_MOTOR_ACTUAL_PWM];
  right_pwm = buffer[ORC_RIGHT_MOTOR_ACTUAL_PWM];

  left_slew = buffer[ORC_LEFT_MOTOR_SLEW];
  right_slew = buffer[ORC_RIGHT_MOTOR_SLEW];

  if (0)
  carmen_warn("left slew: %d right slew: %d\n", left_slew,
	      right_slew);

  left_dir = ((buffer[ORC_LEFT_MOTOR_DIR] & 0x0f) >> 2) & 0x03;
  right_dir = ((buffer[ORC_RIGHT_MOTOR_DIR] & 0x0f) >> 2) & 0x03;

  if (left_dir == ORC_BACKWARD)
    left_pwm = -left_pwm;
  if (right_dir == ORC_FORWARD)
    right_pwm = -right_pwm;

  left_tick = ((buffer[ORC_LEFT_MOTOR_ENCODER]&0x00ff)<<8)+
    (buffer[ORC_LEFT_MOTOR_ENCODER+1]&0x00ff);

  left_delta_tick = left_tick - left_last_tick;
  if (left_delta_tick > SHRT_MAX/2)
    left_delta_tick = left_delta_tick - 2*SHRT_MAX;
  if (left_delta_tick < -SHRT_MAX/2)
    left_delta_tick = left_delta_tick + 2*SHRT_MAX;

  right_tick = ((buffer[ORC_RIGHT_MOTOR_ENCODER]&0x00ff)<<8)+
    (buffer[ORC_RIGHT_MOTOR_ENCODER+1]&0x00ff);
  right_delta_tick = right_tick - right_last_tick;
  if (right_delta_tick > SHRT_MAX/2)
    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;

  left_delta_tick = -left_delta_tick;

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

  if (0)
  carmen_warn("left %d %f right %d %f\n", left_delta_tick, left_displacement,
	      right_delta_tick, right_displacement);

  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);

  if (0) {
    carmen_warn("Displacement: %f rotation: %f\n", displacement,
		carmen_radians_to_degrees(rotation));
    carmen_warn("before %f %f %f\n", x, y, carmen_radians_to_degrees(theta));
  }

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

  if(0)
  carmen_warn("after %f %f %f (%f %f)\n", x, y, carmen_radians_to_degrees(theta),
	      displacement, carmen_radians_to_degrees(rotation));

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

  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();
}

int carmen_base_direct_sonar_on(void)
{
  printf("carmen_base_direct_sonar_on\n");
  sonar_on = 1;

  return 2;
}

int carmen_base_direct_sonar_off(void)
{
  sonar_on = 0;
  return 2;
}


int carmen_base_direct_reset(void)
{
  printf("carmen_base_direct_reset\n");
  x = 0;
  y = 0;
  theta = 0;

  initialized = 0;
  return 0;
}

static int orc_set_encoder_quad_phase_fast(int motor) 
{
  unsigned char buffer[5];

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

  fprintf(stderr, "Setting motor channel %d into quad phase fast mode... ",
	  motor);
  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");

  return 0;
}

static int orc_set_motor_slew(int motor, int slew) 
{
  unsigned char buffer[5];

  buffer[0] = 'W';
  buffer[1] = motor;
  buffer[2] = slew;
  fprintf(stderr, "Setting %s motor slew to %d... ", 
	  (motor == ORC_LEFT_MOTOR ? "left" : "right") , slew);
  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");

  return 0;
}

int orc_set_pin(int pin, char *description, int pin_mode) 
{
  unsigned char buffer[5];
  char *modes[15] = {"digital in", "digital in (pull-up)",
		    "digital in (pull-down)", "digital out",
		    "digital out (slow)", "servo", "sonar ping",
		    "sonar echo", "analog in", "analog out",
		    "clock generator out", "quadrature phase",
		    "mono phase", "unknown", "quad-phase fast"};
  buffer[0] = 'C';
  buffer[1] = pin; // Servo0
  buffer[2] = pin_mode; //Servo
  fprintf(stderr, "Setting %s pin %d into %s mode... ", description, pin,
	  modes[pin_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");

  return 0;
}

int orc_set_pwm(int motor, int dir, int pwm)
{
  unsigned char buffer[5];

  buffer[0] = 0x4D;