interp_find.cc

来自「CNC 的开放码,EMC2 V2.2.8版」· CC 代码 · 共 412 行 · 第 1/2 页

    *u_p = (block->u_flag == ON) ? block->u_number : settings->u_current;
    *v_p = (block->v_flag == ON) ? block->v_number : settings->v_current;
    *w_p = (block->w_flag == ON) ? block->w_number : settings->w_current;
  } else {                      /* mode is MODE_INCREMENTAL */

    *px = (block->x_flag == ON)
      ? ((comp
          && middle) ? (block->x_number +
                        settings->program_x) : (block->x_number +
                                                settings->current_x))
      : ((comp && middle) ? settings->program_x : settings->current_x);

    *py = (block->y_flag == ON)
      ? ((comp
          && middle && settings->plane == CANON_PLANE_XY ) ? (block->y_number +
                        settings->program_y) : (block->y_number +
                                                settings->current_y))
      : ((comp && middle) ? settings->program_y : settings->current_y);

    *pz = (block->z_flag == ON)
      ? ((comp
          && middle && settings->plane == CANON_PLANE_XZ ) ? (block->z_number +
                        settings->program_z) : (block->z_number +
                                                settings->current_z))
      : ((comp && middle) ? settings->program_z : settings->current_z);
    *AA_p = (block->a_flag == ON) ?
      (settings->AA_current + block->a_number) : settings->AA_current;
    *BB_p =
      (block->b_flag ==
       ON) ? (settings->BB_current + block->b_number) : settings->BB_current;
    *CC_p =
      (block->c_flag ==
       ON) ? (settings->CC_current + block->c_number) : settings->CC_current;
    *u_p = (block->u_flag == ON) ?
      (settings->u_current + block->u_number) : settings->u_current;
    *v_p =
      (block->v_flag ==
       ON) ? (settings->v_current + block->v_number) : settings->v_current;
    *w_p =
      (block->w_flag ==
       ON) ? (settings->w_current + block->w_number) : settings->w_current;
  }
  return INTERP_OK;
}

/****************************************************************************/

/*! find_relative

Returned Value: int (INTERP_OK)

Side effects:
   The values of x2, y2, z2, aa_2, bb_2, and cc_2 are set.
   (NOTE: aa_2 etc. are written with lower case letters in this
    documentation because upper case would confuse the pre-preprocessor.)

Called by:
   convert_home

This finds the coordinates in the current system, under the current
tool length offset, of a point (x1, y1, z1, aa_1, bb_1, cc_1) whose absolute
coordinates are known.

Don't confuse this with the inverse operation.

*/

int Interp::find_relative(double x1,     //!< absolute x position        
                          double y1,     //!< absolute y position        
                          double z1,     //!< absolute z position        
                          double AA_1,   //!< absolute a position         
                          double BB_1,   //!< absolute b position         
                          double CC_1,   //!< absolute c position         
                          double u_1,
                          double v_1,
                          double w_1,
                          double *x2,    //!< pointer to relative x      
                          double *y2,    //!< pointer to relative y      
                          double *z2,    //!< pointer to relative z      
                          double *AA_2,  //!< pointer to relative a       
                          double *BB_2,  //!< pointer to relative b       
                          double *CC_2,  //!< pointer to relative c       
                          double *u_2,
                          double *v_2,
                          double *w_2,
                          setup_pointer settings)        //!< pointer to machine settings
{
  *x2 = (x1 - (settings->tool_xoffset + settings->origin_offset_x + settings->axis_offset_x));
  *y2 = (y1 - (settings->origin_offset_y + settings->axis_offset_y));
  *z2 =
    (z1 -
     (settings->tool_zoffset + settings->origin_offset_z +
      settings->axis_offset_z));
  *AA_2 = (AA_1 - (settings->AA_origin_offset + settings->AA_axis_offset));
  *BB_2 = (BB_1 - (settings->BB_origin_offset + settings->BB_axis_offset));
  *CC_2 = (CC_1 - (settings->CC_origin_offset + settings->CC_axis_offset));
  *u_2 = (u_1 - (settings->u_origin_offset + settings->u_axis_offset));
  *v_2 = (v_1 - (settings->v_origin_offset + settings->v_axis_offset));
  *w_2 = (w_1 - (settings->w_origin_offset + settings->w_axis_offset));
  return INTERP_OK;
}

/****************************************************************************/

/*! find_straight_length

Returned Value: double (length of path between start and end points)

Side effects: none

Called by:
  inverse_time_rate_straight
  inverse_time_rate_as

This calculates a number to use in feed rate calculations when inverse
time feed mode is used, for a motion in which X,Y,Z,A,B, and C each change
linearly or not at all from their initial value to their end value.

This is used when the feed_reference mode is CANON_XYZ, which is
always in rs274NGC.

If any of the X, Y, or Z axes move or the A-axis, B-axis, and C-axis
do not move, this is the length of the path relative to the XYZ axes
from the first point to the second, and any rotary axis motion is
ignored. The length is the simple Euclidean distance.

The formula for the Euclidean distance "length" of a move involving
only the A, B and C axes is based on a conversation with Jim Frohardt at
Boeing, who says that the Fanuc controller on their 5-axis machine
interprets the feed rate this way. Note that if only one rotary axis
moves, this formula returns the absolute value of that axis move,
which is what is desired.

*/

double Interp::find_straight_length(double x2,   //!< X-coordinate of end point   
                                    double y2,   //!< Y-coordinate of end point   
                                    double z2,   //!< Z-coordinate of end point   
                                    double AA_2, //!< A-coordinate of end point    
                                    double BB_2, //!< B-coordinate of end point    
                                    double CC_2, //!< C-coordinate of end point    
                                    double u_2,
                                    double v_2,
                                    double w_2,
                                    double x1,   //!< X-coordinate of start point 
                                    double y1,   //!< Y-coordinate of start point 
                                    double z1,    //!< Z-coordinate of start point 
                                    double AA_1,        //!< A-coordinate of start point  
                                    double BB_1,        //!< B-coordinate of start point  
                                    double CC_1,        //!< C-coordinate of start point  
                                    double u_1,
                                    double v_1,
                                    double w_1
  )
{
    if ((x1 != x2) || (y1 != y2) || (z1 != z2))
        return sqrt(pow((x2 - x1), 2) + pow((y2 - y1), 2) + pow((z2 - z1), 2));
    else if ((u_1 != u_2) || (v_1 != v_2) || (w_1 != w_2))
        return sqrt(pow((u_2 - u_1), 2) + pow((v_2 - v_1), 2) + pow((w_2 - w_1), 2));
    else
        return sqrt(pow((AA_2 - AA_1), 2) + pow((BB_2 - BB_1), 2) + pow((CC_2 - CC_1), 2));
}

/****************************************************************************/

/*! find_turn

Returned Value: double (angle in radians between two radii of a circle)

Side effects: none

Called by: find_arc_length

All angles are in radians.

*/

double Interp::find_turn(double x1,      //!< X-coordinate of start point       
                        double y1,      //!< Y-coordinate of start point       
                        double center_x,        //!< X-coordinate of arc center        
                        double center_y,        //!< Y-coordinate of arc center        
                        int turn,       //!< no. of full or partial circles CCW
                        double x2,      //!< X-coordinate of end point         
                        double y2)      //!< Y-coordinate of end point         
{
  double alpha;                 /* angle of first radius */
  double beta;                  /* angle of second radius */
  double theta;                 /* amount of turn of arc CCW - negative if CW */

  if (turn == 0)
    return 0.0;
  alpha = atan2((y1 - center_y), (x1 - center_x));
  beta = atan2((y2 - center_y), (x2 - center_x));
  if (turn > 0) {
    if (beta <= alpha)
      beta = (beta + (2 * M_PIl));
    theta = ((beta - alpha) + ((turn - 1) * (2 * M_PIl)));
  } else {                      /* turn < 0 */

    if (alpha <= beta)
      alpha = (alpha + (2 * M_PIl));
    theta = ((beta - alpha) + ((turn + 1) * (2 * M_PIl)));
  }
  return (theta);
}