interp_find.cc

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

/********************************************************************
* Description: interp_find.cc
*
*   Derived from a work by Thomas Kramer
*
* Author:
* License: GPL Version 2
* System: Linux
*    
* Copyright (c) 2004 All rights reserved.
*
* Last change:
* $Revision: 1.12 $
* $Author: cradek $
* $Date: 2007/07/14 21:43:25 $
********************************************************************/
#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <ctype.h>
#include <sys/types.h>
#include <sys/stat.h>
#include "rs274ngc.hh"
#include "interp_return.hh"
#include "interp_internal.hh"

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

/*! find_arc_length

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

Side effects: none

Called by:
   inverse_time_rate_arc
   inverse_time_rate_arc2
   inverse_time_rate_as

This calculates the length of the path that will be made relative to
the XYZ axes for a motion in which the X,Y,Z, motion is a circular or
helical arc with its axis parallel to the Z-axis. If tool length
compensation is on, this is the path of the tool tip; if off, the
length of the path of the spindle tip. Any rotary axis motion is
ignored.

If the arc is helical, it is coincident with the hypotenuse of a right
triangle wrapped around a cylinder. If the triangle is unwrapped, its
base is [the radius of the cylinder times the number of radians in the
helix] and its height is [z2 - z1], and the path length can be found
by the Pythagorean theorem.

This is written as though it is only for arcs whose axis is parallel to
the Z-axis, but it will serve also for arcs whose axis is parallel
to the X-axis or Y-axis, with suitable permutation of the arguments.

This works correctly when turn is zero (find_turn returns 0 in that
case).

*/

double Interp::find_arc_length(double x1,        //!< X-coordinate of start point       
                              double y1,        //!< Y-coordinate of start point       
                              double z1,        //!< Z-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 z2)        //!< Z-coordinate of end point         
{
  double radius;
  double theta;                 /* amount of turn of arc in radians */

  radius = hypot((center_x - x1), (center_y - y1));
  theta = find_turn(x1, y1, center_x, center_y, turn, x2, y2);
  if (z2 == z1)
    return (radius * fabs(theta));
  else
    return hypot((radius * theta), (z2 - z1));
}

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

/*! find_ends

Returned Value: int (INTERP_OK)

Side effects:
   The values of px, py, pz, aa_p, bb_p, and cc_p are set

Called by:
   convert_arc
   convert_home
   convert_probe
   convert_straight

This finds the coordinates of a point, "end", in the currently
active coordinate system, and sets the values of the pointers to the
coordinates (which are the arguments to the function).

In all cases, if no value for the coodinate is given in the block, the
current value for the coordinate is used. When cutter radius
compensation is on, this function is called before compensation
calculations are performed, so the current value of the programmed
point is used, not the current value of the actual current_point.

There are three cases for when the coordinate is included in the block:

1. G_53 is active. This means to interpret the coordinates as machine
coordinates. That is accomplished by adding the two offsets to the
coordinate given in the block.

2. Absolute coordinate mode is in effect. The coordinate in the block
is used.

3. Incremental coordinate mode is in effect. The coordinate in the
block plus either (i) the programmed current position - when cutter
radius compensation is in progress, or (2) the actual current position.

*/

int Interp::find_ends(block_pointer block,       //!< pointer to a block of RS274/NGC instructions
                      setup_pointer settings,    //!< pointer to machine settings                 
                      double *px,        //!< pointer to end_x                            
                      double *py,        //!< pointer to end_y                            
                      double *pz,        //!< pointer to end_z                            
                      double *AA_p,      //!< pointer to end_a                      
                      double *BB_p,      //!< pointer to end_b                      
                      double *CC_p,      //!< pointer to end_c                      
                      double *u_p, double *v_p, double *w_p)
{
  int mode;
  int middle;
  int comp;

  mode = settings->distance_mode;
  middle = settings->cutter_comp_firstmove == OFF;
  comp = (settings->cutter_comp_side != OFF);

  if (block->g_modes[0] == G_53) {      /* distance mode is absolute in this case */
#ifdef DEBUG_EMC
    COMMENT("interpreter: offsets temporarily suspended");
#endif
    *px = (block->x_flag == ON) ? (block->x_number -
                                   (settings->tool_xoffset + 
                                    settings->origin_offset_x +
                                    settings->
                                    axis_offset_x)) : settings->current_x;
    *py =
      (block->y_flag ==
       ON) ? (block->y_number - (settings->origin_offset_y +
                                 settings->
                                 axis_offset_y)) : settings->current_y;
    *pz =
      (block->z_flag ==
       ON) ? (block->z_number - (settings->tool_zoffset +
                                 settings->origin_offset_z +
                                 settings->
                                 axis_offset_z)) : settings->current_z;
    *AA_p = (block->a_flag == ON) ? (block->a_number -
                                     (settings->AA_origin_offset +
                                      settings->
                                      AA_axis_offset)) : settings->AA_current;
    *BB_p =
      (block->b_flag ==
       ON) ? (block->b_number - (settings->BB_origin_offset +
                                 settings->
                                 BB_axis_offset)) : settings->BB_current;
    *CC_p =
      (block->c_flag ==
       ON) ? (block->c_number - (settings->CC_origin_offset +
                                 settings->
                                 CC_axis_offset)) : settings->CC_current;
    *u_p = (block->u_flag == ON) ? (block->u_number -
                                     (settings->u_origin_offset +
                                      settings->
                                      u_axis_offset)) : settings->u_current;
    *v_p =
      (block->v_flag ==
       ON) ? (block->v_number - (settings->v_origin_offset +
                                 settings->
                                 v_axis_offset)) : settings->v_current;
    *w_p =
      (block->w_flag ==
       ON) ? (block->w_number - (settings->w_origin_offset +
                                 settings->
                                 w_axis_offset)) : settings->w_current;
  } else if (mode == MODE_ABSOLUTE) {
    *px = (block->x_flag == ON) ? block->x_number :
      (comp && middle) ? settings->program_x : settings->current_x;

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

    *pz = (block->z_flag == ON) ? block->z_number :
      (comp && middle && settings->plane == CANON_PLANE_XZ ) ? settings->program_z : settings->current_z;

    *AA_p = (block->a_flag == ON) ? block->a_number : settings->AA_current;
    *BB_p = (block->b_flag == ON) ? block->b_number : settings->BB_current;
    *CC_p = (block->c_flag == ON) ? block->c_number : settings->CC_current;