1 |
|
/******************************************************************** |
2 |
|
* Description: interp_find.cc |
3 |
|
* |
4 |
|
* Derived from a work by Thomas Kramer |
5 |
|
* |
6 |
|
* Author: |
7 |
|
* License: GPL Version 2 |
8 |
|
* System: Linux |
9 |
|
* |
10 |
|
* Copyright (c) 2004 All rights reserved. |
11 |
|
* |
12 |
|
* Last change: |
13 |
|
********************************************************************/ |
14 |
|
#ifndef _GNU_SOURCE |
15 |
|
#define _GNU_SOURCE |
16 |
|
#endif |
17 |
|
#include <unistd.h> |
18 |
|
#include <stdio.h> |
19 |
|
#include <stdlib.h> |
20 |
|
#include <math.h> |
21 |
|
#include <string.h> |
22 |
|
#include <ctype.h> |
23 |
|
#include <sys/types.h> |
24 |
|
#include <sys/stat.h> |
25 |
|
#include "rtapi_math.h" |
26 |
|
#include "rs274ngc.hh" |
27 |
|
#include "interp_return.hh" |
28 |
|
#include "interp_internal.hh" |
29 |
|
#include "rs274ngc_interp.hh" |
30 |
|
#include "units.h" |
31 |
|
|
32 |
|
/****************************************************************************/ |
33 |
|
|
34 |
|
/*! find_arc_length |
35 |
|
|
36 |
|
Returned Value: double (length of path between start and end points) |
37 |
|
|
38 |
|
Side effects: none |
39 |
|
|
40 |
|
Called by: |
41 |
|
inverse_time_rate_arc |
42 |
|
inverse_time_rate_arc2 |
43 |
|
inverse_time_rate_as |
44 |
|
|
45 |
|
This calculates the length of the path that will be made relative to |
46 |
|
the XYZ axes for a motion in which the X,Y,Z, motion is a circular or |
47 |
|
helical arc with its axis parallel to the Z-axis. If tool length |
48 |
|
compensation is on, this is the path of the tool tip; if off, the |
49 |
|
length of the path of the spindle tip. Any rotary axis motion is |
50 |
|
ignored. |
51 |
|
|
52 |
|
If the arc is helical, it is coincident with the hypotenuse of a right |
53 |
|
triangle wrapped around a cylinder. If the triangle is unwrapped, its |
54 |
|
base is [the radius of the cylinder times the number of radians in the |
55 |
|
helix] and its height is [z2 - z1], and the path length can be found |
56 |
|
by the Pythagorean theorem. |
57 |
|
|
58 |
|
This is written as though it is only for arcs whose axis is parallel to |
59 |
|
the Z-axis, but it will serve also for arcs whose axis is parallel |
60 |
|
to the X-axis or Y-axis, with suitable permutation of the arguments. |
61 |
|
|
62 |
|
This works correctly when turn is zero (find_turn returns 0 in that |
63 |
|
case). |
64 |
|
|
65 |
|
*/ |
66 |
|
|
67 |
3 |
double Interp::find_arc_length(double x1, //!< X-coordinate of start point |
68 |
|
double y1, //!< Y-coordinate of start point |
69 |
|
double z1, //!< Z-coordinate of start point |
70 |
|
double center_x, //!< X-coordinate of arc center |
71 |
|
double center_y, //!< Y-coordinate of arc center |
72 |
|
int turn, //!< no. of full or partial circles CCW |
73 |
|
double x2, //!< X-coordinate of end point |
74 |
|
double y2, //!< Y-coordinate of end point |
75 |
|
double z2) //!< Z-coordinate of end point |
76 |
|
{ |
77 |
|
double radius; |
78 |
|
double theta; /* amount of turn of arc in radians */ |
79 |
|
|
80 |
3 |
radius = hypot((center_x - x1), (center_y - y1)); |
81 |
3 |
theta = find_turn(x1, y1, center_x, center_y, turn, x2, y2); |
82 |
3 |
if (z2 == z1) |
83 |
3 |
return (radius * fabs(theta)); |
84 |
|
else |
85 |
|
return hypot((radius * theta), (z2 - z1)); |
86 |
|
} |
87 |
|
|
88 |
|
|
89 |
|
/* Find the real destination, given the axis's current position, the |
90 |
|
commanded destination, and the direction to turn (which comes from |
91 |
|
the sign of the commanded value in the gcode). Modulo 360 positions |
92 |
|
of the axis are considered equivalent and we just need to find the |
93 |
|
nearest one. */ |
94 |
|
|
95 |
|
int Interp::unwrap_rotary(double *r, double sign_of, double commanded, double current, char axis) { |
96 |
|
double result; |
97 |
|
int neg = copysign(1.0, sign_of) < 0.0; |
98 |
|
CHKS((sign_of <= -360.0 || sign_of >= 360.0), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), sign_of, axis); |
99 |
|
|
100 |
|
double d = floor(current/360.0); |
101 |
|
result = fabs(commanded) + (d*360.0); |
102 |
|
if(!neg && result < current) result += 360.0; |
103 |
|
if(neg && result > current) result -= 360.0; |
104 |
|
*r = result; |
105 |
|
|
106 |
|
return INTERP_OK; |
107 |
|
} |
108 |
|
|
109 |
|
|
110 |
|
/****************************************************************************/ |
111 |
|
|
112 |
|
/*! find_ends |
113 |
|
|
114 |
|
Returned Value: int (INTERP_OK) |
115 |
|
|
116 |
|
Side effects: |
117 |
|
The values of px, py, pz, aa_p, bb_p, and cc_p are set |
118 |
|
|
119 |
|
Called by: |
120 |
|
convert_arc |
121 |
|
convert_home |
122 |
|
convert_probe |
123 |
|
convert_straight |
124 |
|
|
125 |
|
This finds the coordinates of a point, "end", in the currently |
126 |
|
active coordinate system, and sets the values of the pointers to the |
127 |
|
coordinates (which are the arguments to the function). |
128 |
|
|
129 |
|
In all cases, if no value for the coodinate is given in the block, the |
130 |
|
current value for the coordinate is used. When cutter radius |
131 |
|
compensation is on, this function is called before compensation |
132 |
|
calculations are performed, so the current value of the programmed |
133 |
|
point is used, not the current value of the actual current_point. |
134 |
|
|
135 |
|
There are three cases for when the coordinate is included in the block: |
136 |
|
|
137 |
|
1. G_53 is active. This means to interpret the coordinates as machine |
138 |
|
coordinates. That is accomplished by adding the two offsets to the |
139 |
|
coordinate given in the block. |
140 |
|
|
141 |
|
2. Absolute coordinate mode is in effect. The coordinate in the block |
142 |
|
is used. |
143 |
|
|
144 |
|
3. Incremental coordinate mode is in effect. The coordinate in the |
145 |
|
block plus either (i) the programmed current position - when cutter |
146 |
|
radius compensation is in progress, or (2) the actual current position. |
147 |
|
|
148 |
|
*/ |
149 |
|
|
150 |
|
|
151 |
1418 |
int Interp::find_ends(block_pointer block, //!< pointer to a block of RS274/NGC instructions |
152 |
|
setup_pointer s, //!< pointer to machine settings |
153 |
|
double *px, //!< pointer to end_x |
154 |
|
double *py, //!< pointer to end_y |
155 |
|
double *pz, //!< pointer to end_z |
156 |
|
double *AA_p, //!< pointer to end_a |
157 |
|
double *BB_p, //!< pointer to end_b |
158 |
|
double *CC_p, //!< pointer to end_c |
159 |
|
double *u_p, double *v_p, double *w_p) |
160 |
|
{ |
161 |
|
int middle; |
162 |
|
int comp; |
163 |
|
|
164 |
1418 |
middle = !s->cutter_comp_firstmove; |
165 |
1418 |
comp = (s->cutter_comp_side); |
166 |
|
|
167 |
1418 |
if (block->g_modes[0] == G_53) { /* distance mode is absolute in this case */ |
168 |
|
#ifdef DEBUG_EMC |
169 |
|
COMMENT("interpreter: offsets temporarily suspended"); |
170 |
|
#endif |
171 |
8 |
CHKS((block->radius_flag || block->theta_flag), _("Cannot use polar coordinates with G53")); |
172 |
|
|
173 |
8 |
double cx = s->current_x; |
174 |
8 |
double cy = s->current_y; |
175 |
8 |
rotate(&cx, &cy, s->rotation_xy); |
176 |
|
|
177 |
8 |
if(block->x_flag) { |
178 |
4 |
*px = block->x_number - s->origin_offset_x - s->axis_offset_x - s->tool_offset.tran.x; |
179 |
|
} else { |
180 |
4 |
*px = cx; |
181 |
|
} |
182 |
|
|
183 |
8 |
if(block->y_flag) { |
184 |
2 |
*py = block->y_number - s->origin_offset_y - s->axis_offset_y - s->tool_offset.tran.y; |
185 |
|
} else { |
186 |
6 |
*py = cy; |
187 |
|
} |
188 |
|
|
189 |
8 |
rotate(px, py, -s->rotation_xy); |
190 |
|
|
191 |
8 |
if(block->z_flag) { |
192 |
6 |
*pz = block->z_number - s->origin_offset_z - s->axis_offset_z - s->tool_offset.tran.z; |
193 |
|
} else { |
194 |
2 |
*pz = s->current_z; |
195 |
|
} |
196 |
|
|
197 |
8 |
if(block->a_flag) { |
198 |
|
if(s->a_axis_wrapped) { |
199 |
|
CHP(unwrap_rotary(AA_p, block->a_number, |
200 |
|
block->a_number - s->AA_origin_offset - s->AA_axis_offset - s->tool_offset.a, |
201 |
|
s->AA_current, 'A')); |
202 |
|
} else { |
203 |
|
*AA_p = block->a_number - s->AA_origin_offset - s->AA_axis_offset; |
204 |
|
} |
205 |
|
} else { |
206 |
8 |
*AA_p = s->AA_current; |
207 |
|
} |
208 |
|
|
209 |
8 |
if(block->b_flag) { |
210 |
|
if(s->b_axis_wrapped) { |
211 |
|
CHP(unwrap_rotary(BB_p, block->b_number, |
212 |
|
block->b_number - s->BB_origin_offset - s->BB_axis_offset - s->tool_offset.b, |
213 |
|
s->BB_current, 'B')); |
214 |
|
} else { |
215 |
|
*BB_p = block->b_number - s->BB_origin_offset - s->BB_axis_offset; |
216 |
|
} |
217 |
|
} else { |
218 |
8 |
*BB_p = s->BB_current; |
219 |
|
} |
220 |
|
|
221 |
8 |
if(block->c_flag) { |
222 |
|
if(s->c_axis_wrapped) { |
223 |
|
CHP(unwrap_rotary(CC_p, block->c_number, |
224 |
|
block->c_number - s->CC_origin_offset - s->CC_axis_offset - s->tool_offset.c, |
225 |
|
s->CC_current, 'C')); |
226 |
|
} else { |
227 |
|
*CC_p = block->c_number - s->CC_origin_offset - s->CC_axis_offset; |
228 |
|
} |
229 |
|
} else { |
230 |
8 |
*CC_p = s->CC_current; |
231 |
|
} |
232 |
|
|
233 |
8 |
if(block->u_flag) { |
234 |
|
*u_p = block->u_number - s->u_origin_offset - s->u_axis_offset - s->tool_offset.u; |
235 |
|
} else { |
236 |
8 |
*u_p = s->u_current; |
237 |
|
} |
238 |
|
|
239 |
8 |
if(block->v_flag) { |
240 |
|
*v_p = block->v_number - s->v_origin_offset - s->v_axis_offset - s->tool_offset.v; |
241 |
|
} else { |
242 |
8 |
*v_p = s->v_current; |
243 |
|
} |
244 |
|
|
245 |
8 |
if(block->w_flag) { |
246 |
|
*w_p = block->w_number - s->w_origin_offset - s->w_axis_offset - s->tool_offset.w; |
247 |
|
} else { |
248 |
8 |
*w_p = s->w_current; |
249 |
|
} |
250 |
1410 |
} else if (s->distance_mode == MODE_ABSOLUTE) { |
251 |
|
|
252 |
1353 |
if(block->x_flag) { |
253 |
1164 |
*px = block->x_number; |
254 |
|
} else { |
255 |
|
// both cutter comp planes affect X ... |
256 |
189 |
*px = (comp && middle) ? s->program_x : s->current_x; |
257 |
|
} |
258 |
|
|
259 |
1353 |
if(block->y_flag) { |
260 |
1169 |
*py = block->y_number; |
261 |
|
} else { |
262 |
|
// but only XY affects Y ... |
263 |
184 |
*py = (comp && middle && s->plane == CANON_PLANE_XY) ? s->program_y : s->current_y; |
264 |
|
} |
265 |
|
|
266 |
1353 |
if(block->radius_flag && block->theta_flag) { |
267 |
|
CHKS((block->x_flag || block->y_flag), _("Cannot specify X or Y words with polar coordinate")); |
268 |
|
*px = block->radius * cos(D2R(block->theta)); |
269 |
|
*py = block->radius * sin(D2R(block->theta)); |
270 |
1353 |
} else if(block->radius_flag) { |
271 |
|
double theta; |
272 |
|
CHKS((block->x_flag || block->y_flag), _("Cannot specify X or Y words with polar coordinate")); |
273 |
|
CHKS((*py == 0 && *px == 0), _("Must specify angle in polar coordinate if at the origin")); |
274 |
|
theta = atan2(*py, *px); |
275 |
|
*px = block->radius * cos(theta); |
276 |
|
*py = block->radius * sin(theta); |
277 |
1353 |
} else if(block->theta_flag) { |
278 |
|
double radius; |
279 |
|
CHKS((block->x_flag || block->y_flag), _("Cannot specify X or Y words with polar coordinate")); |
280 |
|
radius = hypot(*py, *px); |
281 |
|
*px = radius * cos(D2R(block->theta)); |
282 |
|
*py = radius * sin(D2R(block->theta)); |
283 |
|
} |
284 |
|
|
285 |
1353 |
if(block->z_flag) { |
286 |
208 |
*pz = block->z_number; |
287 |
|
} else { |
288 |
|
// and only XZ affects Z. |
289 |
1145 |
*pz = (comp && middle && s->plane == CANON_PLANE_XZ) ? s->program_z : s->current_z; |
290 |
|
} |
291 |
|
|
292 |
1353 |
if(block->a_flag) { |
293 |
1 |
if(s->a_axis_wrapped) { |
294 |
|
CHP(unwrap_rotary(AA_p, block->a_number, block->a_number, s->AA_current, 'A')); |
295 |
|
} else { |
296 |
1 |
*AA_p = block->a_number; |
297 |
|
} |
298 |
|
} else { |
299 |
1352 |
*AA_p = s->AA_current; |
300 |
|
} |
301 |
|
|
302 |
1353 |
if(block->b_flag) { |
303 |
|
if(s->b_axis_wrapped) { |
304 |
|
CHP(unwrap_rotary(BB_p, block->b_number, block->b_number, s->BB_current, 'B')); |
305 |
|
} else { |
306 |
|
*BB_p = block->b_number; |
307 |
|
} |
308 |
|
} else { |
309 |
1353 |
*BB_p = s->BB_current; |
310 |
|
} |
311 |
|
|
312 |
1353 |
if(block->c_flag) { |
313 |
|
if(s->c_axis_wrapped) { |
314 |
|
CHP(unwrap_rotary(CC_p, block->c_number, block->c_number, s->CC_current, 'C')); |
315 |
|
} else { |
316 |
|
*CC_p = block->c_number; |
317 |
|
} |
318 |
|
} else { |
319 |
1353 |
*CC_p = s->CC_current; |
320 |
|
} |
321 |
|
|
322 |
1353 |
*u_p = (block->u_flag) ? block->u_number : s->u_current; |
323 |
1353 |
*v_p = (block->v_flag) ? block->v_number : s->v_current; |
324 |
1353 |
*w_p = (block->w_flag) ? block->w_number : s->w_current; |
325 |
|
|
326 |
|
} else { /* mode is MODE_INCREMENTAL */ |
327 |
|
|
328 |
|
// both cutter comp planes affect X ... |
329 |
57 |
*px = (comp && middle) ? s->program_x: s->current_x; |
330 |
57 |
if(block->x_flag) *px += block->x_number; |
331 |
|
|
332 |
|
// but only XY affects Y ... |
333 |
57 |
*py = (comp && middle && s->plane == CANON_PLANE_XY) ? s->program_y: s->current_y; |
334 |
57 |
if(block->y_flag) *py += block->y_number; |
335 |
|
|
336 |
57 |
if(block->radius_flag) { |
337 |
|
double radius, theta; |
338 |
|
CHKS((block->x_flag || block->y_flag), _("Cannot specify X or Y words with polar coordinate")); |
339 |
|
CHKS((*py == 0 && *px == 0), _("Incremental motion with polar coordinates is indeterminate when at the origin")); |
340 |
|
theta = atan2(*py, *px); |
341 |
|
radius = hypot(*py, *px) + block->radius; |
342 |
|
*px = radius * cos(theta); |
343 |
|
*py = radius * sin(theta); |
344 |
|
} |
345 |
|
|
346 |
57 |
if(block->theta_flag) { |
347 |
|
double radius, theta; |
348 |
|
CHKS((block->x_flag || block->y_flag), _("Cannot specify X or Y words with polar coordinate")); |
349 |
|
CHKS((*py == 0 && *px == 0), _("G91 motion with polar coordinates is indeterminate when at the origin")); |
350 |
|
theta = atan2(*py, *px) + D2R(block->theta); |
351 |
|
radius = hypot(*py, *px); |
352 |
|
*px = radius * cos(theta); |
353 |
|
*py = radius * sin(theta); |
354 |
|
} |
355 |
|
|
356 |
|
// and only XZ affects Z. |
357 |
57 |
*pz = (comp && middle && s->plane == CANON_PLANE_XZ) ? s->program_z: s->current_z; |
358 |
57 |
if(block->z_flag) *pz += block->z_number; |
359 |
|
|
360 |
57 |
*AA_p = s->AA_current; |
361 |
57 |
if(block->a_flag) *AA_p += block->a_number; |
362 |
|
|
363 |
57 |
*BB_p = s->BB_current; |
364 |
57 |
if(block->b_flag) *BB_p += block->b_number; |
365 |
|
|
366 |
57 |
*CC_p = s->CC_current; |
367 |
57 |
if(block->c_flag) *CC_p += block->c_number; |
368 |
|
|
369 |
57 |
*u_p = s->u_current; |
370 |
57 |
if(block->u_flag) *u_p += block->u_number; |
371 |
|
|
372 |
57 |
*v_p = s->v_current; |
373 |
57 |
if(block->v_flag) *v_p += block->v_number; |
374 |
|
|
375 |
57 |
*w_p = s->w_current; |
376 |
57 |
if(block->w_flag) *w_p += block->w_number; |
377 |
|
} |
378 |
|
return INTERP_OK; |
379 |
|
} |
380 |
|
|
381 |
|
/****************************************************************************/ |
382 |
|
|
383 |
|
/*! find_relative |
384 |
|
|
385 |
|
Returned Value: int (INTERP_OK) |
386 |
|
|
387 |
|
Side effects: |
388 |
|
The values of x2, y2, z2, aa_2, bb_2, and cc_2 are set. |
389 |
|
(NOTE: aa_2 etc. are written with lower case letters in this |
390 |
|
documentation because upper case would confuse the pre-preprocessor.) |
391 |
|
|
392 |
|
Called by: |
393 |
|
convert_home |
394 |
|
|
395 |
|
This finds the coordinates in the current system, under the current |
396 |
|
tool length offset, of a point (x1, y1, z1, aa_1, bb_1, cc_1) whose absolute |
397 |
|
coordinates are known. |
398 |
|
|
399 |
|
Don't confuse this with the inverse operation. |
400 |
|
|
401 |
|
*/ |
402 |
|
|
403 |
1 |
int Interp::find_relative(double x1, //!< absolute x position |
404 |
|
double y1, //!< absolute y position |
405 |
|
double z1, //!< absolute z position |
406 |
|
double AA_1, //!< absolute a position |
407 |
|
double BB_1, //!< absolute b position |
408 |
|
double CC_1, //!< absolute c position |
409 |
|
double u_1, |
410 |
|
double v_1, |
411 |
|
double w_1, |
412 |
|
double *x2, //!< pointer to relative x |
413 |
|
double *y2, //!< pointer to relative y |
414 |
|
double *z2, //!< pointer to relative z |
415 |
|
double *AA_2, //!< pointer to relative a |
416 |
|
double *BB_2, //!< pointer to relative b |
417 |
|
double *CC_2, //!< pointer to relative c |
418 |
|
double *u_2, |
419 |
|
double *v_2, |
420 |
|
double *w_2, |
421 |
|
setup_pointer settings) //!< pointer to machine settings |
422 |
|
{ |
423 |
1 |
*x2 = x1 - settings->origin_offset_x - settings->axis_offset_x - settings->tool_offset.tran.x; |
424 |
1 |
*y2 = y1 - settings->origin_offset_y - settings->axis_offset_y - settings->tool_offset.tran.y; |
425 |
1 |
rotate(x2, y2, -settings->rotation_xy); |
426 |
1 |
*z2 = z1 - settings->origin_offset_z - settings->axis_offset_z - settings->tool_offset.tran.z; |
427 |
|
|
428 |
1 |
if(settings->a_axis_wrapped) { |
429 |
|
CHP(unwrap_rotary(AA_2, AA_1, |
430 |
|
AA_1 - settings->AA_origin_offset - settings->AA_axis_offset - settings->tool_offset.a, |
431 |
|
settings->AA_current, 'A')); |
432 |
|
} else { |
433 |
1 |
*AA_2 = AA_1 - settings->AA_origin_offset - settings->AA_axis_offset; |
434 |
|
} |
435 |
|
|
436 |
1 |
if(settings->b_axis_wrapped) { |
437 |
|
CHP(unwrap_rotary(BB_2, BB_1, |
438 |
|
BB_1 - settings->BB_origin_offset - settings->BB_axis_offset - settings->tool_offset.b, |
439 |
|
settings->BB_current, 'B')); |
440 |
|
} else { |
441 |
1 |
*BB_2 = BB_1 - settings->BB_origin_offset - settings->BB_axis_offset; |
442 |
|
} |
443 |
|
|
444 |
1 |
if(settings->c_axis_wrapped) { |
445 |
|
CHP(unwrap_rotary(CC_2, CC_1, |
446 |
|
CC_1 - settings->CC_origin_offset - settings->CC_axis_offset - settings->tool_offset.c, |
447 |
|
settings->CC_current, 'C')); |
448 |
|
} else { |
449 |
1 |
*CC_2 = CC_1 - settings->CC_origin_offset - settings->CC_axis_offset; |
450 |
|
} |
451 |
|
|
452 |
1 |
*u_2 = u_1 - settings->u_origin_offset - settings->u_axis_offset - settings->tool_offset.u; |
453 |
1 |
*v_2 = v_1 - settings->v_origin_offset - settings->v_axis_offset - settings->tool_offset.v; |
454 |
1 |
*w_2 = w_1 - settings->w_origin_offset - settings->w_axis_offset - settings->tool_offset.w; |
455 |
1 |
return INTERP_OK; |
456 |
|
} |
457 |
|
|
458 |
|
// find what the current coordinates would be if we were in a different system |
459 |
|
|
460 |
62 |
int Interp::find_current_in_system(setup_pointer s, int system, |
461 |
|
double *x, double *y, double *z, |
462 |
|
double *a, double *b, double *c, |
463 |
|
double *u, double *v, double *w) { |
464 |
62 |
double *p = s->parameters; |
465 |
|
|
466 |
62 |
*x = s->current_x; |
467 |
62 |
*y = s->current_y; |
468 |
62 |
*z = s->current_z; |
469 |
62 |
*a = s->AA_current; |
470 |
62 |
*b = s->BB_current; |
471 |
62 |
*c = s->CC_current; |
472 |
62 |
*u = s->u_current; |
473 |
62 |
*v = s->v_current; |
474 |
62 |
*w = s->w_current; |
475 |
|
|
476 |
62 |
*x += s->axis_offset_x; |
477 |
62 |
*y += s->axis_offset_y; |
478 |
62 |
*z += s->axis_offset_z; |
479 |
62 |
*a += s->AA_axis_offset; |
480 |
62 |
*b += s->BB_axis_offset; |
481 |
62 |
*c += s->CC_axis_offset; |
482 |
62 |
*u += s->u_axis_offset; |
483 |
62 |
*v += s->v_axis_offset; |
484 |
62 |
*w += s->w_axis_offset; |
485 |
|
|
486 |
62 |
rotate(x, y, s->rotation_xy); |
487 |
|
|
488 |
62 |
*x += s->origin_offset_x; |
489 |
62 |
*y += s->origin_offset_y; |
490 |
62 |
*z += s->origin_offset_z; |
491 |
62 |
*a += s->AA_origin_offset; |
492 |
62 |
*b += s->BB_origin_offset; |
493 |
62 |
*c += s->CC_origin_offset; |
494 |
62 |
*u += s->u_origin_offset; |
495 |
62 |
*v += s->v_origin_offset; |
496 |
62 |
*w += s->w_origin_offset; |
497 |
|
|
498 |
62 |
*x -= USER_TO_PROGRAM_LEN(p[5201 + system * 20]); |
499 |
62 |
*y -= USER_TO_PROGRAM_LEN(p[5202 + system * 20]); |
500 |
62 |
*z -= USER_TO_PROGRAM_LEN(p[5203 + system * 20]); |
501 |
62 |
*a -= USER_TO_PROGRAM_ANG(p[5204 + system * 20]); |
502 |
62 |
*b -= USER_TO_PROGRAM_ANG(p[5205 + system * 20]); |
503 |
62 |
*c -= USER_TO_PROGRAM_ANG(p[5206 + system * 20]); |
504 |
62 |
*u -= USER_TO_PROGRAM_LEN(p[5207 + system * 20]); |
505 |
62 |
*v -= USER_TO_PROGRAM_LEN(p[5208 + system * 20]); |
506 |
62 |
*w -= USER_TO_PROGRAM_LEN(p[5209 + system * 20]); |
507 |
|
|
508 |
62 |
rotate(x, y, -p[5210 + system * 20]); |
509 |
|
|
510 |
62 |
if (p[5210]) { |
511 |
11 |
*x -= USER_TO_PROGRAM_LEN(p[5211]); |
512 |
11 |
*y -= USER_TO_PROGRAM_LEN(p[5212]); |
513 |
11 |
*z -= USER_TO_PROGRAM_LEN(p[5213]); |
514 |
11 |
*a -= USER_TO_PROGRAM_ANG(p[5214]); |
515 |
11 |
*b -= USER_TO_PROGRAM_ANG(p[5215]); |
516 |
11 |
*c -= USER_TO_PROGRAM_ANG(p[5216]); |
517 |
11 |
*u -= USER_TO_PROGRAM_LEN(p[5217]); |
518 |
11 |
*v -= USER_TO_PROGRAM_LEN(p[5218]); |
519 |
11 |
*w -= USER_TO_PROGRAM_LEN(p[5219]); |
520 |
|
} |
521 |
|
|
522 |
62 |
return INTERP_OK; |
523 |
|
} |
524 |
|
|
525 |
|
|
526 |
|
// find what the current coordinates would be if we were in a different system, |
527 |
|
// if TLO were unapplied |
528 |
|
|
529 |
17 |
int Interp::find_current_in_system_without_tlo(setup_pointer s, int system, |
530 |
|
double *x, double *y, double *z, |
531 |
|
double *a, double *b, double *c, |
532 |
|
double *u, double *v, double *w) { |
533 |
17 |
double *p = s->parameters; |
534 |
|
|
535 |
17 |
*x = s->current_x; |
536 |
17 |
*y = s->current_y; |
537 |
17 |
*z = s->current_z; |
538 |
17 |
*a = s->AA_current; |
539 |
17 |
*b = s->BB_current; |
540 |
17 |
*c = s->CC_current; |
541 |
17 |
*u = s->u_current; |
542 |
17 |
*v = s->v_current; |
543 |
17 |
*w = s->w_current; |
544 |
|
|
545 |
17 |
*x += s->axis_offset_x; |
546 |
17 |
*y += s->axis_offset_y; |
547 |
17 |
*z += s->axis_offset_z; |
548 |
17 |
*a += s->AA_axis_offset; |
549 |
17 |
*b += s->BB_axis_offset; |
550 |
17 |
*c += s->CC_axis_offset; |
551 |
17 |
*u += s->u_axis_offset; |
552 |
17 |
*v += s->v_axis_offset; |
553 |
17 |
*w += s->w_axis_offset; |
554 |
|
|
555 |
17 |
rotate(x, y, s->rotation_xy); |
556 |
|
|
557 |
17 |
*x += s->origin_offset_x; |
558 |
17 |
*y += s->origin_offset_y; |
559 |
17 |
*z += s->origin_offset_z; |
560 |
17 |
*a += s->AA_origin_offset; |
561 |
17 |
*b += s->BB_origin_offset; |
562 |
17 |
*c += s->CC_origin_offset; |
563 |
17 |
*u += s->u_origin_offset; |
564 |
17 |
*v += s->v_origin_offset; |
565 |
17 |
*w += s->w_origin_offset; |
566 |
|
|
567 |
17 |
*x += s->tool_offset.tran.x; |
568 |
17 |
*y += s->tool_offset.tran.y; |
569 |
17 |
*z += s->tool_offset.tran.z; |
570 |
17 |
*a += s->tool_offset.a; |
571 |
17 |
*b += s->tool_offset.b; |
572 |
17 |
*c += s->tool_offset.c; |
573 |
17 |
*u += s->tool_offset.u; |
574 |
17 |
*v += s->tool_offset.v; |
575 |
17 |
*w += s->tool_offset.w; |
576 |
|
|
577 |
17 |
*x -= USER_TO_PROGRAM_LEN(p[5201 + system * 20]); |
578 |
17 |
*y -= USER_TO_PROGRAM_LEN(p[5202 + system * 20]); |
579 |
17 |
*z -= USER_TO_PROGRAM_LEN(p[5203 + system * 20]); |
580 |
17 |
*a -= USER_TO_PROGRAM_ANG(p[5204 + system * 20]); |
581 |
17 |
*b -= USER_TO_PROGRAM_ANG(p[5205 + system * 20]); |
582 |
17 |
*c -= USER_TO_PROGRAM_ANG(p[5206 + system * 20]); |
583 |
17 |
*u -= USER_TO_PROGRAM_LEN(p[5207 + system * 20]); |
584 |
17 |
*v -= USER_TO_PROGRAM_LEN(p[5208 + system * 20]); |
585 |
17 |
*w -= USER_TO_PROGRAM_LEN(p[5209 + system * 20]); |
586 |
|
|
587 |
17 |
rotate(x, y, -p[5210 + system * 20]); |
588 |
|
|
589 |
17 |
if (p[5210]) { |
590 |
2 |
*x -= USER_TO_PROGRAM_LEN(p[5211]); |
591 |
2 |
*y -= USER_TO_PROGRAM_LEN(p[5212]); |
592 |
2 |
*z -= USER_TO_PROGRAM_LEN(p[5213]); |
593 |
2 |
*a -= USER_TO_PROGRAM_ANG(p[5214]); |
594 |
2 |
*b -= USER_TO_PROGRAM_ANG(p[5215]); |
595 |
2 |
*c -= USER_TO_PROGRAM_ANG(p[5216]); |
596 |
2 |
*u -= USER_TO_PROGRAM_LEN(p[5217]); |
597 |
2 |
*v -= USER_TO_PROGRAM_LEN(p[5218]); |
598 |
2 |
*w -= USER_TO_PROGRAM_LEN(p[5219]); |
599 |
|
} |
600 |
|
|
601 |
17 |
return INTERP_OK; |
602 |
|
} |
603 |
|
|
604 |
|
/****************************************************************************/ |
605 |
|
|
606 |
|
/*! find_straight_length |
607 |
|
|
608 |
|
Returned Value: double (length of path between start and end points) |
609 |
|
|
610 |
|
Side effects: none |
611 |
|
|
612 |
|
Called by: |
613 |
|
inverse_time_rate_straight |
614 |
|
inverse_time_rate_as |
615 |
|
|
616 |
|
This calculates a number to use in feed rate calculations when inverse |
617 |
|
time feed mode is used, for a motion in which X,Y,Z,A,B, and C each change |
618 |
|
linearly or not at all from their initial value to their end value. |
619 |
|
|
620 |
|
This is used when the feed_reference mode is CANON_XYZ, which is |
621 |
|
always in rs274NGC. |
622 |
|
|
623 |
|
If any of the X, Y, or Z axes move or the A-axis, B-axis, and C-axis |
624 |
|
do not move, this is the length of the path relative to the XYZ axes |
625 |
|
from the first point to the second, and any rotary axis motion is |
626 |
|
ignored. The length is the simple Euclidean distance. |
627 |
|
|
628 |
|
The formula for the Euclidean distance "length" of a move involving |
629 |
|
only the A, B and C axes is based on a conversation with Jim Frohardt at |
630 |
|
Boeing, who says that the Fanuc controller on their 5-axis machine |
631 |
|
interprets the feed rate this way. Note that if only one rotary axis |
632 |
|
moves, this formula returns the absolute value of that axis move, |
633 |
|
which is what is desired. |
634 |
|
|
635 |
|
*/ |
636 |
|
|
637 |
12 |
double Interp::find_straight_length(double x2, //!< X-coordinate of end point |
638 |
|
double y2, //!< Y-coordinate of end point |
639 |
|
double z2, //!< Z-coordinate of end point |
640 |
|
double AA_2, //!< A-coordinate of end point |
641 |
|
double BB_2, //!< B-coordinate of end point |
642 |
|
double CC_2, //!< C-coordinate of end point |
643 |
|
double u_2, |
644 |
|
double v_2, |
645 |
|
double w_2, |
646 |
|
double x1, //!< X-coordinate of start point |
647 |
|
double y1, //!< Y-coordinate of start point |
648 |
|
double z1, //!< Z-coordinate of start point |
649 |
|
double AA_1, //!< A-coordinate of start point |
650 |
|
double BB_1, //!< B-coordinate of start point |
651 |
|
double CC_1, //!< C-coordinate of start point |
652 |
|
double u_1, |
653 |
|
double v_1, |
654 |
|
double w_1 |
655 |
|
) |
656 |
|
{ |
657 |
|
#define tiny 1e-7 |
658 |
12 |
if ( (fabs(x1-x2) > tiny) || (fabs(y1-y2) > tiny) || (fabs(z1-z2) > tiny) ) |
659 |
12 |
return sqrt(pow((x2 - x1), 2) + pow((y2 - y1), 2) + pow((z2 - z1), 2)); |
660 |
|
else if ( (fabs(u_1-u_2) > tiny) || (fabs(v_1-v_2) > tiny) || (fabs(w_1-w_2) > tiny) ) |
661 |
|
return sqrt(pow((u_2 - u_1), 2) + pow((v_2 - v_1), 2) + pow((w_2 - w_1), 2)); |
662 |
|
else |
663 |
|
return sqrt(pow((AA_2 - AA_1), 2) + pow((BB_2 - BB_1), 2) + pow((CC_2 - CC_1), 2)); |
664 |
|
} |
665 |
|
|
666 |
|
/****************************************************************************/ |
667 |
|
|
668 |
|
/*! find_turn |
669 |
|
|
670 |
|
Returned Value: double (angle in radians between two radii of a circle) |
671 |
|
|
672 |
|
Side effects: none |
673 |
|
|
674 |
|
Called by: find_arc_length |
675 |
|
|
676 |
|
All angles are in radians. |
677 |
|
|
678 |
|
*/ |
679 |
|
|
680 |
544 |
double Interp::find_turn(double x1, //!< X-coordinate of start point |
681 |
|
double y1, //!< Y-coordinate of start point |
682 |
|
double center_x, //!< X-coordinate of arc center |
683 |
|
double center_y, //!< Y-coordinate of arc center |
684 |
|
int turn, //!< no. of full or partial circles CCW |
685 |
|
double x2, //!< X-coordinate of end point |
686 |
|
double y2) //!< Y-coordinate of end point |
687 |
|
{ |
688 |
|
double alpha; /* angle of first radius */ |
689 |
|
double beta; /* angle of second radius */ |
690 |
|
double theta; /* amount of turn of arc CCW - negative if CW */ |
691 |
|
|
692 |
544 |
if (turn == 0) |
693 |
|
return 0.0; |
694 |
544 |
alpha = atan2((y1 - center_y), (x1 - center_x)); |
695 |
544 |
beta = atan2((y2 - center_y), (x2 - center_x)); |
696 |
544 |
if (turn > 0) { |
697 |
180 |
if (beta <= alpha) |
698 |
107 |
beta = (beta + (2 * M_PIl)); |
699 |
180 |
theta = ((beta - alpha) + ((turn - 1) * (2 * M_PIl))); |
700 |
|
} else { /* turn < 0 */ |
701 |
|
|
702 |
364 |
if (alpha <= beta) |
703 |
121 |
alpha = (alpha + (2 * M_PIl)); |
704 |
364 |
theta = ((beta - alpha) + ((turn + 1) * (2 * M_PIl))); |
705 |
|
} |
706 |
544 |
return (theta); |
707 |
|
} |
708 |
|
|
709 |
36 |
int Interp::find_tool_pocket(setup_pointer settings, int toolno, int *pocket) |
710 |
|
{ |
711 |
36 |
if(!settings->random_toolchanger && toolno == 0) { |
712 |
|
*pocket = 0; |
713 |
|
return INTERP_OK; |
714 |
|
} |
715 |
36 |
*pocket = -1; |
716 |
2052 |
for(int i=0; i<CANON_POCKETS_MAX; i++) { |
717 |
2016 |
if(settings->tool_table[i].toolno == toolno) |
718 |
67 |
*pocket = i; |
719 |
|
} |
720 |
|
|
721 |
36 |
CHKS((*pocket == -1), (_("Requested tool %d not found in the tool table")), toolno); |
722 |
|
return INTERP_OK; |
723 |
|
} |