1 |
|
/******************************************************************** |
2 |
|
* Description: interp_queue.cc |
3 |
|
* |
4 |
|
* Author: Chris Radek |
5 |
|
* License: GPL Version 2 |
6 |
|
* System: Linux |
7 |
|
* |
8 |
|
* Copyright (c) 2009 All rights reserved. |
9 |
|
* |
10 |
|
********************************************************************/ |
11 |
|
|
12 |
|
#include <string.h> |
13 |
|
#include <stdlib.h> |
14 |
|
#include <math.h> |
15 |
|
|
16 |
|
#include "rs274ngc.hh" |
17 |
|
#include "rs274ngc_return.hh" |
18 |
|
#include "interp_queue.hh" |
19 |
|
#include "interp_internal.hh" |
20 |
|
#include "rs274ngc_interp.hh" |
21 |
|
|
22 |
|
static int debug_qc = 0; |
23 |
|
|
24 |
|
// lathe tools have strange origin points that are not at |
25 |
|
// the center of the radius. This means that the point that |
26 |
|
// radius compensation controls (center of radius) is not at |
27 |
|
// the tool's origin. These functions do the necessary |
28 |
|
// translation. Notice tool orientations 0 (mill) and 9, and |
29 |
|
// those with radius 0 (a point) do not need any translation. |
30 |
|
|
31 |
1482 |
static double latheorigin_x(setup_pointer settings, double x) { |
32 |
1482 |
int o = settings->cutter_comp_orientation; |
33 |
1482 |
double r = settings->cutter_comp_radius; |
34 |
1482 |
if(settings->plane != CANON_PLANE_XZ) return x; |
35 |
|
|
36 |
170 |
if(o==2 || o==6 || o==1) x -= r; |
37 |
170 |
if(o==3 || o==8 || o==4) x += r; |
38 |
170 |
return x; |
39 |
|
} |
40 |
|
|
41 |
1482 |
static double latheorigin_z(setup_pointer settings, double z) { |
42 |
1482 |
int o = settings->cutter_comp_orientation; |
43 |
1482 |
double r = settings->cutter_comp_radius; |
44 |
1482 |
if(settings->plane != CANON_PLANE_XZ) return z; |
45 |
|
|
46 |
170 |
if(o==2 || o==7 || o==3) z -= r; |
47 |
170 |
if(o==1 || o==5 || o==4) z += r; |
48 |
170 |
return z; |
49 |
|
} |
50 |
|
|
51 |
|
static double endpoint[2]; |
52 |
|
static int endpoint_valid = 0; |
53 |
|
|
54 |
12463 |
std::vector<queued_canon>& qc(void) { |
55 |
12531 |
static std::vector<queued_canon> c; |
56 |
|
if(0) printf("len %d\n", (int)c.size()); |
57 |
12463 |
return c; |
58 |
|
} |
59 |
|
|
60 |
256 |
void qc_reset(void) { |
61 |
256 |
if(debug_qc) printf("qc cleared\n"); |
62 |
256 |
qc().clear(); |
63 |
256 |
endpoint_valid = 0; |
64 |
256 |
} |
65 |
|
|
66 |
591 |
void enqueue_SET_FEED_RATE(double feed) { |
67 |
1182 |
if(qc().empty()) { |
68 |
423 |
if(debug_qc) printf("immediate set feed rate %f\n", feed); |
69 |
423 |
SET_FEED_RATE(feed); |
70 |
1014 |
return; |
71 |
|
} |
72 |
|
queued_canon q; |
73 |
168 |
q.type = QSET_FEED_RATE; |
74 |
168 |
q.data.set_feed_rate.feed = feed; |
75 |
168 |
if(debug_qc) printf("enqueue set feed rate %f\n", feed); |
76 |
168 |
qc().push_back(q); |
77 |
|
} |
78 |
|
|
79 |
3 |
void enqueue_DWELL(double time) { |
80 |
6 |
if(qc().empty()) { |
81 |
3 |
if(debug_qc) printf("immediate dwell %f\n", time); |
82 |
3 |
DWELL(time); |
83 |
6 |
return; |
84 |
|
} |
85 |
|
queued_canon q; |
86 |
|
q.type = QDWELL; |
87 |
|
q.data.dwell.time = time; |
88 |
|
if(debug_qc) printf("enqueue dwell %f\n", time); |
89 |
|
qc().push_back(q); |
90 |
|
} |
91 |
|
|
92 |
13 |
void enqueue_SET_FEED_MODE(int mode) { |
93 |
26 |
if(qc().empty()) { |
94 |
7 |
if(debug_qc) printf("immediate set feed mode %d\n", mode); |
95 |
7 |
SET_FEED_MODE(mode); |
96 |
20 |
return; |
97 |
|
} |
98 |
|
queued_canon q; |
99 |
6 |
q.type = QSET_FEED_MODE; |
100 |
6 |
q.data.set_feed_mode.mode = mode; |
101 |
6 |
if(debug_qc) printf("enqueue set feed mode %d\n", mode); |
102 |
6 |
qc().push_back(q); |
103 |
|
} |
104 |
|
|
105 |
|
void enqueue_MIST_ON(void) { |
106 |
|
if(qc().empty()) { |
107 |
|
if(debug_qc) printf("immediate mist on\n"); |
108 |
|
MIST_ON(); |
109 |
|
return; |
110 |
|
} |
111 |
|
queued_canon q; |
112 |
|
q.type = QMIST_ON; |
113 |
|
if(debug_qc) printf("enqueue mist on\n"); |
114 |
|
qc().push_back(q); |
115 |
|
} |
116 |
|
|
117 |
|
void enqueue_MIST_OFF(void) { |
118 |
|
if(qc().empty()) { |
119 |
|
if(debug_qc) printf("immediate mist off\n"); |
120 |
|
MIST_OFF(); |
121 |
|
return; |
122 |
|
} |
123 |
|
queued_canon q; |
124 |
|
q.type = QMIST_OFF; |
125 |
|
if(debug_qc) printf("enqueue mist off\n"); |
126 |
|
qc().push_back(q); |
127 |
|
} |
128 |
|
|
129 |
|
void enqueue_FLOOD_ON(void) { |
130 |
|
if(qc().empty()) { |
131 |
|
if(debug_qc) printf("immediate flood on\n"); |
132 |
|
FLOOD_ON(); |
133 |
|
return; |
134 |
|
} |
135 |
|
queued_canon q; |
136 |
|
q.type = QFLOOD_ON; |
137 |
|
if(debug_qc) printf("enqueue flood on\n"); |
138 |
|
qc().push_back(q); |
139 |
|
} |
140 |
|
|
141 |
|
void enqueue_FLOOD_OFF(void) { |
142 |
|
if(qc().empty()) { |
143 |
|
if(debug_qc) printf("immediate flood on\n"); |
144 |
|
FLOOD_OFF(); |
145 |
|
return; |
146 |
|
} |
147 |
|
queued_canon q; |
148 |
|
q.type = QFLOOD_OFF; |
149 |
|
if(debug_qc) printf("enqueue flood off\n"); |
150 |
|
qc().push_back(q); |
151 |
|
} |
152 |
|
|
153 |
4 |
void enqueue_START_SPINDLE_CLOCKWISE(void) { |
154 |
8 |
if(qc().empty()) { |
155 |
4 |
if(debug_qc) printf("immediate spindle clockwise\n"); |
156 |
4 |
START_SPINDLE_CLOCKWISE(); |
157 |
8 |
return; |
158 |
|
} |
159 |
|
queued_canon q; |
160 |
|
q.type = QSTART_SPINDLE_CLOCKWISE; |
161 |
|
if(debug_qc) printf("enqueue spindle clockwise\n"); |
162 |
|
qc().push_back(q); |
163 |
|
} |
164 |
|
|
165 |
|
void enqueue_START_SPINDLE_COUNTERCLOCKWISE(void) { |
166 |
|
if(qc().empty()) { |
167 |
|
if(debug_qc) printf("immediate spindle counterclockwise\n"); |
168 |
|
START_SPINDLE_COUNTERCLOCKWISE(); |
169 |
|
return; |
170 |
|
} |
171 |
|
queued_canon q; |
172 |
|
q.type = QSTART_SPINDLE_COUNTERCLOCKWISE; |
173 |
|
if(debug_qc) printf("enqueue spindle counterclockwise\n"); |
174 |
|
qc().push_back(q); |
175 |
|
} |
176 |
|
|
177 |
4 |
void enqueue_STOP_SPINDLE_TURNING(void) { |
178 |
8 |
if(qc().empty()) { |
179 |
3 |
if(debug_qc) printf("immediate spindle stop\n"); |
180 |
3 |
STOP_SPINDLE_TURNING(); |
181 |
7 |
return; |
182 |
|
} |
183 |
|
queued_canon q; |
184 |
1 |
q.type = QSTOP_SPINDLE_TURNING; |
185 |
1 |
if(debug_qc) printf("enqueue spindle stop\n"); |
186 |
1 |
qc().push_back(q); |
187 |
|
} |
188 |
|
|
189 |
4 |
void enqueue_ORIENT_SPINDLE(double orientation, int mode) { |
190 |
8 |
if(qc().empty()) { |
191 |
4 |
if(debug_qc) printf("immediate spindle orient\n"); |
192 |
4 |
ORIENT_SPINDLE(orientation,mode); |
193 |
8 |
return; |
194 |
|
} |
195 |
|
queued_canon q; |
196 |
|
q.type = QORIENT_SPINDLE; |
197 |
|
q.data.orient_spindle.orientation = orientation; |
198 |
|
q.data.orient_spindle.mode = mode; |
199 |
|
if(debug_qc) printf("enqueue spindle orient\n"); |
200 |
|
qc().push_back(q); |
201 |
|
} |
202 |
|
|
203 |
2 |
void enqueue_WAIT_ORIENT_SPINDLE_COMPLETE(double timeout) { |
204 |
4 |
if(qc().empty()) { |
205 |
2 |
if(debug_qc) printf("immediate wait spindle orient complete\n"); |
206 |
2 |
WAIT_SPINDLE_ORIENT_COMPLETE(timeout); |
207 |
4 |
return; |
208 |
|
} |
209 |
|
queued_canon q; |
210 |
|
q.type = QWAIT_ORIENT_SPINDLE_COMPLETE; |
211 |
|
q.data.wait_orient_spindle_complete.timeout = timeout; |
212 |
|
if(debug_qc) printf("enqueue wait spindle orient complete\n"); |
213 |
|
qc().push_back(q); |
214 |
|
} |
215 |
|
|
216 |
|
void enqueue_SET_SPINDLE_MODE(double mode) { |
217 |
|
if(qc().empty()) { |
218 |
|
if(debug_qc) printf("immediate spindle mode %f\n", mode); |
219 |
|
SET_SPINDLE_MODE(mode); |
220 |
|
return; |
221 |
|
} |
222 |
|
queued_canon q; |
223 |
|
q.type = QSET_SPINDLE_MODE; |
224 |
|
q.data.set_spindle_mode.mode = mode; |
225 |
|
if(debug_qc) printf("enqueue spindle mode %f\n", mode); |
226 |
|
qc().push_back(q); |
227 |
|
} |
228 |
|
|
229 |
7 |
void enqueue_SET_SPINDLE_SPEED(double speed) { |
230 |
14 |
if(qc().empty()) { |
231 |
7 |
if(debug_qc) printf("immediate set spindle speed %f\n", speed); |
232 |
7 |
SET_SPINDLE_SPEED(speed); |
233 |
14 |
return; |
234 |
|
} |
235 |
|
queued_canon q; |
236 |
|
q.type = QSET_SPINDLE_SPEED; |
237 |
|
q.data.set_spindle_speed.speed = speed; |
238 |
|
if(debug_qc) printf("enqueue set spindle speed %f\n", speed); |
239 |
|
qc().push_back(q); |
240 |
|
} |
241 |
|
|
242 |
1164 |
void enqueue_COMMENT(const char *c) { |
243 |
2328 |
if(qc().empty()) { |
244 |
287 |
if(debug_qc) printf("immediate comment \"%s\"\n", c); |
245 |
287 |
COMMENT(c); |
246 |
1451 |
return; |
247 |
|
} |
248 |
|
queued_canon q; |
249 |
877 |
q.type = QCOMMENT; |
250 |
877 |
q.data.comment.comment = strdup(c); |
251 |
877 |
if(debug_qc) printf("enqueue comment \"%s\"\n", c); |
252 |
877 |
qc().push_back(q); |
253 |
|
} |
254 |
|
|
255 |
306 |
int enqueue_STRAIGHT_FEED(setup_pointer settings, int l, |
256 |
|
double dx, double dy, double dz, |
257 |
|
double x, double y, double z, |
258 |
|
double a, double b, double c, |
259 |
|
double u, double v, double w) { |
260 |
|
queued_canon q; |
261 |
306 |
q.type = QSTRAIGHT_FEED; |
262 |
306 |
q.data.straight_feed.line_number = l; |
263 |
306 |
switch(settings->plane) { |
264 |
|
case CANON_PLANE_XY: |
265 |
274 |
q.data.straight_feed.dx = dx; |
266 |
274 |
q.data.straight_feed.dy = dy; |
267 |
274 |
q.data.straight_feed.dz = dz; |
268 |
274 |
q.data.straight_feed.x = x; |
269 |
274 |
q.data.straight_feed.y = y; |
270 |
274 |
q.data.straight_feed.z = z; |
271 |
274 |
break; |
272 |
|
case CANON_PLANE_XZ: |
273 |
32 |
q.data.straight_feed.dz = dx; |
274 |
32 |
q.data.straight_feed.dx = dy; |
275 |
32 |
q.data.straight_feed.dy = dz; |
276 |
32 |
q.data.straight_feed.z = x; |
277 |
32 |
q.data.straight_feed.x = y; |
278 |
32 |
q.data.straight_feed.y = z; |
279 |
32 |
break; |
280 |
|
default: |
281 |
|
; |
282 |
|
} |
283 |
306 |
q.data.straight_feed.a = a; |
284 |
306 |
q.data.straight_feed.b = b; |
285 |
306 |
q.data.straight_feed.c = c; |
286 |
306 |
q.data.straight_feed.u = u; |
287 |
306 |
q.data.straight_feed.v = v; |
288 |
306 |
q.data.straight_feed.w = w; |
289 |
306 |
qc().push_back(q); |
290 |
306 |
if(debug_qc) printf("enqueue straight feed lineno %d to %f %f %f direction %f %f %f\n", l, x,y,z, dx, dy, dz); |
291 |
306 |
return 0; |
292 |
|
} |
293 |
|
|
294 |
3 |
int enqueue_STRAIGHT_TRAVERSE(setup_pointer settings, int l, |
295 |
|
double dx, double dy, double dz, |
296 |
|
double x, double y, double z, |
297 |
|
double a, double b, double c, |
298 |
|
double u, double v, double w) { |
299 |
|
queued_canon q; |
300 |
3 |
q.type = QSTRAIGHT_TRAVERSE; |
301 |
3 |
q.data.straight_traverse.line_number = l; |
302 |
3 |
switch(settings->plane) { |
303 |
|
case CANON_PLANE_XY: |
304 |
3 |
q.data.straight_traverse.dx = dx; |
305 |
3 |
q.data.straight_traverse.dy = dy; |
306 |
3 |
q.data.straight_traverse.dz = dz; |
307 |
3 |
q.data.straight_traverse.x = x; |
308 |
3 |
q.data.straight_traverse.y = y; |
309 |
3 |
q.data.straight_traverse.z = z; |
310 |
3 |
break; |
311 |
|
case CANON_PLANE_XZ: |
312 |
|
q.data.straight_traverse.dz = dx; |
313 |
|
q.data.straight_traverse.dx = dy; |
314 |
|
q.data.straight_traverse.dy = dz; |
315 |
|
q.data.straight_traverse.z = x; |
316 |
|
q.data.straight_traverse.x = y; |
317 |
|
q.data.straight_traverse.y = z; |
318 |
|
break; |
319 |
|
default: |
320 |
|
; |
321 |
|
} |
322 |
3 |
q.data.straight_traverse.a = a; |
323 |
3 |
q.data.straight_traverse.b = b; |
324 |
3 |
q.data.straight_traverse.c = c; |
325 |
3 |
q.data.straight_traverse.u = u; |
326 |
3 |
q.data.straight_traverse.v = v; |
327 |
3 |
q.data.straight_traverse.w = w; |
328 |
3 |
if(debug_qc) printf("enqueue straight traverse lineno %d to %f %f %f direction %f %f %f\n", l, x,y,z, dx, dy, dz); |
329 |
3 |
qc().push_back(q); |
330 |
3 |
return 0; |
331 |
|
} |
332 |
|
|
333 |
587 |
void enqueue_ARC_FEED(setup_pointer settings, int l, |
334 |
|
double original_turns, |
335 |
|
double end1, double end2, double center1, double center2, |
336 |
|
int turn, |
337 |
|
double end3, |
338 |
|
double a, double b, double c, |
339 |
|
double u, double v, double w) { |
340 |
|
queued_canon q; |
341 |
|
|
342 |
587 |
q.type = QARC_FEED; |
343 |
587 |
q.data.arc_feed.line_number = l; |
344 |
587 |
q.data.arc_feed.original_turns = original_turns; |
345 |
587 |
q.data.arc_feed.end1 = end1; |
346 |
587 |
q.data.arc_feed.end2 = end2; |
347 |
587 |
q.data.arc_feed.center1 = center1; |
348 |
587 |
q.data.arc_feed.center2 = center2; |
349 |
587 |
q.data.arc_feed.turn = turn; |
350 |
587 |
q.data.arc_feed.end3 = end3; |
351 |
587 |
q.data.arc_feed.a = a; |
352 |
587 |
q.data.arc_feed.b = b; |
353 |
587 |
q.data.arc_feed.c = c; |
354 |
587 |
q.data.arc_feed.u = u; |
355 |
587 |
q.data.arc_feed.v = v; |
356 |
587 |
q.data.arc_feed.w = w; |
357 |
|
|
358 |
587 |
if(debug_qc) printf("enqueue arc lineno %d to %f %f center %f %f turn %d sweeping %f\n", l, end1, end2, center1, center2, turn, original_turns); |
359 |
587 |
qc().push_back(q); |
360 |
587 |
} |
361 |
|
|
362 |
|
void enqueue_M_USER_COMMAND (int index, double p_number, double q_number) { |
363 |
|
if(qc().empty()) { |
364 |
|
if(debug_qc) printf("immediate M_USER_COMMAND index=%d p=%f q=%f\n", |
365 |
|
index,p_number,q_number); |
366 |
|
(*(USER_DEFINED_FUNCTION[index - 100])) (index - 100,p_number,q_number); |
367 |
|
return; |
368 |
|
} |
369 |
|
queued_canon q; |
370 |
|
q.type = QM_USER_COMMAND; |
371 |
|
q.data.mcommand.index = index; |
372 |
|
q.data.mcommand.p_number = p_number; |
373 |
|
q.data.mcommand.q_number = q_number; |
374 |
|
if(debug_qc) printf("enqueue M_USER_COMMAND index=%d p=%f q=%f\n", |
375 |
|
index,p_number,q_number); |
376 |
|
qc().push_back(q); |
377 |
|
} |
378 |
|
|
379 |
|
void enqueue_START_CHANGE (void) { |
380 |
|
queued_canon q; |
381 |
|
q.type = QSTART_CHANGE; |
382 |
|
if(debug_qc) printf("enqueue START_CHANGE\n"); |
383 |
|
qc().push_back(q); |
384 |
|
} |
385 |
|
|
386 |
|
|
387 |
|
|
388 |
|
|
389 |
20 |
void qc_scale(double scale) { |
390 |
|
|
391 |
40 |
if(qc().empty()) { |
392 |
20 |
if(debug_qc) printf("not scaling because qc is empty\n"); |
393 |
20 |
return; |
394 |
|
} |
395 |
|
|
396 |
|
if(debug_qc) printf("scaling qc by %f\n", scale); |
397 |
|
|
398 |
|
for(unsigned int i = 0; i<qc().size(); i++) { |
399 |
|
queued_canon &q = qc()[i]; |
400 |
|
endpoint[0] *= scale; |
401 |
|
endpoint[1] *= scale; |
402 |
|
switch(q.type) { |
403 |
|
case QARC_FEED: |
404 |
|
q.data.arc_feed.end1 *= scale; |
405 |
|
q.data.arc_feed.end2 *= scale; |
406 |
|
q.data.arc_feed.end3 *= scale; |
407 |
|
q.data.arc_feed.center1 *= scale; |
408 |
|
q.data.arc_feed.center2 *= scale; |
409 |
|
q.data.arc_feed.u *= scale; |
410 |
|
q.data.arc_feed.v *= scale; |
411 |
|
q.data.arc_feed.w *= scale; |
412 |
|
break; |
413 |
|
case QSTRAIGHT_FEED: |
414 |
|
q.data.straight_feed.x *= scale; |
415 |
|
q.data.straight_feed.y *= scale; |
416 |
|
q.data.straight_feed.z *= scale; |
417 |
|
q.data.straight_feed.u *= scale; |
418 |
|
q.data.straight_feed.v *= scale; |
419 |
|
q.data.straight_feed.w *= scale; |
420 |
|
break; |
421 |
|
case QSTRAIGHT_TRAVERSE: |
422 |
|
q.data.straight_traverse.x *= scale; |
423 |
|
q.data.straight_traverse.y *= scale; |
424 |
|
q.data.straight_traverse.z *= scale; |
425 |
|
q.data.straight_traverse.u *= scale; |
426 |
|
q.data.straight_traverse.v *= scale; |
427 |
|
q.data.straight_traverse.w *= scale; |
428 |
|
break; |
429 |
|
default: |
430 |
|
; |
431 |
|
} |
432 |
|
} |
433 |
|
} |
434 |
|
|
435 |
979 |
void dequeue_canons(setup_pointer settings) { |
436 |
|
|
437 |
979 |
if(debug_qc) printf("dequeueing: endpoint is now invalid\n"); |
438 |
979 |
endpoint_valid = 0; |
439 |
|
|
440 |
2937 |
if(qc().empty()) return; |
441 |
|
|
442 |
4772 |
for(unsigned int i = 0; i<qc().size(); i++) { |
443 |
3894 |
queued_canon &q = qc()[i]; |
444 |
|
|
445 |
1947 |
switch(q.type) { |
446 |
|
case QARC_FEED: |
447 |
587 |
if(debug_qc) printf("issuing arc feed lineno %d\n", q.data.arc_feed.line_number); |
448 |
|
ARC_FEED(q.data.arc_feed.line_number, |
449 |
|
latheorigin_z(settings, q.data.arc_feed.end1), |
450 |
|
latheorigin_x(settings, q.data.arc_feed.end2), |
451 |
|
latheorigin_z(settings, q.data.arc_feed.center1), |
452 |
|
latheorigin_x(settings, q.data.arc_feed.center2), |
453 |
|
q.data.arc_feed.turn, |
454 |
|
q.data.arc_feed.end3, |
455 |
|
q.data.arc_feed.a, q.data.arc_feed.b, q.data.arc_feed.c, |
456 |
587 |
q.data.arc_feed.u, q.data.arc_feed.v, q.data.arc_feed.w); |
457 |
587 |
break; |
458 |
|
case QSTRAIGHT_FEED: |
459 |
305 |
if(debug_qc) printf("issuing straight feed lineno %d\n", q.data.straight_feed.line_number); |
460 |
|
STRAIGHT_FEED(q.data.straight_feed.line_number, |
461 |
|
latheorigin_x(settings, q.data.straight_feed.x), |
462 |
|
q.data.straight_feed.y, |
463 |
|
latheorigin_z(settings, q.data.straight_feed.z), |
464 |
|
q.data.straight_feed.a, q.data.straight_feed.b, q.data.straight_feed.c, |
465 |
305 |
q.data.straight_feed.u, q.data.straight_feed.v, q.data.straight_feed.w); |
466 |
305 |
break; |
467 |
|
case QSTRAIGHT_TRAVERSE: |
468 |
3 |
if(debug_qc) printf("issuing straight traverse lineno %d\n", q.data.straight_traverse.line_number); |
469 |
|
STRAIGHT_TRAVERSE(q.data.straight_traverse.line_number, |
470 |
|
latheorigin_x(settings, q.data.straight_traverse.x), |
471 |
|
q.data.straight_traverse.y, |
472 |
|
latheorigin_z(settings, q.data.straight_traverse.z), |
473 |
|
q.data.straight_traverse.a, q.data.straight_traverse.b, q.data.straight_traverse.c, |
474 |
3 |
q.data.straight_traverse.u, q.data.straight_traverse.v, q.data.straight_traverse.w); |
475 |
3 |
break; |
476 |
|
case QSET_FEED_RATE: |
477 |
168 |
if(debug_qc) printf("issuing set feed rate\n"); |
478 |
168 |
SET_FEED_RATE(q.data.set_feed_rate.feed); |
479 |
168 |
break; |
480 |
|
case QDWELL: |
481 |
|
if(debug_qc) printf("issuing dwell\n"); |
482 |
|
DWELL(q.data.dwell.time); |
483 |
|
break; |
484 |
|
case QSET_FEED_MODE: |
485 |
6 |
if(debug_qc) printf("issuing set feed mode\n"); |
486 |
6 |
SET_FEED_MODE(q.data.set_feed_mode.mode); |
487 |
6 |
break; |
488 |
|
case QMIST_ON: |
489 |
|
if(debug_qc) printf("issuing mist on\n"); |
490 |
|
MIST_ON(); |
491 |
|
break; |
492 |
|
case QMIST_OFF: |
493 |
|
if(debug_qc) printf("issuing mist off\n"); |
494 |
|
MIST_OFF(); |
495 |
|
break; |
496 |
|
case QFLOOD_ON: |
497 |
|
if(debug_qc) printf("issuing flood on\n"); |
498 |
|
FLOOD_ON(); |
499 |
|
break; |
500 |
|
case QFLOOD_OFF: |
501 |
|
if(debug_qc) printf("issuing flood off\n"); |
502 |
|
FLOOD_OFF(); |
503 |
|
break; |
504 |
|
case QSTART_SPINDLE_CLOCKWISE: |
505 |
|
if(debug_qc) printf("issuing spindle clockwise\n"); |
506 |
|
START_SPINDLE_CLOCKWISE(); |
507 |
|
break; |
508 |
|
case QSTART_SPINDLE_COUNTERCLOCKWISE: |
509 |
|
if(debug_qc) printf("issuing spindle counterclockwise\n"); |
510 |
|
START_SPINDLE_COUNTERCLOCKWISE(); |
511 |
|
break; |
512 |
|
case QSTOP_SPINDLE_TURNING: |
513 |
1 |
if(debug_qc) printf("issuing stop spindle\n"); |
514 |
1 |
STOP_SPINDLE_TURNING(); |
515 |
1 |
break; |
516 |
|
case QSET_SPINDLE_MODE: |
517 |
|
if(debug_qc) printf("issuing set spindle mode\n"); |
518 |
|
SET_SPINDLE_MODE(q.data.set_spindle_mode.mode); |
519 |
|
break; |
520 |
|
case QSET_SPINDLE_SPEED: |
521 |
|
if(debug_qc) printf("issuing set spindle speed\n"); |
522 |
|
SET_SPINDLE_SPEED(q.data.set_spindle_speed.speed); |
523 |
|
break; |
524 |
|
case QCOMMENT: |
525 |
877 |
if(debug_qc) printf("issuing comment\n"); |
526 |
877 |
COMMENT(q.data.comment.comment); |
527 |
877 |
free(q.data.comment.comment); |
528 |
877 |
break; |
529 |
|
case QM_USER_COMMAND: |
530 |
|
if(debug_qc) printf("issuing mcommand\n"); |
531 |
|
{int index=q.data.mcommand.index; |
532 |
|
(*(USER_DEFINED_FUNCTION[index - 100])) (index -100, |
533 |
|
q.data.mcommand.p_number, |
534 |
|
q.data.mcommand.q_number); |
535 |
|
} |
536 |
|
break; |
537 |
|
case QSTART_CHANGE: |
538 |
|
if(debug_qc) printf("issuing start_change\n"); |
539 |
|
START_CHANGE(); |
540 |
|
free(q.data.comment.comment); |
541 |
|
break; |
542 |
|
case QORIENT_SPINDLE: |
543 |
|
if(debug_qc) printf("issuing orient spindle\n"); |
544 |
|
ORIENT_SPINDLE(q.data.orient_spindle.orientation, q.data.orient_spindle.mode); |
545 |
|
break; |
546 |
|
case QWAIT_ORIENT_SPINDLE_COMPLETE: |
547 |
|
if(debug_qc) printf("issuing wait orient spindle complete\n"); |
548 |
|
WAIT_SPINDLE_ORIENT_COMPLETE(q.data.wait_orient_spindle_complete.timeout); |
549 |
|
break; |
550 |
|
} |
551 |
|
} |
552 |
878 |
qc().clear(); |
553 |
|
} |
554 |
|
|
555 |
349 |
int Interp::move_endpoint_and_flush(setup_pointer settings, double x, double y) { |
556 |
|
double x1; |
557 |
|
double y1; |
558 |
|
double x2; |
559 |
|
double y2; |
560 |
|
double dot; |
561 |
|
|
562 |
698 |
if(qc().empty()) return 0; |
563 |
|
|
564 |
1250 |
for(unsigned int i = 0; i<qc().size(); i++) { |
565 |
|
// there may be several moves in the queue, and we need to |
566 |
|
// change all of them. consider moving into a concave corner, |
567 |
|
// then up and back down, then continuing on. there will be |
568 |
|
// three moves to change. |
569 |
|
|
570 |
946 |
queued_canon &q = qc()[i]; |
571 |
|
|
572 |
473 |
switch(q.type) { |
573 |
|
case QARC_FEED: |
574 |
|
double r1, r2, l1, l2; |
575 |
|
r1 = hypot(q.data.arc_feed.end1 - q.data.arc_feed.center1, |
576 |
107 |
q.data.arc_feed.end2 - q.data.arc_feed.center2); |
577 |
107 |
l1 = q.data.arc_feed.original_turns; |
578 |
107 |
q.data.arc_feed.end1 = x; |
579 |
107 |
q.data.arc_feed.end2 = y; |
580 |
|
r2 = hypot(x - q.data.arc_feed.center1, |
581 |
107 |
y - q.data.arc_feed.center2); |
582 |
|
l2 = find_turn(endpoint[0], endpoint[1], |
583 |
|
q.data.arc_feed.center1, q.data.arc_feed.center2, |
584 |
|
q.data.arc_feed.turn, |
585 |
107 |
x, y); |
586 |
107 |
if(debug_qc) printf("moving endpoint of arc lineno %d old sweep %f new sweep %f\n", q.data.arc_feed.line_number, l1, l2); |
587 |
|
|
588 |
107 |
if(fabs(r1-r2) > .01) |
589 |
|
ERS(_("BUG: cutter compensation has generated an invalid arc with mismatched radii r1 %f r2 %f\n"), r1, r2); |
590 |
107 |
if(l1 && endpoint_valid && fabs(l2) > fabs(l1) + (settings->length_units == CANON_UNITS_MM? .0254 : .001)) { |
591 |
|
ERS(_("Arc move in concave corner cannot be reached by the tool without gouging")); |
592 |
|
} |
593 |
107 |
q.data.arc_feed.end1 = x; |
594 |
107 |
q.data.arc_feed.end2 = y; |
595 |
107 |
break; |
596 |
|
case QSTRAIGHT_TRAVERSE: |
597 |
1 |
switch(settings->plane) { |
598 |
|
case CANON_PLANE_XY: |
599 |
1 |
x1 = q.data.straight_traverse.dx; // direction of original motion |
600 |
1 |
y1 = q.data.straight_traverse.dy; |
601 |
1 |
x2 = x - endpoint[0]; // new direction after clipping |
602 |
1 |
y2 = y - endpoint[1]; |
603 |
1 |
break; |
604 |
|
case CANON_PLANE_XZ: |
605 |
|
x1 = q.data.straight_traverse.dz; // direction of original motion |
606 |
|
y1 = q.data.straight_traverse.dx; |
607 |
|
x2 = x - endpoint[0]; // new direction after clipping |
608 |
|
y2 = y - endpoint[1]; |
609 |
|
break; |
610 |
|
default: |
611 |
|
ERS(_("BUG: Unsupported plane in cutter compensation")); |
612 |
|
} |
613 |
|
|
614 |
1 |
dot = x1 * x2 + y1 * y2; // not normalized; we only care about the angle |
615 |
1 |
if(debug_qc) printf("moving endpoint of traverse old dir %f new dir %f dot %f endpoint_valid %d\n", atan2(y1,x1), atan2(y2,x2), dot, endpoint_valid); |
616 |
|
|
617 |
1 |
if(endpoint_valid && dot<0) { |
618 |
|
// oops, the move is the wrong way. this means the |
619 |
|
// path has crossed because we backed up further |
620 |
|
// than the line is long. this will gouge. |
621 |
|
ERS(_("Straight traverse in concave corner cannot be reached by the tool without gouging")); |
622 |
|
} |
623 |
1 |
switch(settings->plane) { |
624 |
|
case CANON_PLANE_XY: |
625 |
1 |
q.data.straight_traverse.x = x; |
626 |
1 |
q.data.straight_traverse.y = y; |
627 |
1 |
break; |
628 |
|
case CANON_PLANE_XZ: |
629 |
|
q.data.straight_traverse.z = x; |
630 |
|
q.data.straight_traverse.x = y; |
631 |
|
break; |
632 |
|
} |
633 |
|
break; |
634 |
|
case QSTRAIGHT_FEED: |
635 |
199 |
switch(settings->plane) { |
636 |
|
case CANON_PLANE_XY: |
637 |
177 |
x1 = q.data.straight_feed.dx; // direction of original motion |
638 |
177 |
y1 = q.data.straight_feed.dy; |
639 |
177 |
x2 = x - endpoint[0]; // new direction after clipping |
640 |
177 |
y2 = y - endpoint[1]; |
641 |
177 |
break; |
642 |
|
case CANON_PLANE_XZ: |
643 |
22 |
x1 = q.data.straight_feed.dz; // direction of original motion |
644 |
22 |
y1 = q.data.straight_feed.dx; |
645 |
22 |
x2 = x - endpoint[0]; // new direction after clipping |
646 |
22 |
y2 = y - endpoint[1]; |
647 |
22 |
break; |
648 |
|
default: |
649 |
|
ERS(_("BUG: Unsupported plane [%d] in cutter compensation"), |
650 |
|
settings->plane); |
651 |
|
} |
652 |
|
|
653 |
199 |
dot = x1 * x2 + y1 * y2; |
654 |
199 |
if(debug_qc) printf("moving endpoint of feed old dir %f new dir %f dot %f endpoint_valid %d\n", atan2(y1,x1), atan2(y2,x2), dot, endpoint_valid); |
655 |
|
|
656 |
199 |
if(endpoint_valid && dot<0) { |
657 |
|
// oops, the move is the wrong way. this means the |
658 |
|
// path has crossed because we backed up further |
659 |
|
// than the line is long. this will gouge. |
660 |
1 |
ERS(_("Straight feed in concave corner cannot be reached by the tool without gouging")); |
661 |
|
} |
662 |
198 |
switch(settings->plane) { |
663 |
|
case CANON_PLANE_XY: |
664 |
176 |
q.data.straight_feed.x = x; |
665 |
176 |
q.data.straight_feed.y = y; |
666 |
176 |
break; |
667 |
|
case CANON_PLANE_XZ: |
668 |
22 |
q.data.straight_feed.z = x; |
669 |
22 |
q.data.straight_feed.x = y; |
670 |
22 |
break; |
671 |
|
} |
672 |
|
break; |
673 |
|
default: |
674 |
|
// other things are not moves - we don't have to mess with them. |
675 |
|
; |
676 |
|
} |
677 |
|
} |
678 |
305 |
dequeue_canons(settings); |
679 |
305 |
set_endpoint(x, y); |
680 |
305 |
return 0; |
681 |
|
} |
682 |
|
|
683 |
877 |
void set_endpoint(double x, double y) { |
684 |
877 |
if(debug_qc) printf("setting endpoint %f %f\n", x, y); |
685 |
877 |
endpoint[0] = x; endpoint[1] = y; |
686 |
877 |
endpoint_valid = 1; |
687 |
877 |
} |