orc_arm_lib.c

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

      // update the delta error
      s_arm_error_prev[i] = theta_delta;

    } else {
      desired_angular_velocity = 0.0;
      command_pwm = 0.0;
      *iTermPtr = 0.0;   
    }

    // set the values into our array
    velocity_command_set[i] = desired_angular_velocity;
    pwm_command_set[i] = command_pwm;

  }

  // actually command the velocities
  for( int i = 0; i < s_num_joints; ++i ){
    // velocity_command_set( desired_vel[i], s_arm_angular_velocity[i], MOTOR_PORTMAP[i] );
 
    // for now, don't include the command_angular_velocity function and do this directly
    //printf("---------------- actually commanded port[%d]= %d to do pwm=%d\n",
    //	   i,MOTOR_PORTMAP[i],pwm_command_set[i]);
    orc_motor_set_signed(s_orc, MOTOR_PORTMAP[i], pwm_command_set[i]);

  }

  printf( "\n" );


}

// ---- GETS ---- //
void carmen_arm_direct_get_state(double *joint_angles, double *joint_currents,
				 double *joint_angular_vels, int *gripper_closed ){
  int i;

  update_internal_data();

  // put values into the output from what we have in here
   for( i = 0; i < s_num_joints; ++i ){
     joint_angles[i] = s_arm_theta[i]; 
     joint_currents[i] = s_arm_current[i];
     joint_angular_vels[i] = s_arm_angular_velocity[i];
   }
 
  // for now, since the gripper's not being used
   if (gripper_closed)
     *gripper_closed = 0;
}


// velezj: rssII Arm
//static void command_angular_velocity( double desired_angular_velocity, 
void command_angular_velocity( double desired_angular_velocity, 
				      double current_angular_velocity, int joint ) {
  
  double ffTerm = 0.0, pTerm = 0.0, dTerm = 0.0, velError = 0.0;
  double * iTermPtr;
  double * velErrorPrevPtr;;
  int current_pwm;
  int command_pwm;
  int max_pwm, min_pwm;

  // get base parameters for your joint motor
  iTermPtr = &s_arm_iTerm[joint];
  velErrorPrevPtr = &s_arm_error_prev[joint];
  max_pwm = s_max_pwm[joint];
  min_pwm = s_min_pwm[joint];


  //printf("*********************************\n");
  //printf("global max pwm for joint %d: %d\n", joint, max_pwm);
  //printf("global min pwm for joint %d: %d\n", joint, min_pwm);
 
  // if there is non-zero desired velocity
  if (fabs(desired_angular_velocity) > .0005) {

    // compute PID terms
    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;
    *velErrorPrevPtr = velError;

    // set the pwm between the desired bounds
    current_pwm = (int)(ffTerm + pTerm + *iTermPtr + dTerm);
    if (abs(current_pwm) > max_pwm){
      //printf("In max\n");
      //printf("Current_pwm was: %d\n", current_pwm);
      current_pwm = (current_pwm > 0 ? max_pwm : -max_pwm);
      //printf("Current_pwm is now: %d\n", current_pwm);
    }
    if( abs( current_pwm) < min_pwm ){
      //printf("In min\n");
      current_pwm = ( current_pwm > 0 ? min_pwm : -min_pwm );}

    command_pwm = abs( current_pwm );
    //printf("command_pwm is now: %d\n", current_pwm);
  } else {
    command_pwm = 0;
    *iTermPtr = 0.0;
  } // end if motion desired

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

  //printf("Current Angular Velocity %f\n", current_angular_velocity);
  //printf("Desired Angular Velocity %f\n", desired_angular_velocity);
  //printf("command_pwm %d\n", command_pwm);

  orc_motor_set_signed(s_orc, MOTOR_PORTMAP[joint], command_pwm);
}


// ---- HELPER FUNCTIONS ---- //

// this reads relevant arm sensors and updates static values
static void update_internal_data(void)
{

  double curr_time = carmen_get_time();
  
  // we'll need this to update elbow state after the loop
  double shoulder_delta_theta = 0.0;

  // updates arm angles, velocities, and currents
  for( int i = 0; i < s_num_joints; ++i ){
    int port = MOTOR_PORTMAP[i];
    int curr_tick_count = orc_quadphase_read( s_orc, ENCODER_PORTMAP[i] );
    int prev_tick_count = s_arm_tick[i];

    // compute the change in angle since the last update
    double ticks_to_radian = 1/(double)TICKS_PER_RADIAN[i];
    double delta_theta = compute_delta_theta( curr_tick_count, prev_tick_count,
					      ticks_to_radian );
    delta_theta = REVERSE_THETA[i] * delta_theta;

    //printf("* Internal Update: port[%d]=%d, old theta was %f, d theta is %f curr_tick=%d prev_tick=%d\n",
    //   i,ENCODER_PORTMAP[i],s_arm_theta[i],delta_theta,curr_tick_count,prev_tick_count);
  
    // set variables
    s_arm_theta[i] = carmen_normalize_theta( delta_theta + s_arm_theta[i] );
    s_arm_angular_velocity[i] = delta_theta / ( curr_time - s_time );
    s_arm_current[i] = (double)orc_analog_read( s_orc, 16 + port );  // motor ports are 16 + reg
    s_arm_tick[i] = curr_tick_count;

    // check if this is the shoulder
    if( i == SHOULDER ){
      shoulder_delta_theta = delta_theta;
    }
  }			   
				
  // correct for the fact that when the shoulder moves, the elbow angle also changes
  s_arm_theta[ELBOW] = s_arm_theta[ELBOW] - shoulder_delta_theta;
 
  // update time
  s_delta_time = curr_time - s_time;
  s_time = curr_time;  
}

// we assume the direction is the smallest route around the circle
static double compute_delta_theta( int curr, int prev, double ticks_to_radian )
{

  // compute the number of ticks traversed short and long way around
  // and pick the path with the minimal distance
  int abs_reg = abs( curr - prev );
  int abs_opp = 65536 - abs_reg;
  int actual_delta = min( abs_reg, abs_opp );
  double theta = (double)actual_delta * ticks_to_radian;

  // give the angle the correct sign -- ccw is positive
  if( ( curr > prev && actual_delta == abs_reg ) || 
      ( curr < prev && actual_delta == abs_opp ) )
    {
      theta = -theta;
    }
  
  return theta;
}

static int min( int a, int b ){
  return ( a < b ) ? a : b ;
}

static double d_sign( double v ) {
  if( v < 0.0 )
    return -1.0;
  return 1.0;
}

int i_sign( int v ) {
  if( v < 0 )
    return -1;
  return 1;
}

static void d_bound_value( double *vPtr, double min, double max ) {
  if( *vPtr < min )
    *vPtr = min;
  if( *vPtr > max )
    *vPtr = max;
}

static void i_bound_value( int *vPtr, int min, int max ) {
  if( *vPtr < min )
    *vPtr = min;
  if( *vPtr > max )
    *vPtr = max;
}



/*

  unsigned char *buffer;
  double start_time = carmen_get_time();

  if (!initialized) {
    start = carmen_get_time();
    initialized = 1;
    // velezj: RssII Arm
    shoulder_theta = 0.0;
    elbow_theta = 0.0;
    shoulder_last_tick = orc_quadphase_read(orc, ORC_SHOULDER_MOTOR_ENCODER);
    elbow_last_tick = orc_quadphase_read(orc, ORC_ELBOW_MOTOR_ENCODER);

    shoulder_pwm = orc_analog_read(orc, ORC_SHOULDER_MOTOR_ACTUAL_PWM);
    elbow_pwm = orc_analog_read(orc, ORC_SHOULDER_MOTOR_ACTUAL_PWM);

    return;
  }

  shoulder_pwm = orc_analog_read(orc, ORC_SHOULDER_MOTOR_ACTUAL_PWM);
  elbow_pwm = orc_analog_read(orc, ORC_ELBOW_MOTOR_ACTUAL_PWM);

  shoulder_tick = orc_quadphase_read(orc, ORC_SHOULDER_MOTOR_ENCODER);
  elbow_tick = orc_quadphase_read(orc, ORC_ELBOW_MOTOR_ENCODER);
  
  // velezj: For RssII Arm

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

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

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

  free(buffer);

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

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


// velezj: rssII Arm


// velezj: rssII Arm

*/