orc_arm_lib.c

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

    dTerm = (velError - *velErrorPrevPtr) * ORC_D_GAIN;

    current_pwm = (int)(ffTerm + pTerm + *iTermPtr + dTerm);

    *velErrorPrevPtr = velError;

    if (abs(current_pwm) > ORC_MAX_PWM)
      current_pwm = (current_pwm > 0 ? ORC_MAX_PWM : -ORC_MAX_PWM);
    if (WHICH_MOTOR == ORC_LEFT_MOTOR)
      current_pwm = -current_pwm;

    if (current_pwm < 0) {
      command_pwm = -current_pwm;
      dir = ORC_BACKWARD;
    } else {
      command_pwm = current_pwm;
      dir = ORC_FORWARD;
    }
  } else {
    dir = ORC_FORWARD;
    command_pwm = 0;
    *iTermPtr = 0.0;
  }

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


// velezj: rssII Arm
unsigned short gripper_theta_to_pwm( double theta ) {
  if( theta < 0 ) {
    carmen_warn( "gripper angles can ONLY be positive <= PI radians " );
    return 0;
  }
  
  return ORC_ARM_GRIPPER_MIN_PWM + theta * ORC_ARM_GRIPPER_PWM_PER_RADIAN;
}


// velezj: rssII Arm
double gripper_pwm_to_theta( unsigned short pwm ) {
  return (double)( pwm - ORC_ARM_GRIPPER_MIN_PWM ) / ORC_ARM_GRIPPER_PWM_PER_RADIAN;
}


// velezj: rssII Arm
void carmen_arm_reset() {
  shoulder_iTerm = 0.0;
  elbow_iTerm = 0.0;
}


// velezj: rssII Arm
void carmen_arm_direct_set( double shoulder_desired_angle, double elbow_desired_angle, double gripper_desired_angle ) {

  // reset the iterms of the vel first
  carmen_arm_reset();

  shoulder_desired_theta = shoulder_desired_angle;
  elbow_desired_theta = elbow_desired_angle;
  gripper_desired_theta = gripper_desired_angle;

  // send the gripper angle as a servo command to orc
  unsigned char buffer[4];
  unsigned short pwm = gripper_theta_to_pwm( gripper_desired_angle );
  buffer[0] = 0x53;
  buffer[1] = ORC_ARM_GRIPPER_SERVO;
  buffer[2] = pwm >> 8;
  buffer[3] = pwm & 0x00ff;
  send_packet_and_ack(buffer, 4, ORC_MASTER);
  
  printf( "\ncarmen_arm_direct_set: shoulder=%f  elbow=%f  gripper=%f \n", shoulder_desired_angle, elbow_desired_angle, gripper_desired_angle );
}


void carmen_arm_get_theta_state( double *shoulder_angle, double *elbow_angle, double *gripper_angle ) {
  *shoulder_angle = shoulder_theta;
  *elbow_angle = elbow_theta;
  *gripper_angle = gripper_theta;
}


void carmen_arm_get_state( double *shoulder_avel, double *elbow_avel ) {
  *shoulder_avel = shoulder_angular_velocity;
  *elbow_avel = elbow_angular_velocity;
}


// velezj: rssII Arm
static void carmen_arm_command_angular_velocity( double desired_angular_velocity, double current_angular_velocity, int current_pwm, unsigned char WHICH_MOTOR ) {
  
  double ffTerm = 0.0, pTerm = 0.0, dTerm = 0.0, velError = 0.0;
  double * iTermPtr;
  double * velErrorPrevPtr;;
  unsigned char command_pwm;
  unsigned char dir;
  unsigned char buffer[4];
  char which;
  int motor_direction_sign = 1;
  int max_pwm, min_pwm;

  if (WHICH_MOTOR == ORC_SHOULDER_MOTOR) {
    which = 'S';
    iTermPtr = &shoulder_iTerm;
    velErrorPrevPtr = &shoulder_error_prev;
    motor_direction_sign = ORC_SHOULDER_MOTOR_DIRECTION_SIGN;
    max_pwm = ORC_SHOULDER_MAX_PWM;
    min_pwm = ORC_SHOULDER_MIN_PWM;
  }
  else {
    which = 'E';
    iTermPtr = &elbow_iTerm;
    velErrorPrevPtr = &elbow_error_prev;
    motor_direction_sign = ORC_ELBOW_MOTOR_DIRECTION_SIGN;
    max_pwm = ORC_ELBOW_MAX_PWM;
    min_pwm = ORC_ELBOW_MIN_PWM;
  }

  if (fabs(desired_angular_velocity) > .0005) {
    if (desired_angular_velocity > ORC_ARM_MAX_ANGULAR_VEL)
      desired_angular_velocity = ORC_ARM_MAX_ANGULAR_VEL;
    if (desired_angular_velocity < -ORC_ARM_MAX_ANGULAR_VEL)
      desired_angular_velocity = -ORC_ARM_MAX_ANGULAR_VEL;

    velError = (desired_angular_velocity - current_angular_velocity);
    ffTerm = desired_angular_velocity * ORC_ARM_FF_GAIN;
    pTerm = velError * ORC_ARM_P_GAIN;
    *iTermPtr += velError * ORC_ARM_I_GAIN;
    dTerm = (velError - *velErrorPrevPtr) * ORC_ARM_D_GAIN;

    current_pwm = (int)(ffTerm + pTerm + *iTermPtr + dTerm);

    *velErrorPrevPtr = velError;

    if (abs(current_pwm) > max_pwm)
      current_pwm = (current_pwm > 0 ? max_pwm : -max_pwm);
    if( abs( current_pwm) < min_pwm )
      current_pwm = ( current_pwm > 0 ? min_pwm : -min_pwm );
    if (WHICH_MOTOR == ORC_SHOULDER_MOTOR)
      current_pwm = -current_pwm;

    current_pwm *= motor_direction_sign;

    if (current_pwm < 0) {
      command_pwm = -current_pwm;
      dir = ORC_BACKWARD;
    } else {
      command_pwm = current_pwm;
      dir = ORC_FORWARD;
    }
  } else {
    dir = ORC_FORWARD;
    command_pwm = 0;
    *iTermPtr = 0.0;
  }

  // debug
  if( command_pwm != 0 ) {
    printf( "[ %c ] p: %f  i: %f  d: %f   ve: %f\n", which, pTerm, *iTermPtr, dTerm, velError );  
    printf( "[ %c ] sending PWM: %d\n", which, ( dir == ORC_BACKWARD ? -command_pwm : command_pwm ) );
  }

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


// velezj: rssII Arm
void bound_value( double *vPtr, double min, double max ) {
  if( *vPtr < min )
    *vPtr = min;
  if( *vPtr > max )
    *vPtr = max;
}


// velezj: rssII Arm
double sign( double v ) {
  if( v < 0.0 )
    return -1.0;
  return 1.0;
}


// velezj: rssII Arm
void carmen_arm_control() {

  double shoulder_theta_delta = shoulder_desired_theta - shoulder_theta;
  double elbow_theta_delta = elbow_desired_theta - elbow_theta;
  
  double pTerm = 0.0, dTerm = 0.0;
  double *iTermPtr;

  // shoulder
  iTermPtr = &shoulder_theta_iTerm;
  //printf( "control: shoulder diff: %f    ( %f  -  %f )\n", shoulder_theta_delta, shoulder_desired_theta, shoulder_theta );
  if( fabs( shoulder_theta_delta ) > 0.01 ) {
    pTerm = shoulder_theta_delta * ORC_ARM_THETA_P_GAIN;
    *iTermPtr += ( shoulder_theta_delta * ORC_ARM_THETA_I_GAIN );
    dTerm = ( shoulder_theta_delta - shoulder_theta_error_prev ) * ORC_ARM_THETA_D_GAIN;
    shoulder_desired_angular_velocity = ( pTerm + *iTermPtr * dTerm );
    bound_value( &shoulder_desired_angular_velocity, -ORC_ARM_MAX_ANGULAR_VEL, ORC_ARM_MAX_ANGULAR_VEL );
    if( fabs( shoulder_desired_angular_velocity ) < ORC_ARM_MIN_ANGULAR_VEL ) {
      shoulder_desired_angular_velocity = sign( shoulder_desired_angular_velocity ) * ORC_ARM_MIN_ANGULAR_VEL;
    }

    printf( "contorl: shoulder: p: %f   i: %f   d: %f    av: %f\n", pTerm, *iTermPtr, dTerm, shoulder_desired_angular_velocity );
  } else {
    shoulder_desired_angular_velocity = 0.0;
    *iTermPtr = 0.0;
    //printf( "control: shoulder: done!\n" );
  }

  //elbow
  iTermPtr = &elbow_theta_iTerm;
  //printf( "control: elbow diff: %f    ( %f  -  %f )\n", elbow_theta_delta, elbow_desired_theta, elbow_theta );
  if( fabs( elbow_theta_delta ) > 0.01 ) {
    pTerm = elbow_theta_delta * ORC_ARM_THETA_P_GAIN;
    *iTermPtr += ( elbow_theta_delta * ORC_ARM_THETA_I_GAIN );
    dTerm = ( elbow_theta_delta - elbow_theta_error_prev ) * ORC_ARM_THETA_D_GAIN;
    elbow_desired_angular_velocity = ( pTerm + *iTermPtr * dTerm );
    bound_value( &elbow_desired_angular_velocity, -ORC_ARM_MAX_ANGULAR_VEL, ORC_ARM_MAX_ANGULAR_VEL );
    if( fabs( elbow_desired_angular_velocity ) < ORC_ARM_MIN_ANGULAR_VEL ) {
      elbow_desired_angular_velocity = sign( elbow_desired_angular_velocity ) * ORC_ARM_MIN_ANGULAR_VEL;
    }

    printf( "contorl: elbow: p: %f   i: %f   d: %f    av: %f\n", pTerm, *iTermPtr, dTerm, elbow_desired_angular_velocity );
  } else {
    elbow_desired_angular_velocity = 0.0;
    *iTermPtr = 0.0;
    //printf( "control: elbow: done!\n" );
  }

  carmen_arm_command_angular_velocity( shoulder_desired_angular_velocity, shoulder_angular_velocity, shoulder_pwm, ORC_SHOULDER_MOTOR );
  carmen_arm_command_angular_velocity( elbow_desired_angular_velocity, elbow_angular_velocity, elbow_pwm, ORC_ELBOW_MOTOR );
  //command_velocity( shoulder_desired_angular_velocity * ( ORC_WHEEL_DIAMETER / 2.0 ), shoulder_angular_velocity * ( ORC_WHEEL_DIAMETER / 2.0 ), shoulder_pwm, ORC_LEFT_MOTOR );
} 


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


// velezj: for RssII Arm
static double delta_ticks_to_angle( int delta_ticks ) 
{
  double radians = (double)delta_ticks / (double)ORC_ARM_TICKS_PER_RADIAN;
  return radians;
}


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;
  char command_buffer[2];
  short bumper_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_ECHO);
  if (range == 0xffff)
    left_range = 0;
  else
    left_range = range/1000000.0*331.46/2.0;

  range = unpack_short(buffer, ORC_RIGHT_SONAR_ECHO);
  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();
  }

  bumper_state = unpack_short(buffer, ORC_DIGITAL_IN);

  bumpers[0] = bumper_state >> 8 & 1;
  bumpers[1] = bumper_state >> 9 & 1;
  bumpers[2] = bumper_state >> 10 & 1;
  bumpers[3] = bumper_state >> 11 & 1;
  gripper_state = bumper_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;
  }

  // velezj: rssII Arm
  gripper_theta = gripper_pwm_to_theta( unpack_short( buffer, ORC_SERVO_PWM_STATE + ORC_ARM_GRIPPER_SERVO * 2 ) );

}

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

  // velezj: RssII Arm
  short shoulder_tick, elbow_tick;
  int shoulder_delta_tick, elbow_delta_tick;
  int shoulder_dir, elbow_dir;
  double shoulder_angle_change, elbow_angle_change;

  //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_FORWARD)
      left_pwm = -left_pwm;
    if (right_dir == ORC_BACKWARD)
      right_pwm = -right_pwm;

    // velezj: RssII Arm
    shoulder_theta = 0.0;
    elbow_theta = 0.0;
    shoulder_last_tick = unpack_short( buffer, ORC_SHOULDER_MOTOR_ENCODER );
    elbow_last_tick = unpack_short( buffer, ORC_ELBOW_MOTOR_ENCODER );
    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;

    return;
  }

  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_dir = ((buffer[ORC_LEFT_MOTOR_DIR] & 0x0f) >> 2) & 0x03;
  right_dir = ((buffer[ORC_RIGHT_MOTOR_DIR] & 0x0f) >> 2) & 0x03;

  if (left_dir == ORC_FORWARD)
    left_pwm = -left_pwm;
  if (right_dir == ORC_BACKWARD)
    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)