interp_convert.cc

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

   the tool point to the end point of the move.

Called by: convert_arc.

This function converts a helical or circular arc, generating only one
arc. This is called when cutter radius compensation is on and this is 
the first cut after the turning on.

The arc which is generated is derived from a second arc which passes
through the programmed end point and is tangent to the cutter at its
current location. The generated arc moves the tool so that it stays
tangent to the second arc throughout the move.

*/

int Interp::convert_arc_comp1(int move,  //!< either G_2 (cw arc) or G_3 (ccw arc)            
                             block_pointer block,       //!< pointer to a block of RS274/NGC instructions    
                             setup_pointer settings,    //!< pointer to machine settings                     
                             double end_x,      //!< x-value at end of programmed (then actual) arc  
                             double end_y,      //!< y-value at end of programmed (then actual) arc  
                             double end_z,      //!< z-value at end of arc                           
                             double AA_end,     //!< a-value at end of arc                     
                             double BB_end,     //!< b-value at end of arc                     
                              double CC_end,     //!< c-value at end of arc                     
                              double u_end, double v_end, double w_end) //!< uvw at end of arc
{
  static char name[] = "convert_arc_comp1";
  double center[2];
  double gamma;                 /* direction of perpendicular to arc at end */
  int side;                     /* offset side - right or left              */
  int status;                   /* status returned from CHP function call   */
  double tolerance;             /* tolerance for difference of radii        */
  double tool_radius;
  int turn;                     /* 1 for counterclockwise, -1 for clockwise */
  double end[3], current[3];
  int plane = settings->plane;

  side = settings->cutter_comp_side;
  tool_radius = settings->cutter_comp_radius;   /* always is positive */
  tolerance = (settings->length_units == CANON_UNITS_INCHES) ?
    TOLERANCE_INCH : TOLERANCE_MM;

  if(settings->plane == CANON_PLANE_XZ) {
      end[0] = end_x;
      end[1] = end_z;
      end[2] = end_y;
      current[0] = settings->current_x;
      current[1] = settings->current_z;
      current[2] = settings->current_y;
      if(move == G_2) move = G_3; else move = G_2;
  } else if (settings->plane == CANON_PLANE_XY) {
      end[0] = end_x;
      end[1] = end_y;
      end[2] = end_z;
      current[0] = settings->current_x;
      current[1] = settings->current_y;
      current[2] = settings->current_z;
  } else ERM(NCE_RADIUS_COMP_ONLY_IN_XY_OR_XZ);


  CHK((hypot((end[0] - current[0]),
             (end[1] - current[1])) <= tool_radius),
      NCE_CUTTER_GOUGING_WITH_CUTTER_RADIUS_COMP);

  if (block->r_flag) {
      CHP(arc_data_comp_r(move, plane, side, tool_radius, current[0],
                        current[1], end[0], end[1], block->r_number,
                        &center[0], &center[1], &turn, tolerance));
  } else {
      CHP(arc_data_comp_ijk(move, plane, side, tool_radius, current[0],
                          current[1], end[0], end[1],
                          block->i_number, 
                          settings->plane == CANON_PLANE_XZ? block->k_number: block->j_number,
                          &center[0], &center[1], &turn, tolerance));
  }

  gamma =
    (((side == LEFT) && (move == G_3)) ||
     ((side == RIGHT) && (move == G_2))) ?
    atan2((center[1] - end[1]), (center[0] - end[0])) :
    atan2((end[1] - center[1]), (end[0] - center[0]));

  settings->cutter_comp_firstmove = OFF;
  if(settings->plane == CANON_PLANE_XZ) {
    settings->program_x = end[0];
    settings->program_z = end[1];
    settings->program_y = end[2];
  } else if (settings->plane == CANON_PLANE_XY) {
    settings->program_x = end[0];
    settings->program_y = end[1];
    settings->program_z = end[2];
  }
  end[0] = (end[0] + (tool_radius * cos(gamma))); /* end_x reset actual */
  end[1] = (end[1] + (tool_radius * sin(gamma))); /* end_y reset actual */

  /* imagine a right triangle ABC with A being the endpoint of the
     compensated arc, B being the center of the compensated arc, C being
     the midpoint between start and end of the compensated arc. AB_ang
     is the direction of A->B.  A_ang is the angle of the triangle
     itself.  We need to find a new center for the compensated arc
     (point B). */

  double b_len = hypot(current[1] - end[1], current[0] - end[0]) / 2.0;
  double AB_ang = atan2(center[1] - end[1], center[0] - end[0]);
  double A_ang = atan2(current[1] - end[1], current[0] - end[0]) - AB_ang;

  CHK((fabs(cos(A_ang)) < TOLERANCE_EQUAL), NCE_CUTTER_GOUGING_WITH_CUTTER_RADIUS_COMP);
  
  double c_len = b_len/cos(A_ang);

  center[0] = end[0] + c_len * cos(AB_ang);
  center[1] = end[1] + c_len * sin(AB_ang);

  /* center to endpoint distances matched before - they still should. */

  CHK((fabs(hypot(center[0]-end[0],center[1]-end[1]) - 
            hypot(center[0]-current[0],center[1]-current[1])) > tolerance),
      NCE_BUG_IN_TOOL_RADIUS_COMP);

  if(settings->plane == CANON_PLANE_XZ) {
      if (settings->feed_mode == INVERSE_TIME)
        inverse_time_rate_straight(xtrans(settings, current[0]), current[2], ztrans(settings, current[1]),
                                   AA_end, BB_end, CC_end, u_end, v_end, w_end, block, settings);
      STRAIGHT_FEED(xtrans(settings, current[0]), current[2], ztrans(settings, current[1]),
                    AA_end, BB_end, CC_end, u_end, v_end, w_end);

      if (settings->feed_mode == INVERSE_TIME)
        inverse_time_rate_arc(current[0], current[1],
                              current[2], center[0], center[1], turn,
                              end[0], end[1], end[2], block, settings);
      ARC_FEED(ztrans(settings, end[1]), xtrans(settings, end[0]), 
               ztrans(settings, center[1]), xtrans(settings, center[0]), 
               -turn, end[2], AA_end, BB_end, CC_end, u_end, v_end, w_end);
      settings->current_x = end[0];
      settings->current_z = end[1];
      settings->current_y = end[2];
      settings->AA_current = AA_end;
      settings->BB_current = BB_end;
      settings->CC_current = CC_end;
      settings->u_current = u_end;
      settings->v_current = v_end;
      settings->w_current = w_end;
  } else if (settings->plane == CANON_PLANE_XY) {
      if (settings->feed_mode == INVERSE_TIME)
        inverse_time_rate_arc(current[0], current[1],
                              current[2], center[0], center[1], turn,
                              end[0], end[1], end[2], block, settings);
      ARC_FEED(end[0], end[1], center[0], center[1], turn, end[2],
               AA_end, BB_end, CC_end, u_end, v_end, w_end);
      settings->current_x = end[0];
      settings->current_y = end[1];
      settings->current_z = end[2];
      settings->AA_current = AA_end;
      settings->BB_current = BB_end;
      settings->CC_current = CC_end;
      settings->u_current = u_end;
      settings->v_current = v_end;
      settings->w_current = w_end;
  }

  return INTERP_OK;
}

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

/*! convert_arc_comp2

Returned Value: int
   If arc_data_ijk or arc_data_r returns an error code,
   this returns that code.
   If any of the following errors occurs, this returns the error code shown.
   Otherwise, it returns INTERP_OK.
   1. A concave corner is found: NCE_CONCAVE_CORNER_WITH_CUTTER_RADIUS_COMP
   2. The tool will not fit inside an arc:
      NCE_TOOL_RADIUS_NOT_LESS_THAN_ARC_RADIUS_WITH_COMP

Side effects:
   This executes an arc command feed rate. If needed, at also generates
   an arc to go around a convex corner. It also updates the setting of
   the position of the tool point to the end point of the move. If
   inverse time feed rate mode is in effect, the feed rate is reset.

Called by: convert_arc.

This function converts a helical or circular arc. The axis must be
parallel to the z-axis. This is called when cutter radius compensation
is on and this is not the first cut after the turning on.

If one or more rotary axes is moved in this block and an extra arc is
required to go around a sharp corner, all the rotary axis motion
occurs on the main arc and none on the extra arc.  An alternative
might be to distribute the rotary axis motion over the extra arc and
the programmed arc in proportion to their lengths.

If the Z-axis is moved in this block and an extra arc is required to
go around a sharp corner, all the Z-axis motion occurs on the main arc
and none on the extra arc.  An alternative might be to distribute the
Z-axis motion over the extra arc and the main arc in proportion to
their lengths.

*/

int Interp::convert_arc_comp2(int move,  //!< either G_2 (cw arc) or G_3 (ccw arc)          
                             block_pointer block,       //!< pointer to a block of RS274/NGC instructions  
                             setup_pointer settings,    //!< pointer to machine settings                   
                             double end_x,      //!< x-value at end of programmed (then actual) arc
                             double end_y,      //!< y-value at end of programmed (then actual) arc
                             double end_z,      //!< z-value at end of arc                         
                             double AA_end,     //!< a-value at end of arc
                             double BB_end,     //!< b-value at end of arc
                              double CC_end,     //!< c-value at end of arc
                              double u, double v, double w) //!< uvw at end of arc
{
  static char name[] = "convert_arc_comp2";
  double alpha;                 /* direction of tangent to start of arc */
  double arc_radius;
  double beta;                  /* angle between two tangents above */
  double center[2];              /* center of arc */
  double delta;                 /* direction of radius from start of arc to center of arc */
  double gamma;                 /* direction of perpendicular to arc at end */
  double mid[2];
  int side;
  double small = TOLERANCE_CONCAVE_CORNER;      /* angle for testing corners */
  double start[2];
  int status;                   /* status returned from CHP function call     */
  double theta;                 /* direction of tangent to last cut */
  double tolerance;
  double tool_radius;
  int turn;                     /* number of full or partial circles CCW */
  double end[3], current[3];
  int plane = settings->plane;

/* find basic arc data: center_x, center_y, and turn */

  if(settings->plane == CANON_PLANE_XZ) {
    start[0] = settings->program_x;
    start[1] = settings->program_z;
    end[0] = end_x;
    end[1] = end_z;
    end[2] = end_y;
    current[0] = settings->current_x;
    current[1] = settings->current_z;
    current[2] = settings->current_y;
    if(move == G_2) move = G_3; else move = G_2;
  } else if (settings->plane == CANON_PLANE_XY) {
    start[0] = settings->program_x;
    start[1] = settings->program_y;
    end[0] = end_x;
    end[1] = end_y;
    end[2] = end_z;
    current[0] = settings->current_x;
    current[1] = settings->current_y;
    current[2] = settings->current_z;
  } else ERM(NCE_RADIUS_COMP_ONLY_IN_XY_OR_XZ);

  tolerance = (settings->length_units == CANON_UNITS_INCHES) ?
    TOLERANCE_INCH : TOLERANCE_MM;

  if (block->r_flag) {
      CHP(arc_data_r(move, plane, start[0], start[1], end[0], end[1],
                   block->r_number, &center[0], &center[1], &turn, tolerance));
  } else {
      CHP(arc_data_ijk(move, plane,
                     start[0], start[1], end[0], end[1],
                     block->i_number,
                     settings->plane == CANON_PLANE_XZ? block->k_number: block->j_number,
                     &center[0], &center[1], &turn, tolerance));
  }

/* compute other data */
  side = settings->cutter_comp_side;
  tool_radius = settings->cutter_comp_radius;   /* always is positive */
  arc_radius = hypot((center[0] - end[0]), (center[1] - end[1]));
  theta = atan2(current[1] - start[1], current[0] - start[0]);
  theta = (side == LEFT) ? (theta - M_PI_2l) : (theta + M_PI_2l);
  delta = atan2(center[1] - start[1], center[0] - start[0]);
  alpha = (move == G_3) ? (delta - M_PI_2l) : (delta + M_PI_2l);
  beta = (side == LEFT) ? (theta - alpha) : (alpha - theta);
  beta = (beta > (1.5 * M_PIl)) ? (beta - (2 * M_PIl)) :
    (beta < -M_PI_2l) ? (beta + (2 * M_PIl)) : beta;

  if (((side == LEFT) && (move == G_3)) || ((side == RIGHT) && (move == G_2))) {
    gamma = atan2((center[1] - end[1]), (center[0] - end[0]));
    CHK((arc_radius <= tool_radius),
        NCE_TOOL_RADIUS_NOT_LESS_THAN_ARC_RADIUS_WITH_COMP);
  } else {
    gamma = atan2((end[1] - center[1]), (end[0] - center[0]));
    delta = (delta + M_PIl);
  }

  if(settings->plane == CANON_PLANE_XZ) {
    settings->program_x = end[0];
    settings->program_z = end[1];
    settings->program_y = end[2];
  } else if (settings->plane == CANON_PLANE_XY) {
    settings->program_x = end[0];
    settings->program_y = end[1];
    settings->program_z = end[2];
  }
  end[0] = (end[0] + (tool_radius * cos(gamma))); /* end_x reset actual */
  end[1] = (end[1] + (tool_radius * sin(gamma))); /* end_y reset actual */

/* check if extra arc needed and insert if so */

  CHK(((beta < -small) || (beta > (M_PIl + small))),
      NCE_CONCAVE_CORNER_WITH_CUTTER_RADIUS_COMP);
  if (beta > small) {           /* two arcs needed */
    mid[0] = (start[0] + (tool_radius * cos(delta)));
    mid[1] = (start[1] + (tool_radius * sin(delta)));
    if (settings->feed_mode == INVERSE_TIME)
      inverse_time_rate_arc2(start[0], start[1], (side == LEFT) ? -1 : 1,
                             mid[0], mid[1], center[0], center[1], turn,
                             end[0], end[1], end[2], block, settings);
    if(settings->plane == CANON_PLANE_XZ) {
      ARC_FEED(ztrans(settings, mid[1]), xtrans(settings, mid[0]), ztrans(settings, start[1]), xtrans(settings, start[0]),
               ((side == LEFT) ? 1 : -1),
               current[2], AA_end, BB_end, CC_end, u, v, w);
      ARC_FEED(ztrans(settings, end[1]), xtrans(settings, end[0]), ztrans(settings, center[1]), xtrans(settings, center[0]),
               -turn, end[2], AA_end, BB_end, CC_end, u, v, w);
    } else if (settings->plane == CANON_PLANE_XY) {
      ARC_FEED(mid[0], mid[1], start[0], start[1], ((side == LEFT) ? -1 : 1),
               current[2],
               AA_end, BB_end, CC_end, u, v, w);
      ARC_FEED(end[0], end[1], center[0], center[1], turn, end[2],
               AA_end, BB_end, CC_end, u, v, w);
    }
  } else {                      /* one arc needed */
    if (settings->feed_mode == INVERSE_TIME)
      inverse_time_rate_arc(current[0], current[1],
                            current[2], center[0], center[1], turn,
                            end[0], end[1], end[2], block, settings);