rs274ngc_pre.cc

数控系统中的解释器源代码

      happen if the theory is correct: NCE_BUG_IN_TOOL_RADIUS_COMP

Side effects:
   This finds and sets the values of center_x, center_y, and turn.

Called by: convert_arc_comp1

This finds the center coordinates and number of full or partial turns
counterclockwise of a helical or circular arc (call it arc1) in
r-format in the XY plane.  Arc2 is constructed so that it is tangent
to a circle whose radius is tool_radius and whose center is at the
point (current_x, current_y) and passes through the point (end_x,
end_y). Arc1 has the same center as arc2. The radius of arc1 is one
tool radius larger or smaller than the radius of arc2.

If the value of the big_radius argument is negative, that means [NCMS,
page 21] that an arc larger than a semicircle is to be made.
Otherwise, an arc of a semicircle or less is made.

The algorithm implemented here is to construct a line L from the
current point to the end point, and a perpendicular to it from the
center of the arc which intersects L at point P. Since the distance
from the end point to the center and the distance from the current
point to the center are known, two equations for the length of the
perpendicular can be written. The right sides of the equations can be
set equal to one another and the resulting equation solved for the
length of the line from the current point to P. Then the location of
P, the length of the perpendicular, the angle of the perpendicular,
and the location of the center, can be found in turn.

This needs to be better documented, with figures. There are eight
possible arcs, since there are three binary possibilities: (1) tool
inside or outside arc, (2) clockwise or counterclockwise (3) two
positions for each arc (of the given radius) tangent to the tool
outline and through the end point. All eight are calculated below,
since theta, radius2, and turn may each have two values.

To see two positions for each arc, imagine the arc is a hoop, the
tool is a cylindrical pin, and the arc may rotate around the end point.
The rotation covers all possible positions of the arc. It is easy to
see the hoop is constrained by the pin at two different angles, whether
the pin is inside or outside the hoop.

*/

static int arc_data_comp_r( /* ARGUMENTS                                 */
 int move,           /* either G_2 (cw arc) or G_3 (ccw arc)             */
 int side,           /* either RIGHT or LEFT                             */
 double tool_radius, /* radius of the tool                               */
 double current_x,   /* first coordinate of current point                */
 double current_y,   /* second coordinate of current point               */
 double end_x,       /* first coordinate of arc end point                */
 double end_y,       /* second coordinate of arc end point               */
 double big_radius,  /* radius of arc                                    */
 double * center_x,  /* pointer to first coordinate of center of arc     */
 double * center_y,  /* pointer to second coordinate of center of arc    */
 int * turn)         /* pointer to number of full or partial circles CCW */
{
  static char name[] SET_TO "arc_data_comp_r";
  double abs_radius; /* absolute value of big_radius          */
  double alpha;      /* direction of line from current to end */
  double distance;   /* length of line L from current to end  */
  double mid_length; /* length from current point to point P  */
  double offset;     /* length of line from P to center       */
  double radius2;    /* distance from center to current point */
  double mid_x;      /* x-value of point P                    */
  double mid_y;      /* y-value of point P                    */
  double theta;      /* direction of line from P to center    */

  abs_radius SET_TO fabs(big_radius);
  CHK(((abs_radius <= tool_radius) AND (((side IS LEFT ) AND (move IS G_3)) OR
                                        ((side IS RIGHT) AND (move IS G_2)))),
      NCE_TOOL_RADIUS_NOT_LESS_THAN_ARC_RADIUS_WITH_COMP);

  distance SET_TO hypot((end_x - current_x), (end_y - current_y));
  alpha SET_TO atan2 ((end_y - current_y), (end_x - current_x));
  theta SET_TO (((move IS G_3) AND (big_radius > 0)) OR
                ((move IS G_2) AND (big_radius < 0))) ?
                  (alpha + PI2) : (alpha - PI2);
  radius2 SET_TO (((side IS LEFT ) AND (move IS G_3)) OR
                  ((side IS RIGHT) AND (move IS G_2))) ?
                    (abs_radius - tool_radius) : (abs_radius + tool_radius);
  CHK((distance > (radius2 + abs_radius)),
      NCE_RADIUS_TOO_SMALL_TO_REACH_END_POINT);
  mid_length SET_TO (((radius2 * radius2) + (distance * distance) -
                      (abs_radius * abs_radius)) / (2.0 * distance));
  mid_x SET_TO (current_x + (mid_length * cos(alpha)));
  mid_y SET_TO (current_y + (mid_length * sin(alpha)));
  CHK(((radius2 * radius2) <= (mid_length * mid_length)),
      NCE_BUG_IN_TOOL_RADIUS_COMP);
  offset SET_TO sqrt((radius2 * radius2) - (mid_length * mid_length));
  *center_x SET_TO mid_x + (offset * cos(theta));
  *center_y SET_TO mid_y + (offset * sin(theta));
  *turn SET_TO (move IS G_2) ? -_setup.block1.l_number : _setup.block1.l_number;
  if (_setup.block1.l_number  IS -1)
	  *turn SET_TO (move IS G_2) ? -1 : 1;
  return RS274NGC_OK;
}

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

/* arc_data_ijk

Returned Value: int
   If any of the following errors occur, this returns the error code shown.
   Otherwise, it returns RS274NGC_OK.
   1. The two calculable values of the radius differ by more than
      tolerance: NCE_RADIUS_TO_END_OF_ARC_DIFFERS_FROM_RADIUS_TO_START
   2. The move code is not G_2 or G_3: NCE_BUG_CODE_NOT_G2_OR_G3
   3. Either of the two calculable values of the radius is zero:
      NCE_ZERO_RADIUS_ARC

Side effects:
   This finds and sets the values of center_x, center_y, and turn.

Called by:
   convert_arc2
   convert_arc_comp2

This finds the center coordinates and number of full or partial turns
counterclockwise of a helical or circular arc in ijk-format. This
function is used by convert_arc2 for all three planes, so "x" and
"y" really mean "first_coordinate" and "second_coordinate" wherever
they are used here as suffixes of variable names. The i and j prefixes
are handled similarly.

*/

static int arc_data_ijk( /* ARGUMENTS                                       */
 int move,               /* either G_2 (cw arc) or G_3 (ccw arc)            */
 double current_x,       /* first coordinate of current point               */
 double current_y,       /* second coordinate of current point              */
 double end_x,           /* first coordinate of arc end point               */
 double end_y,           /* second coordinate of arc end point              */
 double i_number,        /* first coordinate offset of center from current  */
 double j_number,        /* second coordinate offset of center from current */
 double * center_x,      /* pointer to first coordinate of center of arc    */
 double * center_y,      /* pointer to second coordinate of center of arc   */
 int * turn,             /* pointer to no. of full or partial circles CCW   */
 double tolerance)       /* tolerance of differing radii                    */
{
  static char name[] SET_TO "arc_data_ijk";
  double radius;    /* radius to current point */
  double radius2;   /* radius to end point     */
  *center_x SET_TO (current_x + i_number);
  *center_y SET_TO (current_y + j_number);
  radius SET_TO hypot((*center_x - current_x), (*center_y - current_y));
  radius2 SET_TO hypot((*center_x - end_x), (*center_y - end_y));
  CHK(((radius IS 0.0) OR (radius2 IS 0.0)), NCE_ZERO_RADIUS_ARC);
  CHK((fabs(radius - radius2) > tolerance),
      NCE_RADIUS_TO_END_OF_ARC_DIFFERS_FROM_RADIUS_TO_START);
 
  
  
  if (_setup.block1.l_number IS -1)
	  *turn SET_TO (move IS G_2) ? -1 : 1;
  else if (move IS G_2)
    *turn SET_TO -_setup.block1.l_number;
  else if (move IS G_3)
    *turn SET_TO _setup.block1.l_number;
  else
    ERM(NCE_BUG_CODE_NOT_G2_OR_G3);

  return RS274NGC_OK;
}

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

/* arc_data_r

Returned Value: int
   If any of the following errors occur, this returns the error shown.
   Otherwise, it returns RS274NGC_OK.
   1. The radius is too small to reach the end point:
      NCE_ARC_RADIUS_TOO_SMALL_TO_REACH_END_POINT
   2. The current point is the same as the end point of the arc
      (so that it is not possible to locate the center of the circle):
      NCE_CURRENT_POINT_SAME_AS_END_POINT_OF_ARC

Side effects:
   This finds and sets the values of center_x, center_y, and turn.

Called by:
   convert_arc2
   convert_arc_comp2

This finds the center coordinates and number of full or partial turns
counterclockwise of a helical or circular arc in the r format. This
function is used by convert_arc2 for all three planes, so "x" and
"y" really mean "first_coordinate" and "second_coordinate" wherever
they are used here as suffixes of variable names.

If the value of the radius argument is negative, that means [NCMS,
page 21] that an arc larger than a semicircle is to be made.
Otherwise, an arc of a semicircle or less is made.

The algorithm used here is based on finding the midpoint M of the line
L between the current point and the end point of the arc. The center
of the arc lies on a line through M perpendicular to L.

*/

static int arc_data_r( /* ARGUMENTS                                     */
 int move,             /* either G_2 (cw arc) or G_3 (ccw arc)          */
 double current_x,     /* first coordinate of current point             */
 double current_y,     /* second coordinate of current point            */
 double end_x,         /* first coordinate of arc end point             */
 double end_y,         /* second coordinate of arc end point            */
 double radius,        /* radius of arc                                 */
 double * center_x,    /* pointer to first coordinate of center of arc  */
 double * center_y,    /* pointer to second coordinate of center of arc */
 int * turn)           /* pointer to no. of full or partial circles CCW */
{
  static char name[] SET_TO "arc_data_r";
  double abs_radius;  /* absolute value of given radius */
  double half_length; /* distance from M to end point   */
  double mid_x;       /* first coordinate of M          */
  double mid_y;       /* second coordinate of M         */
  double offset;      /* distance from M to center      */
  double theta;       /* angle of line from M to center */
  double turn2;       /* absolute value of half of turn */

  CHK(((end_x IS current_x) AND (end_y IS current_y)),
      NCE_CURRENT_POINT_SAME_AS_END_POINT_OF_ARC);
  abs_radius SET_TO fabs(radius);
  mid_x SET_TO (end_x + current_x)/2.0;
  mid_y SET_TO (end_y + current_y)/2.0;
  half_length SET_TO hypot((mid_x - end_x), (mid_y - end_y));
  CHK(((half_length/abs_radius) > (1+TINY)),
      NCE_ARC_RADIUS_TOO_SMALL_TO_REACH_END_POINT);
  if ((half_length/abs_radius) > (1-TINY))
    half_length SET_TO abs_radius; /* allow a small error for semicircle */
                                   /* check needed before calling asin   */
  if (((move IS G_2) AND (radius > 0)) OR
      ((move IS G_3) AND (radius < 0)))
    theta SET_TO atan2((end_y - current_y), (end_x - current_x)) - PI2;
  else
    theta SET_TO atan2((end_y - current_y), (end_x - current_x)) + PI2;

  turn2 SET_TO asin (half_length/abs_radius);
  offset SET_TO abs_radius * cos(turn2);
  *center_x SET_TO mid_x + (offset * cos(theta));
  *center_y SET_TO mid_y + (offset * sin(theta));
   if (_setup.block1.l_number IS -1)
	  *turn SET_TO (move IS G_2) ? -1 : 1;
   else
  	*turn SET_TO (move IS G_2) ? -_setup.block1.l_number : _setup.block1.l_number;
 
  return RS274NGC_OK;
}

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

/* check_g_codes

Returned Value: int
   If any of the following errors occur, this returns the error shown.
   Otherwise, it returns RS274NGC_OK.
   1. NCE_DWELL_TIME_MISSING_WITH_G4
   2. NCE_MUST_USE_G0_OR_G1_WITH_G53
   3. NCE_CANNOT_USE_G53_INCREMENTAL
   4. NCE_LINE_WITH_G10_DOES_NOT_HAVE_L2
   5. NCE_P_VALUE_NOT_AN_INTEGER_WITH_G10_L2
   6. NCE_P_VALUE_OUT_OF_RANGE_WITH_G10_L2
   7. NCE_BUG_BAD_G_CODE_MODAL_GROUP_0

Side effects: none

Called by: check_items

This runs checks on g_codes from a block of RS274/NGC instructions.
Currently, all checks are on g_codes in modal group 0.

The read_g function checks for errors which would foul up the reading.
The enhance_block function checks for logical errors in the use of
axis values by g-codes in modal groups 0 and 1.
This function checks for additional logical errors in g_codes.

[Fanuc, page 45, note 4] says there is no maximum for how many g_codes
may be put on the same line, [NCMS] says nothing one way or the other,
so the test for that is not used.

We are suspending any implicit motion g_code when a g_code from our
group 0 is used.  The implicit motion g_code takes effect again
automatically after the line on which the group 0 g_code occurs.  It
is not clear what the intent of [Fanuc] is in this regard. The
alternative is to require that any implicit motion be explicitly
cancelled.

Not all checks on g_codes are included here. Those checks that are
sensitive to whether other g_codes on the same line have been executed
yet are made by the functions called by convert_g.

Our reference sources differ regarding what codes may be used for
dwell time.  [Fanuc, page 58] says use "p" or "x". [NCMS, page 23] says
use "p", "x", or "u". We are allowing "p" only, since it is consistent
with both sources and "x" would be confusing. However, "p" is also used
with G10, where it must be an integer, so reading "p" values is a bit
more trouble than would be nice.

*/

static int check_g_codes( /* ARGUMENTS                        */
 block_pointer block,     /* pointer to a block to be checked */
 setup_pointer settings)  /* pointer to machine settings      */
{
  static char name[] SET_TO "check_g_codes";
  int mode0;
  int p_int;

  mode0 SET_TO block->g_modes[0];

  if (mode0 IS -1)
    {}
  else if (mode0 IS G_4)
    {
      CHK((block->p_number IS -1.0), NCE_DWELL_TIME_MISSING_WITH_G4);
    }
  else if (mode0 IS G_9)
  	{
  	
  	}
  else if (mode0 IS G_10)
    {
      p_int SET_TO (int)(block->p_number + 0.0001);
      CHK((block->l_number ISNT 2), NCE_LINE_WITH_G10_DOES_NOT_HAVE_L2);
      CHK((((block->p_number + 0.0001) - p_int) > 0.0002),
          NCE_P_VALUE_NOT_AN_INTEGER_WITH_G10_L2);
      CHK(((p_int < 1) OR (p_int > 9)), NCE_P_VALUE_OUT_OF_RANGE_WITH_G10_L2);
    }
  else if (mode0 IS G_28)
    {}
  else if (mode0 IS G_30)