emccanon.cc
1 /******************************************************************** 2 * Description: emccanon.cc 3 * Canonical definitions for 3-axis NC application 4 * 5 * Derived from a work by Fred Proctor & Will Shackleford 6 * 7 * Author: 8 * License: GPL Version 2 9 * System: Linux 10 * 11 * Copyright (c) 2004 All rights reserved. 12 ********************************************************************/ 13 /* 14 15 Notes: 16 17 Units 18 ----- 19 Values are stored internally as mm and degree units, e.g, program 20 offsets, end point, tool length offset. These are "internal 21 units". "External units" are the units used by the EMC motion planner. 22 All lengths and units output by the interpreter are converted to 23 internal units here, using FROM_PROG_LEN,ANG, and then 24 TO_EXT_LEN(),ANG are called to convert these to external units. 25 26 Tool Length Offsets 27 ------------------- 28 The interpreter does not subtract off tool length offsets. It calls 29 USE_TOOL_LENGTH_OFFSETS(length), which we record here and apply to 30 all appropriate values subsequently. 31 */ 32 33 #include "config.h" 34 #include <stdio.h> 35 #include <stdarg.h> 36 #include <math.h> 37 #include <string.h> // strncpy() 38 #include <ctype.h> // isspace() 39 #include "emc.hh" // EMC NML 40 #include "emc_nml.hh" 41 #include "canon.hh" 42 #include "canon_position.hh" // data type for a machine position 43 #include "interpl.hh" // interp_list 44 #include "emcglb.h" // TRAJ_MAX_VELOCITY 45 46 //#define EMCCANON_DEBUG 47 48 //Simple compile-time debug macro 49 #ifdef EMCCANON_DEBUG 50 #define canon_debug(...) printf(__VA_ARGS__) 51 #else 52 #define canon_debug(...) 53 #endif 54 55 /* 56 Origin offsets, length units, and active plane are all maintained 57 here in this file. Controller runs in absolute mode, and does not 58 have plane select concept. 59 60 programOrigin is stored in mm always, and converted when set or read. 61 When it's applied to positions, convert positions to mm units first 62 and then add programOrigin. 63 64 Units are then converted from mm to external units, as reported by 65 the GET_EXTERNAL_LENGTH_UNITS() function. 66 */ 67 68 static CanonConfig_t canon; 69 70 static int debug_velacc = 0; 71 static const double tiny = 1e-7; 72 73 #ifndef MIN 74 #define MIN(a,b) ((a)<(b)?(a):(b)) 75 #endif 76 77 #ifndef MIN3 78 #define MIN3(a,b,c) (MIN(MIN((a),(b)),(c))) 79 #endif 80 81 #ifndef MAX 82 #define MAX(a,b) ((a)>(b)?(a):(b)) 83 #endif 84 85 #ifndef MAX3 86 #define MAX3(a,b,c) (MAX(MAX((a),(b)),(c))) 87 #endif 88 89 #ifndef MAX4 90 #define MAX4(a,b,c,d) (MAX(MAX((a),(b)),MAX((c),(d)))) 91 #endif 92 93 #ifndef MAX9 94 #define MAX9(a,b,c,d,e,f,g,h,i) (MAX3((MAX3(a,b,c)),(MAX3(d,e,f)),(MAX3(g,h,i)))) 95 #endif 96 97 /* macros for converting internal (mm/deg) units to external units */ 98 #define TO_EXT_LEN(mm) ((mm) * GET_EXTERNAL_LENGTH_UNITS()) 99 #define TO_EXT_ANG(deg) ((deg) * GET_EXTERNAL_ANGLE_UNITS()) 100 101 /* macros for converting external units to internal (mm/deg) units */ 102 #define FROM_EXT_LEN(ext) ((ext) / GET_EXTERNAL_LENGTH_UNITS()) 103 #define FROM_EXT_ANG(ext) ((ext) / GET_EXTERNAL_ANGLE_UNITS()) 104 105 /* macros for converting internal (mm/deg) units to program units */ 106 #define TO_PROG_LEN(mm) ((mm) / (canon.lengthUnits == CANON_UNITS_INCHES ? 25.4 : canon.lengthUnits == CANON_UNITS_CM ? 10.0 : 1.0)) 107 #define TO_PROG_ANG(deg) (deg) 108 109 /* macros for converting program units to internal (mm/deg) units */ 110 #define FROM_PROG_LEN(prog) ((prog) * (canon.lengthUnits == CANON_UNITS_INCHES ? 25.4 : canon.lengthUnits == CANON_UNITS_CM ? 10.0 : 1.0)) 111 #define FROM_PROG_ANG(prog) (prog) 112 113 /* Certain axes are periodic. Hardcode this for now */ 114 #define IS_PERIODIC(axisnum) \ 115 ((axisnum) == 3 || (axisnum) == 4 || (axisnum) == 5) 116 117 // this doesn't quite work yet: disable 118 #undef IS_PERIODIC 119 #define IS_PERIODIC(axisnum) (0) 120 121 #define AXIS_PERIOD(axisnum) (IS_PERIODIC(axisnum) ? 360 : 0) 122 123 //KLUDGE kinematic data struct (instead of returning a single float value) 124 //FIXME This should really be refactored into a more general structure, but this 125 //means tearing up the getStraightXXX functions, which probably means 126 //converting to canon_position operators 127 struct VelData { 128 double tmax; 129 double vel; 130 double dtot; 131 }; 132 133 struct AccelData{ 134 double tmax; 135 double acc; 136 double dtot; 137 }; 138 139 static PM_QUATERNION quat(1, 0, 0, 0); 140 141 static void flush_segments(void); 142 143 /* 144 These decls were from the old 3-axis canon.hh, and refer functions 145 defined here that are used for convenience but no longer have decls 146 in the 6-axis canon.hh. So, we declare them here now. 147 */ 148 149 #ifndef D2R 150 #define D2R(r) ((r)*M_PI/180.0) 151 #endif 152 153 static void rotate(double &x, double &y, double theta) { 154 double xx, yy; 155 double t = D2R(theta); 156 xx = x; 157 yy = y; 158 x = xx * cos(t) - yy * sin(t); 159 y = xx * sin(t) + yy * cos(t); 160 } 161 162 163 /** 164 * Implementation of planar rotation for a 3D vector. 165 * This is basically a shortcut for "rotate" when the values are stored in a 166 * cartesian vector. 167 * The use of static "xy_rotation" is ugly here, but is at least consistent. 168 */ 169 static void to_rotated(PM_CARTESIAN &vec) { 170 rotate(vec.x,vec.y,canon.xy_rotation); 171 } 172 #if 0 173 static void from_rotated(PM_CARTESIAN &vec) { 174 rotate(vec.x,vec.y,-canon.xy_rotation); 175 } 176 #endif 177 static void rotate_and_offset(CANON_POSITION & pos) { 178 179 pos += canon.g92Offset; 180 181 rotate(pos.x, pos.y, canon.xy_rotation); 182 183 pos += canon.g5xOffset; 184 185 pos += canon.toolOffset; 186 } 187 188 static void rotate_and_offset_xyz(PM_CARTESIAN & xyz) { 189 190 xyz += canon.g92Offset.xyz(); 191 192 rotate(xyz.x, xyz.y, canon.xy_rotation); 193 194 xyz += canon.g5xOffset.xyz(); 195 196 xyz += PM_CARTESIAN(canon.toolOffset.tran.x, 197 canon.toolOffset.tran.y, 198 canon.toolOffset.tran.z); 199 } 200 201 static CANON_POSITION unoffset_and_unrotate_pos(const CANON_POSITION pos) { 202 CANON_POSITION res; 203 204 res = pos; 205 206 res -= canon.toolOffset; 207 208 res -= canon.g5xOffset; 209 210 rotate(res.x, res.y, -canon.xy_rotation); 211 212 res -= canon.g92Offset; 213 214 return res; 215 } 216 217 static void rotate_and_offset_pos(double &x, double &y, double &z, double &a, double &b, double &c, double &u, double &v, double &w) { 218 x += canon.g92Offset.x; 219 y += canon.g92Offset.y; 220 z += canon.g92Offset.z; 221 a += canon.g92Offset.a; 222 b += canon.g92Offset.b; 223 c += canon.g92Offset.c; 224 u += canon.g92Offset.u; 225 v += canon.g92Offset.v; 226 w += canon.g92Offset.w; 227 228 rotate(x, y, canon.xy_rotation); 229 230 x += canon.g5xOffset.x; 231 y += canon.g5xOffset.y; 232 z += canon.g5xOffset.z; 233 a += canon.g5xOffset.a; 234 b += canon.g5xOffset.b; 235 c += canon.g5xOffset.c; 236 u += canon.g5xOffset.u; 237 v += canon.g5xOffset.v; 238 w += canon.g5xOffset.w; 239 240 x += canon.toolOffset.tran.x; 241 y += canon.toolOffset.tran.y; 242 z += canon.toolOffset.tran.z; 243 a += canon.toolOffset.a; 244 b += canon.toolOffset.b; 245 c += canon.toolOffset.c; 246 u += canon.toolOffset.u; 247 v += canon.toolOffset.v; 248 w += canon.toolOffset.w; 249 } 250 251 252 static CANON_POSITION unoffset_and_unrotate_pos(const EmcPose pos) { 253 CANON_POSITION res(pos); 254 return unoffset_and_unrotate_pos(res); 255 } 256 257 static void from_prog(double &x, double &y, double &z, double &a, double &b, double &c, double &u, double &v, double &w) { 258 x = FROM_PROG_LEN(x); 259 y = FROM_PROG_LEN(y); 260 z = FROM_PROG_LEN(z); 261 a = FROM_PROG_ANG(a); 262 b = FROM_PROG_ANG(b); 263 c = FROM_PROG_ANG(c); 264 u = FROM_PROG_LEN(u); 265 v = FROM_PROG_LEN(v); 266 w = FROM_PROG_LEN(w); 267 } 268 269 static void from_prog(CANON_POSITION &pos) { 270 pos.x = FROM_PROG_LEN(pos.x); 271 pos.y = FROM_PROG_LEN(pos.y); 272 pos.z = FROM_PROG_LEN(pos.z); 273 pos.a = FROM_PROG_ANG(pos.a); 274 pos.b = FROM_PROG_ANG(pos.b); 275 pos.c = FROM_PROG_ANG(pos.c); 276 pos.u = FROM_PROG_LEN(pos.u); 277 pos.v = FROM_PROG_LEN(pos.v); 278 pos.w = FROM_PROG_LEN(pos.w); 279 } 280 281 static void from_prog_len(PM_CARTESIAN &vec) { 282 vec.x = FROM_PROG_LEN(vec.x); 283 vec.y = FROM_PROG_LEN(vec.y); 284 vec.z = FROM_PROG_LEN(vec.z); 285 } 286 #if 0 287 static void to_ext(double &x, double &y, double &z, double &a, double &b, double &c, double &u, double &v, double &w) { 288 x = TO_EXT_LEN(x); 289 y = TO_EXT_LEN(y); 290 z = TO_EXT_LEN(z); 291 a = TO_EXT_ANG(a); 292 b = TO_EXT_ANG(b); 293 c = TO_EXT_ANG(c); 294 u = TO_EXT_LEN(u); 295 v = TO_EXT_LEN(v); 296 w = TO_EXT_LEN(w); 297 } 298 299 static void to_ext(CANON_POSITION & pos) { 300 pos.x=TO_EXT_LEN(pos.x); 301 pos.y=TO_EXT_LEN(pos.y); 302 pos.z=TO_EXT_LEN(pos.z); 303 pos.a=TO_EXT_ANG(pos.a); 304 pos.b=TO_EXT_ANG(pos.b); 305 pos.c=TO_EXT_ANG(pos.c); 306 pos.u=TO_EXT_LEN(pos.u); 307 pos.v=TO_EXT_LEN(pos.v); 308 pos.w=TO_EXT_LEN(pos.w); 309 } 310 #endif 311 312 static PM_CARTESIAN to_ext_len(const PM_CARTESIAN & pos) { 313 PM_CARTESIAN ret; 314 ret.x = TO_EXT_LEN(pos.x); 315 ret.y = TO_EXT_LEN(pos.y); 316 ret.z = TO_EXT_LEN(pos.z); 317 return ret; 318 } 319 320 static EmcPose to_ext_pose(double x, double y, double z, double a, double b, double c, double u, double v, double w) { 321 EmcPose result; 322 result.tran.x = TO_EXT_LEN(x); 323 result.tran.y = TO_EXT_LEN(y); 324 result.tran.z = TO_EXT_LEN(z); 325 result.a = TO_EXT_ANG(a); 326 result.b = TO_EXT_ANG(b); 327 result.c = TO_EXT_ANG(c); 328 result.u = TO_EXT_LEN(u); 329 result.v = TO_EXT_LEN(v); 330 result.w = TO_EXT_LEN(w); 331 return result; 332 } 333 334 static EmcPose to_ext_pose(const CANON_POSITION & pos) { 335 EmcPose result; 336 result.tran.x = TO_EXT_LEN(pos.x); 337 result.tran.y = TO_EXT_LEN(pos.y); 338 result.tran.z = TO_EXT_LEN(pos.z); 339 result.a = TO_EXT_ANG(pos.a); 340 result.b = TO_EXT_ANG(pos.b); 341 result.c = TO_EXT_ANG(pos.c); 342 result.u = TO_EXT_LEN(pos.u); 343 result.v = TO_EXT_LEN(pos.v); 344 result.w = TO_EXT_LEN(pos.w); 345 return result; 346 } 347 348 static void to_prog(CANON_POSITION &e) { 349 e.x = TO_PROG_LEN(e.x); 350 e.y = TO_PROG_LEN(e.y); 351 e.z = TO_PROG_LEN(e.z); 352 e.a = TO_PROG_ANG(e.a); 353 e.b = TO_PROG_ANG(e.b); 354 e.c = TO_PROG_ANG(e.c); 355 e.u = TO_PROG_LEN(e.u); 356 e.v = TO_PROG_LEN(e.v); 357 e.w = TO_PROG_LEN(e.w); 358 } 359 360 static int axis_valid(int n) { 361 return emcStatus->motion.traj.axis_mask & (1<<n); 362 } 363 364 static void canonUpdateEndPoint(double x, double y, double z, 365 double a, double b, double c, 366 double u, double v, double w) 367 { 368 canon.endPoint.x = x; 369 canon.endPoint.y = y; 370 canon.endPoint.z = z; 371 372 canon.endPoint.a = a; 373 canon.endPoint.b = b; 374 canon.endPoint.c = c; 375 376 canon.endPoint.u = u; 377 canon.endPoint.v = v; 378 canon.endPoint.w = w; 379 } 380 381 static void canonUpdateEndPoint(const CANON_POSITION & pos) 382 { 383 canon.endPoint = pos; 384 } 385 386 /* External call to update the canon end point. 387 Called by emctask during skipping of lines (run-from-line) */ 388 void CANON_UPDATE_END_POINT(double x, double y, double z, 389 double a, double b, double c, 390 double u, double v, double w) 391 { 392 canonUpdateEndPoint(FROM_PROG_LEN(x),FROM_PROG_LEN(y),FROM_PROG_LEN(z), 393 FROM_PROG_ANG(a),FROM_PROG_ANG(b),FROM_PROG_ANG(c), 394 FROM_PROG_LEN(u),FROM_PROG_LEN(v),FROM_PROG_LEN(w)); 395 } 396 397 static double toExtVel(double vel) { 398 if (canon.cartesian_move && !canon.angular_move) { 399 return TO_EXT_LEN(vel); 400 } else if (!canon.cartesian_move && canon.angular_move) { 401 return TO_EXT_ANG(vel); 402 } else if (canon.cartesian_move && canon.angular_move) { 403 return TO_EXT_LEN(vel); 404 } else { //seems this case was forgotten, neither linear, neither angular move (we are only sending vel) 405 return TO_EXT_LEN(vel); 406 } 407 } 408 409 static double toExtAcc(double acc) { return toExtVel(acc); } 410 411 static void send_g5x_msg(int index) { 412 flush_segments(); 413 414 /* append it to interp list so it gets updated at the right time, not at 415 read-ahead time */ 416 EMC_TRAJ_SET_G5X set_g5x_msg; 417 418 set_g5x_msg.g5x_index = index; 419 420 set_g5x_msg.origin = to_ext_pose(canon.g5xOffset); 421 422 for (int s = 0; s < emcStatus->motion.traj.spindles; s++){ 423 if(canon.spindle[s].css_maximum) { 424 SET_SPINDLE_SPEED(s, canon.spindle[s].speed); 425 } 426 } 427 interp_list.append(set_g5x_msg); 428 } 429 430 static void send_g92_msg(void) { 431 flush_segments(); 432 433 /* append it to interp list so it gets updated at the right time, not at 434 read-ahead time */ 435 EMC_TRAJ_SET_G92 set_g92_msg; 436 437 set_g92_msg.origin = to_ext_pose(canon.g92Offset); 438 439 for (int s = 0; s < emcStatus->motion.traj.spindles; s++){ 440 if(canon.spindle[s].css_maximum) { 441 SET_SPINDLE_SPEED(s, canon.spindle[s].speed); 442 } 443 } 444 interp_list.append(set_g92_msg); 445 } 446 447 void SET_XY_ROTATION(double t) { 448 EMC_TRAJ_SET_ROTATION sr; 449 sr.rotation = t; 450 interp_list.append(sr); 451 452 canon.xy_rotation = t; 453 } 454 455 void SET_G5X_OFFSET(int index, 456 double x, double y, double z, 457 double a, double b, double c, 458 double u, double v, double w) 459 { 460 CANON_POSITION pos(x,y,z,a,b,c,u,v,w); 461 from_prog(pos); 462 /* convert to mm units */ 463 canon.g5xOffset = pos; 464 465 send_g5x_msg(index); 466 } 467 468 void SET_G92_OFFSET(double x, double y, double z, 469 double a, double b, double c, 470 double u, double v, double w) { 471 /* convert to mm units */ 472 CANON_POSITION pos(x,y,z,a,b,c,u,v,w); 473 from_prog(pos); 474 475 canon.g92Offset = pos; 476 477 send_g92_msg(); 478 } 479 480 void USE_LENGTH_UNITS(CANON_UNITS in_unit) 481 { 482 canon.lengthUnits = in_unit; 483 484 emcStatus->task.programUnits = in_unit; 485 } 486 487 /* Free Space Motion */ 488 void SET_TRAVERSE_RATE(double rate) 489 { 490 // nothing need be done here 491 } 492 493 void SET_FEED_MODE(int spindle, int mode) { 494 flush_segments(); 495 canon.feed_mode = mode; 496 canon.spindle_num = spindle; 497 if(canon.feed_mode == 0) STOP_SPEED_FEED_SYNCH(); 498 } 499 500 void SET_FEED_RATE(double rate) 501 { 502 503 if(canon.feed_mode) { 504 START_SPEED_FEED_SYNCH(canon.spindle_num, rate, 1); 505 canon.linearFeedRate = rate; 506 } else { 507 /* convert from /min to /sec */ 508 rate /= 60.0; 509 510 511 /* convert to traj units (mm & deg) if needed */ 512 double newLinearFeedRate = FROM_PROG_LEN(rate), 513 newAngularFeedRate = FROM_PROG_ANG(rate); 514 515 if(newLinearFeedRate != canon.linearFeedRate 516 || newAngularFeedRate != canon.angularFeedRate) 517 flush_segments(); 518 519 canon.linearFeedRate = newLinearFeedRate; 520 canon.angularFeedRate = newAngularFeedRate; 521 } 522 } 523 524 void SET_FEED_REFERENCE(CANON_FEED_REFERENCE reference) 525 { 526 // nothing need be done here 527 } 528 529 /** 530 * Get the shortest linear axis displacement that the TP can handle as a discrete move. 531 * 532 * If this looks dirty, it's because it is. Canon runs in its own units, but 533 * the TP uses user units. Therefore, the minimum displacement has to be 534 * computed the same way, with the same threshold, or short moves do strange 535 * things (accel violations or infinite pauses). 536 * 537 * @todo revisit this when the TP is overhauled to use a consistent set of internal units. 538 */ 539 static double getMinLinearDisplacement() 540 { 541 return FROM_EXT_LEN(CART_FUZZ); 542 } 543 544 /** 545 * Equivalent of getMinLinearDisplacement for rotary axes. 546 */ 547 static double getMinAngularDisplacement() 548 { 549 return FROM_EXT_ANG(CART_FUZZ); 550 } 551 552 /** 553 * Apply the minimum displacement check to each axis delta. 554 * 555 * Checks that the axis is valid / active, and looks up the appropriate minimum 556 * displacement for the axis type and user units. 557 */ 558 static void applyMinDisplacement(double &dx, 559 double &dy, 560 double &dz, 561 double &da, 562 double &db, 563 double &dc, 564 double &du, 565 double &dv, 566 double &dw 567 ) 568 { 569 const double tiny_linear = getMinLinearDisplacement(); 570 const double tiny_angular = getMinAngularDisplacement(); 571 if(!axis_valid(0) || dx < tiny_linear) dx = 0.0; 572 if(!axis_valid(1) || dy < tiny_linear) dy = 0.0; 573 if(!axis_valid(2) || dz < tiny_linear) dz = 0.0; 574 if(!axis_valid(3) || da < tiny_angular) da = 0.0; 575 if(!axis_valid(4) || db < tiny_linear) db = 0.0; 576 if(!axis_valid(5) || dc < tiny_linear) dc = 0.0; 577 if(!axis_valid(6) || du < tiny_linear) du = 0.0; 578 if(!axis_valid(7) || dv < tiny_linear) dv = 0.0; 579 if(!axis_valid(8) || dw < tiny_linear) dw = 0.0; 580 } 581 582 583 /** 584 * Get the limiting acceleration for a displacement from the current position to the given position. 585 * returns a single acceleration that is the minimum of all axis accelerations. 586 */ 587 static AccelData getStraightAcceleration(double x, double y, double z, 588 double a, double b, double c, 589 double u, double v, double w) 590 { 591 double dx, dy, dz, du, dv, dw, da, db, dc; 592 double tx, ty, tz, tu, tv, tw, ta, tb, tc; 593 AccelData out; 594 595 out.acc = 0.0; // if a move to nowhere 596 out.tmax = 0.0; 597 out.dtot = 0.0; 598 599 // Compute absolute travel distance for each axis: 600 dx = fabs(x - canon.endPoint.x); 601 dy = fabs(y - canon.endPoint.y); 602 dz = fabs(z - canon.endPoint.z); 603 da = fabs(a - canon.endPoint.a); 604 db = fabs(b - canon.endPoint.b); 605 dc = fabs(c - canon.endPoint.c); 606 du = fabs(u - canon.endPoint.u); 607 dv = fabs(v - canon.endPoint.v); 608 dw = fabs(w - canon.endPoint.w); 609 610 applyMinDisplacement(dx, dy, dz, da, db, dc, du, dv, dw); 611 612 if(debug_velacc) 613 printf("getStraightAcceleration dx %g dy %g dz %g da %g db %g dc %g du %g dv %g dw %g ", 614 dx, dy, dz, da, db, dc, du, dv, dw); 615 616 // Figure out what kind of move we're making. This is used to determine 617 // the units of vel/acc. 618 if (dx <= 0.0 && dy <= 0.0 && dz <= 0.0 && 619 du <= 0.0 && dv <= 0.0 && dw <= 0.0) { 620 canon.cartesian_move = 0; 621 } else { 622 canon.cartesian_move = 1; 623 } 624 if (da <= 0.0 && db <= 0.0 && dc <= 0.0) { 625 canon.angular_move = 0; 626 } else { 627 canon.angular_move = 1; 628 } 629 630 // Pure linear move: 631 if (canon.cartesian_move && !canon.angular_move) { 632 tx = dx? (dx / FROM_EXT_LEN(emcAxisGetMaxAcceleration(0))): 0.0; 633 ty = dy? (dy / FROM_EXT_LEN(emcAxisGetMaxAcceleration(1))): 0.0; 634 tz = dz? (dz / FROM_EXT_LEN(emcAxisGetMaxAcceleration(2))): 0.0; 635 tu = du? (du / FROM_EXT_LEN(emcAxisGetMaxAcceleration(6))): 0.0; 636 tv = dv? (dv / FROM_EXT_LEN(emcAxisGetMaxAcceleration(7))): 0.0; 637 tw = dw? (dw / FROM_EXT_LEN(emcAxisGetMaxAcceleration(8))): 0.0; 638 out.tmax = MAX3(tx, ty ,tz); 639 out.tmax = MAX4(tu, tv, tw, out.tmax); 640 641 if(dx || dy || dz) 642 out.dtot = sqrt(dx * dx + dy * dy + dz * dz); 643 else 644 out.dtot = sqrt(du * du + dv * dv + dw * dw); 645 646 if (out.tmax > 0.0) { 647 out.acc = out.dtot / out.tmax; 648 } 649 } 650 // Pure angular move: 651 else if (!canon.cartesian_move && canon.angular_move) { 652 ta = da? (da / FROM_EXT_ANG(emcAxisGetMaxAcceleration(3))): 0.0; 653 tb = db? (db / FROM_EXT_ANG(emcAxisGetMaxAcceleration(4))): 0.0; 654 tc = dc? (dc / FROM_EXT_ANG(emcAxisGetMaxAcceleration(5))): 0.0; 655 out.tmax = MAX3(ta, tb, tc); 656 657 out.dtot = sqrt(da * da + db * db + dc * dc); 658 if (out.tmax > 0.0) { 659 out.acc = out.dtot / out.tmax; 660 } 661 } 662 // Combination angular and linear move: 663 else if (canon.cartesian_move && canon.angular_move) { 664 tx = dx? (dx / FROM_EXT_LEN(emcAxisGetMaxAcceleration(0))): 0.0; 665 ty = dy? (dy / FROM_EXT_LEN(emcAxisGetMaxAcceleration(1))): 0.0; 666 tz = dz? (dz / FROM_EXT_LEN(emcAxisGetMaxAcceleration(2))): 0.0; 667 ta = da? (da / FROM_EXT_ANG(emcAxisGetMaxAcceleration(3))): 0.0; 668 tb = db? (db / FROM_EXT_ANG(emcAxisGetMaxAcceleration(4))): 0.0; 669 tc = dc? (dc / FROM_EXT_ANG(emcAxisGetMaxAcceleration(5))): 0.0; 670 tu = du? (du / FROM_EXT_LEN(emcAxisGetMaxAcceleration(6))): 0.0; 671 tv = dv? (dv / FROM_EXT_LEN(emcAxisGetMaxAcceleration(7))): 0.0; 672 tw = dw? (dw / FROM_EXT_LEN(emcAxisGetMaxAcceleration(8))): 0.0; 673 out.tmax = MAX9(tx, ty, tz, 674 ta, tb, tc, 675 tu, tv, tw); 676 677 if(debug_velacc) 678 printf("getStraightAcceleration t^2 tx %g ty %g tz %g ta %g tb %g tc %g tu %g tv %g tw %g\n", 679 tx, ty, tz, ta, tb, tc, tu, tv, tw); 680 /* According to NIST IR6556 Section 2.1.2.5 Paragraph A 681 a combnation move is handled like a linear move, except 682 that the angular axes are allowed sufficient time to 683 complete their motion coordinated with the motion of 684 the linear axes. 685 */ 686 if(dx || dy || dz) 687 out.dtot = sqrt(dx * dx + dy * dy + dz * dz); 688 else 689 out.dtot = sqrt(du * du + dv * dv + dw * dw); 690 691 if (out.tmax > 0.0) { 692 out.acc = out.dtot / out.tmax; 693 } 694 } 695 if(debug_velacc) 696 printf("cartesian %d ang %d acc %g\n", canon.cartesian_move, canon.angular_move, out.acc); 697 return out; 698 } 699 700 static AccelData getStraightAcceleration(CANON_POSITION pos) 701 { 702 703 return getStraightAcceleration(pos.x, 704 pos.y, 705 pos.z, 706 pos.a, 707 pos.b, 708 pos.c, 709 pos.u, 710 pos.v, 711 pos.w); 712 } 713 714 static VelData getStraightVelocity(double x, double y, double z, 715 double a, double b, double c, 716 double u, double v, double w) 717 { 718 double dx, dy, dz, da, db, dc, du, dv, dw; 719 double tx, ty, tz, ta, tb, tc, tu, tv, tw; 720 VelData out; 721 722 /* If we get a move to nowhere (!canon.cartesian_move && !canon.angular_move) 723 we might as well go there at the canon.linearFeedRate... 724 */ 725 out.vel = canon.linearFeedRate; 726 out.tmax = 0; 727 out.dtot = 0; 728 729 // Compute absolute travel distance for each axis: 730 dx = fabs(x - canon.endPoint.x); 731 dy = fabs(y - canon.endPoint.y); 732 dz = fabs(z - canon.endPoint.z); 733 da = fabs(a - canon.endPoint.a); 734 db = fabs(b - canon.endPoint.b); 735 dc = fabs(c - canon.endPoint.c); 736 du = fabs(u - canon.endPoint.u); 737 dv = fabs(v - canon.endPoint.v); 738 dw = fabs(w - canon.endPoint.w); 739 740 applyMinDisplacement(dx, dy, dz, da, db, dc, du, dv, dw); 741 742 if(debug_velacc) 743 printf("getStraightVelocity dx %g dy %g dz %g da %g db %g dc %g du %g dv %g dw %g\n", 744 dx, dy, dz, da, db, dc, du, dv, dw); 745 746 // Figure out what kind of move we're making: 747 if (dx <= 0.0 && dy <= 0.0 && dz <= 0.0 && 748 du <= 0.0 && dv <= 0.0 && dw <= 0.0) { 749 canon.cartesian_move = 0; 750 } else { 751 canon.cartesian_move = 1; 752 } 753 if (da <= 0.0 && db <= 0.0 && dc <= 0.0) { 754 canon.angular_move = 0; 755 } else { 756 canon.angular_move = 1; 757 } 758 759 // Pure linear move: 760 if (canon.cartesian_move && !canon.angular_move) { 761 tx = dx? fabs(dx / FROM_EXT_LEN(emcAxisGetMaxVelocity(0))): 0.0; 762 ty = dy? fabs(dy / FROM_EXT_LEN(emcAxisGetMaxVelocity(1))): 0.0; 763 tz = dz? fabs(dz / FROM_EXT_LEN(emcAxisGetMaxVelocity(2))): 0.0; 764 tu = du? fabs(du / FROM_EXT_LEN(emcAxisGetMaxVelocity(6))): 0.0; 765 tv = dv? fabs(dv / FROM_EXT_LEN(emcAxisGetMaxVelocity(7))): 0.0; 766 tw = dw? fabs(dw / FROM_EXT_LEN(emcAxisGetMaxVelocity(8))): 0.0; 767 out.tmax = MAX3(tx, ty ,tz); 768 out.tmax = MAX4(tu, tv, tw, out.tmax); 769 770 if(dx || dy || dz) 771 out.dtot = sqrt(dx * dx + dy * dy + dz * dz); 772 else 773 out.dtot = sqrt(du * du + dv * dv + dw * dw); 774 775 if (out.tmax <= 0.0) { 776 out.vel = canon.linearFeedRate; 777 } else { 778 out.vel = out.dtot / out.tmax; 779 } 780 } 781 // Pure angular move: 782 else if (!canon.cartesian_move && canon.angular_move) { 783 ta = da? fabs(da / FROM_EXT_ANG(emcAxisGetMaxVelocity(3))): 0.0; 784 tb = db? fabs(db / FROM_EXT_ANG(emcAxisGetMaxVelocity(4))): 0.0; 785 tc = dc? fabs(dc / FROM_EXT_ANG(emcAxisGetMaxVelocity(5))): 0.0; 786 out.tmax = MAX3(ta, tb, tc); 787 788 out.dtot = sqrt(da * da + db * db + dc * dc); 789 if (out.tmax <= 0.0) { 790 out.vel = canon.angularFeedRate; 791 } else { 792 out.vel = out.dtot / out.tmax; 793 } 794 } 795 // Combination angular and linear move: 796 else if (canon.cartesian_move && canon.angular_move) { 797 tx = dx? fabs(dx / FROM_EXT_LEN(emcAxisGetMaxVelocity(0))): 0.0; 798 ty = dy? fabs(dy / FROM_EXT_LEN(emcAxisGetMaxVelocity(1))): 0.0; 799 tz = dz? fabs(dz / FROM_EXT_LEN(emcAxisGetMaxVelocity(2))): 0.0; 800 ta = da? fabs(da / FROM_EXT_ANG(emcAxisGetMaxVelocity(3))): 0.0; 801 tb = db? fabs(db / FROM_EXT_ANG(emcAxisGetMaxVelocity(4))): 0.0; 802 tc = dc? fabs(dc / FROM_EXT_ANG(emcAxisGetMaxVelocity(5))): 0.0; 803 tu = du? fabs(du / FROM_EXT_LEN(emcAxisGetMaxVelocity(6))): 0.0; 804 tv = dv? fabs(dv / FROM_EXT_LEN(emcAxisGetMaxVelocity(7))): 0.0; 805 tw = dw? fabs(dw / FROM_EXT_LEN(emcAxisGetMaxVelocity(8))): 0.0; 806 out.tmax = MAX9(tx, ty, tz, 807 ta, tb, tc, 808 tu, tv, tw); 809 810 if(debug_velacc) 811 printf("getStraightVelocity times tx %g ty %g tz %g ta %g tb %g tc %g tu %g tv %g tw %g\n", 812 tx, ty, tz, ta, tb, tc, tu, tv, tw); 813 814 /* According to NIST IR6556 Section 2.1.2.5 Paragraph A 815 a combnation move is handled like a linear move, except 816 that the angular axes are allowed sufficient time to 817 complete their motion coordinated with the motion of 818 the linear axes. 819 */ 820 if(dx || dy || dz) 821 out.dtot = sqrt(dx * dx + dy * dy + dz * dz); 822 else 823 out.dtot = sqrt(du * du + dv * dv + dw * dw); 824 825 if (out.tmax <= 0.0) { 826 out.vel = canon.linearFeedRate; 827 } else { 828 out.vel = out.dtot / out.tmax; 829 } 830 } 831 if(debug_velacc) 832 printf("cartesian %d ang %d vel %g\n", canon.cartesian_move, canon.angular_move, out.vel); 833 return out; 834 } 835 836 static VelData getStraightVelocity(CANON_POSITION pos) 837 { 838 839 return getStraightVelocity(pos.x, 840 pos.y, 841 pos.z, 842 pos.a, 843 pos.b, 844 pos.c, 845 pos.u, 846 pos.v, 847 pos.w); 848 } 849 850 #include <vector> 851 struct pt { double x, y, z, a, b, c, u, v, w; int line_no;}; 852 853 static std::vector<struct pt> chained_points; 854 855 static void drop_segments(void) { 856 chained_points.clear(); 857 } 858 859 static void flush_segments(void) { 860 if(chained_points.empty()) return; 861 862 struct pt &pos = chained_points.back(); 863 864 double x = pos.x, y = pos.y, z = pos.z; 865 double a = pos.a, b = pos.b, c = pos.c; 866 double u = pos.u, v = pos.v, w = pos.w; 867 868 int line_no = pos.line_no; 869 870 #ifdef SHOW_JOINED_SEGMENTS 871 for(unsigned int i=0; i != chained_points.size(); i++) { printf("."); } 872 printf("\n"); 873 #endif 874 875 VelData linedata = getStraightVelocity(x, y, z, a, b, c, u, v, w); 876 double vel = linedata.vel; 877 878 if (canon.cartesian_move && !canon.angular_move) { 879 if (vel > canon.linearFeedRate) { 880 vel = canon.linearFeedRate; 881 } 882 } else if (!canon.cartesian_move && canon.angular_move) { 883 if (vel > canon.angularFeedRate) { 884 vel = canon.angularFeedRate; 885 } 886 } else if (canon.cartesian_move && canon.angular_move) { 887 if (vel > canon.linearFeedRate) { 888 vel = canon.linearFeedRate; 889 } 890 } 891 892 893 EMC_TRAJ_LINEAR_MOVE linearMoveMsg; 894 linearMoveMsg.feed_mode = canon.feed_mode; 895 896 // now x, y, z, and b are in absolute mm or degree units 897 linearMoveMsg.end.tran.x = TO_EXT_LEN(x); 898 linearMoveMsg.end.tran.y = TO_EXT_LEN(y); 899 linearMoveMsg.end.tran.z = TO_EXT_LEN(z); 900 901 linearMoveMsg.end.u = TO_EXT_LEN(u); 902 linearMoveMsg.end.v = TO_EXT_LEN(v); 903 linearMoveMsg.end.w = TO_EXT_LEN(w); 904 905 // fill in the orientation 906 linearMoveMsg.end.a = TO_EXT_ANG(a); 907 linearMoveMsg.end.b = TO_EXT_ANG(b); 908 linearMoveMsg.end.c = TO_EXT_ANG(c); 909 910 linearMoveMsg.vel = toExtVel(vel); 911 linearMoveMsg.ini_maxvel = toExtVel(linedata.vel); 912 AccelData lineaccdata = getStraightAcceleration(x, y, z, a, b, c, u, v, w); 913 double acc = lineaccdata.acc; 914 linearMoveMsg.acc = toExtAcc(acc); 915 916 linearMoveMsg.type = EMC_MOTION_TYPE_FEED; 917 linearMoveMsg.indexer_jnum = -1; 918 if ((vel && acc) || canon.spindle[canon.spindle_num].synched) { 919 interp_list.set_line_number(line_no); 920 interp_list.append(linearMoveMsg); 921 } 922 canonUpdateEndPoint(x, y, z, a, b, c, u, v, w); 923 924 drop_segments(); 925 } 926 927 static void get_last_pos(double &lx, double &ly, double &lz) { 928 if(chained_points.empty()) { 929 lx = canon.endPoint.x; 930 ly = canon.endPoint.y; 931 lz = canon.endPoint.z; 932 } else { 933 struct pt &pos = chained_points.back(); 934 lx = pos.x; 935 ly = pos.y; 936 lz = pos.z; 937 } 938 } 939 940 static bool 941 linkable(double x, double y, double z, 942 double a, double b, double c, 943 double u, double v, double w) { 944 struct pt &pos = chained_points.back(); 945 if(canon.motionMode != CANON_CONTINUOUS || canon.naivecamTolerance == 0) 946 return false; 947 //FIXME make this length controlled elsewhere? 948 if(chained_points.size() > 100) return false; 949 950 //If ABCUVW motion, then the tangent calculation fails? 951 // TODO is there a fundamental reason that we can't handle 9D motion here? 952 if(a != pos.a) return false; 953 if(b != pos.b) return false; 954 if(c != pos.c) return false; 955 if(u != pos.u) return false; 956 if(v != pos.v) return false; 957 if(w != pos.w) return false; 958 959 if(x==canon.endPoint.x && y==canon.endPoint.y && z==canon.endPoint.z) return false; 960 961 for(std::vector<struct pt>::iterator it = chained_points.begin(); 962 it != chained_points.end(); it++) { 963 PM_CARTESIAN M(x-canon.endPoint.x, y-canon.endPoint.y, z-canon.endPoint.z), 964 B(canon.endPoint.x, canon.endPoint.y, canon.endPoint.z), 965 P(it->x, it->y, it->z); 966 double t0 = dot(M, P-B) / dot(M, M); 967 if(t0 < 0) t0 = 0; 968 if(t0 > 1) t0 = 1; 969 970 double D = mag(P - (B + t0 * M)); 971 if(D > canon.naivecamTolerance) return false; 972 } 973 return true; 974 } 975 976 static void 977 see_segment(int line_number, 978 double x, double y, double z, 979 double a, double b, double c, 980 double u, double v, double w) { 981 bool changed_abc = (a != canon.endPoint.a) 982 || (b != canon.endPoint.b) 983 || (c != canon.endPoint.c); 984 985 bool changed_uvw = (u != canon.endPoint.u) 986 || (v != canon.endPoint.v) 987 || (w != canon.endPoint.w); 988 989 if(!chained_points.empty() && !linkable(x, y, z, a, b, c, u, v, w)) { 990 flush_segments(); 991 } 992 pt pos = {x, y, z, a, b, c, u, v, w, line_number}; 993 chained_points.push_back(pos); 994 if(changed_abc || changed_uvw) { 995 flush_segments(); 996 } 997 } 998 999 void FINISH() { 1000 flush_segments(); 1001 } 1002 1003 void ON_RESET() { 1004 drop_segments(); 1005 } 1006 1007 1008 void STRAIGHT_TRAVERSE(int line_number, 1009 double x, double y, double z, 1010 double a, double b, double c, 1011 double u, double v, double w) 1012 { 1013 double vel, acc; 1014 1015 flush_segments(); 1016 1017 EMC_TRAJ_LINEAR_MOVE linearMoveMsg; 1018 1019 linearMoveMsg.feed_mode = 0; 1020 if (canon.rotary_unlock_for_traverse != -1) 1021 linearMoveMsg.type = EMC_MOTION_TYPE_INDEXROTARY; 1022 else 1023 linearMoveMsg.type = EMC_MOTION_TYPE_TRAVERSE; 1024 1025 from_prog(x,y,z,a,b,c,u,v,w); 1026 rotate_and_offset_pos(x,y,z,a,b,c,u,v,w); 1027 1028 VelData veldata = getStraightVelocity(x, y, z, a, b, c, u, v, w); 1029 AccelData accdata = getStraightAcceleration(x, y, z, a, b, c, u, v, w); 1030 1031 vel = veldata.vel; 1032 acc = accdata.acc; 1033 1034 linearMoveMsg.end = to_ext_pose(x,y,z,a,b,c,u,v,w); 1035 linearMoveMsg.vel = linearMoveMsg.ini_maxvel = toExtVel(vel); 1036 linearMoveMsg.acc = toExtAcc(acc); 1037 linearMoveMsg.indexer_jnum = canon.rotary_unlock_for_traverse; 1038 1039 int old_feed_mode = canon.feed_mode; 1040 if(canon.feed_mode) 1041 STOP_SPEED_FEED_SYNCH(); 1042 1043 if(vel && acc) { 1044 interp_list.set_line_number(line_number); 1045 interp_list.append(linearMoveMsg); 1046 } 1047 1048 if(old_feed_mode) 1049 START_SPEED_FEED_SYNCH(canon.spindle_num, canon.linearFeedRate, 1); 1050 1051 canonUpdateEndPoint(x, y, z, a, b, c, u, v, w); 1052 } 1053 1054 void STRAIGHT_FEED(int line_number, 1055 double x, double y, double z, 1056 double a, double b, double c, 1057 double u, double v, double w) 1058 { 1059 EMC_TRAJ_LINEAR_MOVE linearMoveMsg; 1060 linearMoveMsg.feed_mode = canon.feed_mode; 1061 1062 from_prog(x,y,z,a,b,c,u,v,w); 1063 rotate_and_offset_pos(x,y,z,a,b,c,u,v,w); 1064 see_segment(line_number, x, y, z, a, b, c, u, v, w); 1065 } 1066 1067 1068 void RIGID_TAP(int line_number, double x, double y, double z, double scale) 1069 { 1070 double ini_maxvel,acc; 1071 EMC_TRAJ_RIGID_TAP rigidTapMsg; 1072 double unused=0; 1073 1074 from_prog(x,y,z,unused,unused,unused,unused,unused,unused); 1075 rotate_and_offset_pos(x,y,z,unused,unused,unused,unused,unused,unused); 1076 1077 VelData veldata = getStraightVelocity(x, y, z, 1078 canon.endPoint.a, canon.endPoint.b, canon.endPoint.c, 1079 canon.endPoint.u, canon.endPoint.v, canon.endPoint.w); 1080 ini_maxvel = veldata.vel; 1081 1082 AccelData accdata = getStraightAcceleration(x, y, z, 1083 canon.endPoint.a, canon.endPoint.b, canon.endPoint.c, 1084 canon.endPoint.u, canon.endPoint.v, canon.endPoint.w); 1085 acc = accdata.acc; 1086 1087 rigidTapMsg.pos = to_ext_pose(x,y,z, 1088 canon.endPoint.a, canon.endPoint.b, canon.endPoint.c, 1089 canon.endPoint.u, canon.endPoint.v, canon.endPoint.w); 1090 1091 rigidTapMsg.vel = toExtVel(ini_maxvel); 1092 rigidTapMsg.ini_maxvel = toExtVel(ini_maxvel); 1093 rigidTapMsg.acc = toExtAcc(acc); 1094 rigidTapMsg.scale = scale; 1095 flush_segments(); 1096 1097 if(ini_maxvel && acc) { 1098 interp_list.set_line_number(line_number); 1099 interp_list.append(rigidTapMsg); 1100 } 1101 1102 // don't move the endpoint because after this move, we are back where we started 1103 } 1104 1105 1106 /* 1107 STRAIGHT_PROBE is exactly the same as STRAIGHT_FEED, except that it 1108 uses a probe message instead of a linear move message. 1109 */ 1110 1111 void STRAIGHT_PROBE(int line_number, 1112 double x, double y, double z, 1113 double a, double b, double c, 1114 double u, double v, double w, 1115 unsigned char probe_type) 1116 { 1117 double ini_maxvel, vel, acc; 1118 EMC_TRAJ_PROBE probeMsg; 1119 1120 from_prog(x,y,z,a,b,c,u,v,w); 1121 rotate_and_offset_pos(x,y,z,a,b,c,u,v,w); 1122 1123 flush_segments(); 1124 1125 VelData veldata = getStraightVelocity(x, y, z, a, b, c, u, v, w); 1126 ini_maxvel = vel = veldata.vel; 1127 1128 if (canon.cartesian_move && !canon.angular_move) { 1129 if (vel > canon.linearFeedRate) { 1130 vel = canon.linearFeedRate; 1131 } 1132 } else if (!canon.cartesian_move && canon.angular_move) { 1133 if (vel > canon.angularFeedRate) { 1134 vel = canon.angularFeedRate; 1135 } 1136 } else if (canon.cartesian_move && canon.angular_move) { 1137 if (vel > canon.linearFeedRate) { 1138 vel = canon.linearFeedRate; 1139 } 1140 } 1141 1142 AccelData accdata = getStraightAcceleration(x, y, z, a, b, c, u, v, w); 1143 acc = accdata.acc; 1144 1145 probeMsg.vel = toExtVel(vel); 1146 probeMsg.ini_maxvel = toExtVel(ini_maxvel); 1147 probeMsg.acc = toExtAcc(acc); 1148 1149 probeMsg.type = EMC_MOTION_TYPE_PROBING; 1150 probeMsg.probe_type = probe_type; 1151 1152 probeMsg.pos = to_ext_pose(x,y,z,a,b,c,u,v,w); 1153 1154 if(vel && acc) { 1155 interp_list.set_line_number(line_number); 1156 interp_list.append(probeMsg); 1157 } 1158 canonUpdateEndPoint(x, y, z, a, b, c, u, v, w); 1159 } 1160 1161 /* Machining Attributes */ 1162 1163 void SET_MOTION_CONTROL_MODE(CANON_MOTION_MODE mode, double tolerance) 1164 { 1165 EMC_TRAJ_SET_TERM_COND setTermCondMsg; 1166 1167 flush_segments(); 1168 1169 canon.motionMode = mode; 1170 canon.motionTolerance = FROM_PROG_LEN(tolerance); 1171 1172 switch (mode) { 1173 case CANON_CONTINUOUS: 1174 setTermCondMsg.cond = EMC_TRAJ_TERM_COND_BLEND; 1175 setTermCondMsg.tolerance = TO_EXT_LEN(canon.motionTolerance); 1176 break; 1177 case CANON_EXACT_PATH: 1178 setTermCondMsg.cond = EMC_TRAJ_TERM_COND_EXACT; 1179 break; 1180 1181 case CANON_EXACT_STOP: 1182 default: 1183 setTermCondMsg.cond = EMC_TRAJ_TERM_COND_STOP; 1184 break; 1185 } 1186 1187 interp_list.append(setTermCondMsg); 1188 } 1189 1190 void SET_NAIVECAM_TOLERANCE(double tolerance) 1191 { 1192 canon.naivecamTolerance = FROM_PROG_LEN(tolerance); 1193 } 1194 1195 void SELECT_PLANE(CANON_PLANE in_plane) 1196 { 1197 canon.activePlane = in_plane; 1198 } 1199 1200 void SET_CUTTER_RADIUS_COMPENSATION(double radius) 1201 { 1202 // nothing need be done here 1203 } 1204 1205 void START_CUTTER_RADIUS_COMPENSATION(int side) 1206 { 1207 // nothing need be done here 1208 } 1209 1210 void STOP_CUTTER_RADIUS_COMPENSATION() 1211 { 1212 // nothing need be done here 1213 } 1214 1215 1216 1217 void START_SPEED_FEED_SYNCH(int spindle, double feed_per_revolution, bool velocity_mode) 1218 { 1219 flush_segments(); 1220 EMC_TRAJ_SET_SPINDLESYNC spindlesyncMsg; 1221 spindlesyncMsg.spindle = spindle; 1222 spindlesyncMsg.feed_per_revolution = TO_EXT_LEN(FROM_PROG_LEN(feed_per_revolution)); 1223 spindlesyncMsg.velocity_mode = velocity_mode; 1224 interp_list.append(spindlesyncMsg); 1225 canon.spindle[spindle].synched = 1; 1226 } 1227 1228 void STOP_SPEED_FEED_SYNCH() 1229 { 1230 flush_segments(); 1231 EMC_TRAJ_SET_SPINDLESYNC spindlesyncMsg; 1232 spindlesyncMsg.feed_per_revolution = 0.0; 1233 spindlesyncMsg.velocity_mode = false; 1234 interp_list.append(spindlesyncMsg); 1235 canon.spindle[canon.spindle_num].synched = 0; 1236 } 1237 1238 /* Machining Functions */ 1239 static double chord_deviation(double sx, double sy, double ex, double ey, double cx, double cy, int rotation, double &mx, double &my) { 1240 double th1 = atan2(sy-cy, sx-cx), 1241 th2 = atan2(ey-cy, ex-cx), 1242 r = hypot(sy-cy, sx-cx), 1243 dth = th2 - th1; 1244 1245 if(rotation < 0) { 1246 if(dth >= -1e-5) th2 -= 2*M_PI; 1247 // in the edge case where atan2 gives you -pi and pi, a second iteration is needed 1248 // to get these in the right order 1249 dth = th2 - th1; 1250 if(dth >= -1e-5) th2 -= 2*M_PI; 1251 } else { 1252 if(dth <= 1e-5) th2 += 2*M_PI; 1253 dth = th2 - th1; 1254 if(dth <= 1e-5) th2 += 2*M_PI; 1255 } 1256 1257 double included = fabs(th2 - th1); 1258 double mid = (th2 + th1) / 2; 1259 mx = cx + r * cos(mid); 1260 my = cy + r * sin(mid); 1261 double dev = r * (1 - cos(included/2)); 1262 return dev; 1263 } 1264 1265 /* Spline and NURBS additional functions; */ 1266 1267 static double max(double a, double b) { 1268 if(a < b) return b; 1269 return a; 1270 } 1271 static void unit(double *x, double *y) { 1272 double h = hypot(*x, *y); 1273 if(h != 0) { *x/=h; *y/=h; } 1274 } 1275 1276 static void 1277 arc(int lineno, double x0, double y0, double x1, double y1, double dx, double dy) { 1278 double small = 0.000001; 1279 double x = x1-x0, y=y1-y0; 1280 double den = 2 * (y*dx - x*dy); 1281 CANON_POSITION p = unoffset_and_unrotate_pos(canon.endPoint); 1282 to_prog(p); 1283 if (fabs(den) > small) { 1284 double r = -(x*x+y*y)/den; 1285 double i = dy*r, j = -dx*r; 1286 double cx = x0+i, cy=y0+j; 1287 ARC_FEED(lineno, x1, y1, cx, cy, r<0 ? 1 : -1, 1288 p.z, p.a, p.b, p.c, p.u, p.v, p.w); 1289 } else { 1290 STRAIGHT_FEED(lineno, x1, y1, p.z, p.a, p.b, p.c, p.u, p.v, p.w); 1291 } 1292 } 1293 1294 static int 1295 biarc(int lineno, double p0x, double p0y, double tsx, double tsy, 1296 double p4x, double p4y, double tex, double tey, double r=1.0) { 1297 unit(&tsx, &tsy); 1298 unit(&tex, &tey); 1299 1300 double vx = p0x - p4x, vy = p0y - p4y; 1301 double c = vx*vx + vy*vy; 1302 double b = 2 * (vx * (r*tsx + tex) + vy * (r*tsy + tey)); 1303 double a = 2 * r * (tsx * tex + tsy * tey - 1); 1304 1305 double discr = b*b - 4*a*c; 1306 if(discr < 0) return 0; 1307 1308 double disq = sqrt(discr); 1309 double beta1 = (-b-disq) / 2 / a; 1310 double beta2 = (-b+disq) / 2 / a; 1311 1312 if(beta1 > 0 && beta2 > 0) 1313 return 0; 1314 double beta = max(beta1, beta2); 1315 double alpha = beta * r; 1316 double ab = alpha + beta; 1317 double p1x = p0x + alpha * tsx, p1y = p0y + alpha * tsy, 1318 p3x = p4x - beta * tex, p3y = p4y - beta * tey, 1319 p2x = (p1x*beta + p3x*alpha) / ab, 1320 p2y = (p1y*beta + p3y*alpha) / ab; 1321 double tmx = p3x-p2x, tmy = p3y-p2y; 1322 unit(&tmx, &tmy); 1323 1324 arc(lineno, p0x, p0y, p2x, p2y, tsx, tsy); 1325 arc(lineno, p2x, p2y, p4x, p4y, tmx, tmy); 1326 return 1; 1327 } 1328 1329 1330 /* Canon calls */ 1331 1332 void NURBS_FEED(int lineno, std::vector<CONTROL_POINT> nurbs_control_points, unsigned int k) { 1333 flush_segments(); 1334 1335 unsigned int n = nurbs_control_points.size() - 1; 1336 double umax = n - k + 2; 1337 unsigned int div = nurbs_control_points.size()*4; 1338 std::vector<unsigned int> knot_vector = knot_vector_creator(n, k); 1339 PLANE_POINT P0, P0T, P1, P1T; 1340 1341 P0 = nurbs_point(0,k,nurbs_control_points,knot_vector); 1342 P0T = nurbs_tangent(0, k, nurbs_control_points, knot_vector); 1343 1344 for(unsigned int i=1; i<=div; i++) { 1345 double u = umax * i / div; 1346 P1 = nurbs_point(u,k,nurbs_control_points,knot_vector); 1347 P1T = nurbs_tangent(u,k,nurbs_control_points,knot_vector); 1348 biarc(lineno, P0.X,P0.Y, P0T.X,P0T.Y, P1.X,P1.Y, P1T.X,P1T.Y); 1349 P0 = P1; 1350 P0T = P1T; 1351 } 1352 knot_vector.clear(); 1353 } 1354 1355 1356 /** 1357 * Simple circular shift function for PM_CARTESIAN type. 1358 * Cycle around axes without changing the individual values. A circshift of -1 1359 * makes the X value become the new Y, Y become the Z, and Z become the new X. 1360 */ 1361 static PM_CARTESIAN circshift(PM_CARTESIAN & vec, int steps) 1362 { 1363 int X=0,Y=1,Z=2; 1364 1365 int s = 3; 1366 // Use mod to cycle indices around by steps 1367 X = (X + steps + s) % s; 1368 Y = (Y + steps + s) % s; 1369 Z = (Z + steps + s) % s; 1370 return PM_CARTESIAN(vec[X],vec[Y],vec[Z]); 1371 } 1372 1373 #if 0 1374 static CANON_POSITION get_axis_max_velocity() 1375 { 1376 CANON_POSITION maxvel; 1377 maxvel.x = axis_valid(0) ? FROM_EXT_LEN(emcAxisGetMaxVelocity(0)) : 0.0; 1378 maxvel.y = axis_valid(1) ? FROM_EXT_LEN(emcAxisGetMaxVelocity(1)) : 0.0; 1379 maxvel.z = axis_valid(2) ? FROM_EXT_LEN(emcAxisGetMaxVelocity(2)) : 0.0; 1380 1381 maxvel.a = axis_valid(3) ? FROM_EXT_ANG(emcAxisGetMaxVelocity(3)) : 0.0; 1382 maxvel.b = axis_valid(4) ? FROM_EXT_ANG(emcAxisGetMaxVelocity(4)) : 0.0; 1383 maxvel.c = axis_valid(5) ? FROM_EXT_ANG(emcAxisGetMaxVelocity(5)) : 0.0; 1384 1385 maxvel.u = axis_valid(6) ? FROM_EXT_LEN(emcAxisGetMaxVelocity(6)) : 0.0; 1386 maxvel.v = axis_valid(7) ? FROM_EXT_LEN(emcAxisGetMaxVelocity(7)) : 0.0; 1387 maxvel.w = axis_valid(8) ? FROM_EXT_LEN(emcAxisGetMaxVelocity(8)) : 0.0; 1388 return maxvel; 1389 } 1390 1391 static CANON_POSITION get_axis_max_acceleration() 1392 { 1393 CANON_POSITION maxacc; 1394 maxacc.x = axis_valid(0) ? FROM_EXT_LEN(emcAxisGetMaxAcceleration(0)) : 0.0; 1395 maxacc.y = axis_valid(1) ? FROM_EXT_LEN(emcAxisGetMaxAcceleration(1)) : 0.0; 1396 maxacc.z = axis_valid(2) ? FROM_EXT_LEN(emcAxisGetMaxAcceleration(2)) : 0.0; 1397 1398 maxacc.a = axis_valid(3) ? FROM_EXT_ANG(emcAxisGetMaxAcceleration(3)) : 0.0; 1399 maxacc.b = axis_valid(4) ? FROM_EXT_ANG(emcAxisGetMaxAcceleration(4)) : 0.0; 1400 maxacc.c = axis_valid(5) ? FROM_EXT_ANG(emcAxisGetMaxAcceleration(5)) : 0.0; 1401 1402 maxacc.u = axis_valid(6) ? FROM_EXT_LEN(emcAxisGetMaxAcceleration(6)) : 0.0; 1403 maxacc.v = axis_valid(7) ? FROM_EXT_LEN(emcAxisGetMaxAcceleration(7)) : 0.0; 1404 maxacc.w = axis_valid(8) ? FROM_EXT_LEN(emcAxisGetMaxAcceleration(8)) : 0.0; 1405 return maxacc; 1406 } 1407 1408 static double axis_motion_time(const CANON_POSITION & start, const CANON_POSITION & end) 1409 { 1410 1411 CANON_POSITION disp = end - start; 1412 CANON_POSITION times; 1413 CANON_POSITION maxvel = get_axis_max_velocity(); 1414 1415 canon_debug(" in axis_motion_time\n"); 1416 // For active axes, find the time required to reach the displacement in each axis 1417 int ind = 0; 1418 for (ind = 0; ind < 9; ++ind) { 1419 double v = maxvel[ind]; 1420 if (v > 0.0) { 1421 times[ind] = fabs(disp[ind]) / v; 1422 } else { 1423 times[ind]=0; 1424 } 1425 canon_debug(" ind = %d, maxvel = %f, disp = %f, time = %f\n", ind, v, disp[ind], times[ind]); 1426 } 1427 1428 return times.max(); 1429 } 1430 1431 // NOTE: not exactly times, comment TODO 1432 static double axis_acc_time(const CANON_POSITION & start, const CANON_POSITION & end) 1433 { 1434 1435 CANON_POSITION disp = end - start; 1436 CANON_POSITION times; 1437 CANON_POSITION maxacc = get_axis_max_acceleration(); 1438 1439 for (int i = 0; i < 9; ++i) { 1440 double a = maxacc[i]; 1441 if (a > 0.0) { 1442 times[i] = fabs(disp[i]) / a; 1443 } else { 1444 times[i]=0; 1445 } 1446 } 1447 1448 return times.max(); 1449 } 1450 #endif 1451 1452 void ARC_FEED(int line_number, 1453 double first_end, double second_end, 1454 double first_axis, double second_axis, int rotation, 1455 double axis_end_point, 1456 double a, double b, double c, 1457 double u, double v, double w) 1458 { 1459 1460 EMC_TRAJ_CIRCULAR_MOVE circularMoveMsg; 1461 EMC_TRAJ_LINEAR_MOVE linearMoveMsg; 1462 1463 canon_debug("line = %d\n", line_number); 1464 canon_debug("first_end = %f, second_end = %f\n", first_end,second_end); 1465 1466 if( canon.activePlane == CANON_PLANE_XY && canon.motionMode == CANON_CONTINUOUS) { 1467 double mx, my; 1468 double lx, ly, lz; 1469 double unused; 1470 1471 get_last_pos(lx, ly, lz); 1472 1473 double fe=FROM_PROG_LEN(first_end), se=FROM_PROG_LEN(second_end), ae=FROM_PROG_LEN(axis_end_point); 1474 double fa=FROM_PROG_LEN(first_axis), sa=FROM_PROG_LEN(second_axis); 1475 rotate_and_offset_pos(fe, se, ae, unused, unused, unused, unused, unused, unused); 1476 rotate_and_offset_pos(fa, sa, unused, unused, unused, unused, unused, unused, unused); 1477 if (chord_deviation(lx, ly, fe, se, fa, sa, rotation, mx, my) < canon.naivecamTolerance) { 1478 a = FROM_PROG_ANG(a); 1479 b = FROM_PROG_ANG(b); 1480 c = FROM_PROG_ANG(c); 1481 u = FROM_PROG_LEN(u); 1482 v = FROM_PROG_LEN(v); 1483 w = FROM_PROG_LEN(w); 1484 1485 rotate_and_offset_pos(unused, unused, unused, a, b, c, u, v, w); 1486 see_segment(line_number, mx, my, 1487 (lz + ae)/2, 1488 (canon.endPoint.a + a)/2, 1489 (canon.endPoint.b + b)/2, 1490 (canon.endPoint.c + c)/2, 1491 (canon.endPoint.u + u)/2, 1492 (canon.endPoint.v + v)/2, 1493 (canon.endPoint.w + w)/2); 1494 see_segment(line_number, fe, se, ae, a, b, c, u, v, w); 1495 return; 1496 } 1497 } 1498 1499 linearMoveMsg.feed_mode = canon.feed_mode; 1500 circularMoveMsg.feed_mode = canon.feed_mode; 1501 flush_segments(); 1502 1503 // Start by defining 3D points for the motion end and center. 1504 PM_CARTESIAN end_cart(first_end, second_end, axis_end_point); 1505 PM_CARTESIAN center_cart(first_axis, second_axis, axis_end_point); 1506 PM_CARTESIAN normal_cart(0.0,0.0,1.0); 1507 PM_CARTESIAN plane_x(1.0,0.0,0.0); 1508 PM_CARTESIAN plane_y(0.0,1.0,0.0); 1509 1510 1511 canon_debug("start = %f %f %f\n", 1512 canon.endPoint.x, 1513 canon.endPoint.y, 1514 canon.endPoint.z); 1515 canon_debug("end = %f %f %f\n", 1516 end_cart.x, 1517 end_cart.y, 1518 end_cart.z); 1519 canon_debug("center = %f %f %f\n", 1520 center_cart.x, 1521 center_cart.y, 1522 center_cart.z); 1523 1524 // Rearrange the X Y Z coordinates in the correct order based on the active plane (XY, YZ, or XZ) 1525 // KLUDGE CANON_PLANE is 1-indexed, hence the subtraction here to make a 0-index value 1526 int shift_ind = 0; 1527 switch(canon.activePlane) { 1528 case CANON_PLANE_XY: 1529 shift_ind = 0; 1530 break; 1531 case CANON_PLANE_XZ: 1532 shift_ind = -2; 1533 break; 1534 case CANON_PLANE_YZ: 1535 shift_ind = -1; 1536 break; 1537 case CANON_PLANE_UV: 1538 case CANON_PLANE_VW: 1539 case CANON_PLANE_UW: 1540 CANON_ERROR("Can't set plane in UVW axes, assuming XY"); 1541 break; 1542 } 1543 1544 canon_debug("active plane is %d, shift_ind is %d\n",canon.activePlane,shift_ind); 1545 end_cart = circshift(end_cart, shift_ind); 1546 center_cart = circshift(center_cart, shift_ind); 1547 normal_cart = circshift(normal_cart, shift_ind); 1548 plane_x = circshift(plane_x, shift_ind); 1549 plane_y = circshift(plane_y, shift_ind); 1550 1551 canon_debug("normal = %f %f %f\n", 1552 normal_cart.x, 1553 normal_cart.y, 1554 normal_cart.z); 1555 1556 canon_debug("plane_x = %f %f %f\n", 1557 plane_x.x, 1558 plane_x.y, 1559 plane_x.z); 1560 1561 canon_debug("plane_y = %f %f %f\n", 1562 plane_y.x, 1563 plane_y.y, 1564 plane_y.z); 1565 // Define end point in PROGRAM units and convert to CANON 1566 CANON_POSITION endpt(0,0,0,a,b,c,u,v,w); 1567 from_prog(endpt); 1568 1569 // Store permuted XYZ end position 1570 from_prog_len(end_cart); 1571 endpt.set_xyz(end_cart); 1572 1573 // Convert to CANON units 1574 from_prog_len(center_cart); 1575 1576 // Rotate and offset the new end point to be in the same coordinate system as the current end point 1577 rotate_and_offset(endpt); 1578 rotate_and_offset_xyz(center_cart); 1579 rotate_and_offset_xyz(end_cart); 1580 // Also rotate the basis vectors 1581 to_rotated(plane_x); 1582 to_rotated(plane_y); 1583 to_rotated(normal_cart); 1584 1585 canon_debug("end = %f %f %f\n", 1586 end_cart.x, 1587 end_cart.y, 1588 end_cart.z); 1589 1590 canon_debug("endpt = %f %f %f\n", 1591 endpt.x, 1592 endpt.y, 1593 endpt.z); 1594 canon_debug("center = %f %f %f\n", 1595 center_cart.x, 1596 center_cart.y, 1597 center_cart.z); 1598 1599 canon_debug("normal = %f %f %f\n", 1600 normal_cart.x, 1601 normal_cart.y, 1602 normal_cart.z); 1603 // Note that the "start" point is already rotated and offset 1604 1605 // Define displacement vectors from center to end and center to start (3D) 1606 PM_CARTESIAN end_rel = end_cart - center_cart; 1607 PM_CARTESIAN start_rel = canon.endPoint.xyz() - center_cart; 1608 1609 // Project each displacement onto the active plane 1610 double p_end_1 = dot(end_rel,plane_x); 1611 double p_end_2 = dot(end_rel,plane_y); 1612 double p_start_1 = dot(start_rel,plane_x); 1613 double p_start_2 = dot(start_rel,plane_y); 1614 1615 canon_debug("planar end = %f %f\n", p_end_1, p_end_2); 1616 canon_debug("planar start = %f %f\n", p_start_1, p_start_2); 1617 1618 canon_debug("rotation = %d\n",rotation); 1619 1620 // Use the "X" (1) and Y" (2) components of the planar projections to get 1621 // the starting and ending angle. Note that atan2 arguments are atan2(Y,X). 1622 double theta_start = atan2(p_start_2, p_start_1); 1623 double theta_end= atan2(p_end_2,p_end_1); 1624 double start_radius = hypot(p_start_1, p_start_2); 1625 double end_radius = hypot(p_end_1, p_end_2); 1626 canon_debug("radius = %f\n",start_radius); 1627 canon_debug("raw values: theta_end = %.17e, theta_start = %.17e\n", theta_end, theta_start); 1628 1629 // Correct for angle wrap so that theta_end - theta_start > 0 1630 int is_clockwise = rotation < 0; 1631 1632 // FIXME should be a constant in canon.hh or elsewhere 1633 const double min_arc_angle = 1e-12; 1634 1635 if (is_clockwise) { 1636 if((theta_end + min_arc_angle) >= theta_start) theta_end -= M_PI * 2.0; 1637 } else { 1638 if((theta_end - min_arc_angle) <= theta_start) theta_end += M_PI * 2.0; 1639 } 1640 1641 canon_debug("theta_end = %f, theta_start = %f\n", theta_end, theta_start); 1642 1643 /* 1644 mapping of rotation to full turns: 1645 1646 rotation full COUNTERCLOCKWISE turns (- implies clockwise) 1647 -------- ----- 1648 0 none (linear move) 1649 1 0 1650 2 1 1651 -1 0 1652 -2 -1 */ 1653 1654 // Compute the number of FULL turns in addition to the principal angle 1655 int full_turns = 0; 1656 if (rotation > 1) { 1657 full_turns = rotation - 1; 1658 } 1659 if (rotation < -1) { 1660 full_turns = rotation + 1; 1661 } 1662 1663 double angle = theta_end - theta_start; 1664 double full_angle = angle + 2.0 * M_PI * (double)full_turns; 1665 canon_debug("angle = %f\n", angle); 1666 canon_debug("full turns = %d\n", full_turns); 1667 1668 canon_debug("full_angle = %.17e\n", full_angle); 1669 1670 //Use total angle to get spiral properties 1671 double spiral = end_radius - start_radius; 1672 double dr = spiral / fabs(full_angle); 1673 double min_radius = fmin(start_radius, end_radius); 1674 double effective_radius = sqrt(dr*dr + min_radius*min_radius); 1675 1676 // KLUDGE: assumes 0,1,2 for X Y Z 1677 // Find normal axis 1678 int norm_axis_ind = (2 - shift_ind) % 3; 1679 // Find maximum velocities and accelerations for planar axes 1680 int axis1 = (norm_axis_ind + 1) % 3; 1681 int axis2 = (norm_axis_ind + 2) % 3; 1682 1683 canon_debug("axis1 = %d, axis2 = %d\n",axis1, axis2); 1684 1685 // Get planar velocity bounds 1686 double v1 = FROM_EXT_LEN(emcAxisGetMaxVelocity(axis1)); 1687 double v2 = FROM_EXT_LEN(emcAxisGetMaxVelocity(axis2)); 1688 1689 // Get planar acceleration bounds 1690 double a1 = FROM_EXT_LEN(emcAxisGetMaxAcceleration(axis1)); 1691 double a2 = FROM_EXT_LEN(emcAxisGetMaxAcceleration(axis2)); 1692 double v_max_axes = MIN(v1, v2); 1693 double a_max_axes = MIN(a1, a2); 1694 1695 if(canon.xy_rotation && canon.activePlane != CANON_PLANE_XY) { 1696 // also consider the third plane's constraint, which may get 1697 // involved since we're rotated. 1698 1699 int axis3 = (norm_axis_ind + 3) % 3; 1700 if (axis_valid(axis3)) { 1701 double v3 = FROM_EXT_LEN(emcAxisGetMaxVelocity(axis3)); 1702 double a3 = FROM_EXT_LEN(emcAxisGetMaxAcceleration(axis3)); 1703 v_max_axes = MIN(v3, v_max_axes); 1704 a_max_axes = MIN(a3, a_max_axes); 1705 } 1706 } 1707 1708 //FIXME allow tangential acceleration like in TP 1709 double a_max_normal = a_max_axes * sqrt(3.0)/2.0; 1710 canon_debug("a_max_axes = %f\n", a_max_axes); 1711 1712 // Compute the centripetal acceleration 1713 double v_max_radial = sqrt(a_max_normal * effective_radius); 1714 canon_debug("v_max_radial = %f\n", v_max_radial); 1715 1716 // Restrict our maximum velocity in-plane if need be 1717 double v_max_planar = MIN(v_max_radial, v_max_axes); 1718 canon_debug("v_max_planar = %f\n", v_max_planar); 1719 1720 // Find the equivalent maximum velocity for a linear displacement 1721 // This accounts for speed restrictions due to helical and other axes 1722 VelData veldata = getStraightVelocity(endpt); 1723 1724 // Compute spiral length, first by the minimum circular arc length 1725 double circular_length = min_radius * fabs(full_angle); 1726 // Then by linear approximation of the spiral arc length function of angle 1727 // TODO use quadratic approximation 1728 double spiral_length = hypot(circular_length, spiral); 1729 1730 // Compute length along normal axis and total XYZ arc length 1731 double axis_len = dot(end_cart - canon.endPoint.xyz(), normal_cart); 1732 double total_xyz_length = hypot(spiral_length, axis_len); 1733 1734 // Next, compute the minimum time that we must take to complete the segment. 1735 // The motion computation gives us min time needed for the helical and auxiliary axes 1736 double t_max_motion = veldata.tmax; 1737 // Assumes worst case that velocity can be in any direction in the plane, so 1738 // we assume tangential velocity is always less than the planar velocity limit. 1739 // The spiral time is the min time needed to stay under the planar velocity limit. 1740 double t_max_spiral = spiral_length / v_max_planar; 1741 1742 // Now, compute actual XYZ max velocity from this min time and the total arc length 1743 double t_max = fmax(t_max_motion, t_max_spiral); 1744 1745 double v_max = total_xyz_length / t_max; 1746 canon_debug("v_max = %f\n", v_max); 1747 1748 1749 //COMPUTE ACCEL 1750 1751 // Use "straight" acceleration measure to compute acceleration bounds due 1752 // to non-circular components (helical axis, other axes) 1753 AccelData accdata = getStraightAcceleration(endpt); 1754 1755 double tt_max_motion = accdata.tmax; 1756 double tt_max_spiral = spiral_length / a_max_axes; 1757 double tt_max = fmax(tt_max_motion, tt_max_spiral); 1758 1759 // a_max could be higher than a_max_axes, but the projection onto the 1760 // circle plane and helical axis will still be within limits 1761 double a_max = total_xyz_length / tt_max; 1762 1763 // Limit velocity by maximum 1764 double vel = MIN(canon.linearFeedRate, v_max); 1765 canon_debug("current F = %f\n",canon.linearFeedRate); 1766 canon_debug("vel = %f\n",vel); 1767 1768 canon_debug("v_max = %f\n",v_max); 1769 canon_debug("a_max = %f\n",a_max); 1770 1771 canon.cartesian_move = 1; 1772 1773 if (rotation == 0) { 1774 // linear move 1775 // FIXME (Rob) Am I missing something? the P word should never be zero, 1776 // or we wouldn't be calling ARC_FEED 1777 linearMoveMsg.end = to_ext_pose(endpt); 1778 linearMoveMsg.type = EMC_MOTION_TYPE_ARC; 1779 linearMoveMsg.vel = toExtVel(vel); 1780 linearMoveMsg.ini_maxvel = toExtVel(v_max); 1781 linearMoveMsg.acc = toExtAcc(a_max); 1782 linearMoveMsg.indexer_jnum = -1; 1783 if(vel && a_max){ 1784 interp_list.set_line_number(line_number); 1785 interp_list.append(linearMoveMsg); 1786 } 1787 } else { 1788 circularMoveMsg.end = to_ext_pose(endpt); 1789 1790 // Convert internal center and normal to external units 1791 circularMoveMsg.center = to_ext_len(center_cart); 1792 circularMoveMsg.normal = to_ext_len(normal_cart); 1793 1794 if (rotation > 0) 1795 circularMoveMsg.turn = rotation - 1; 1796 else 1797 // reverse turn 1798 circularMoveMsg.turn = rotation; 1799 1800 circularMoveMsg.type = EMC_MOTION_TYPE_ARC; 1801 1802 circularMoveMsg.vel = toExtVel(vel); 1803 circularMoveMsg.ini_maxvel = toExtVel(v_max); 1804 circularMoveMsg.acc = toExtAcc(a_max); 1805 1806 //FIXME what happens if accel or vel is zero? 1807 // The end point is still updated, but nothing is added to the interp list 1808 // seems to be a crude way to indicate a zero length segment? 1809 if(vel && a_max) { 1810 interp_list.set_line_number(line_number); 1811 interp_list.append(circularMoveMsg); 1812 } 1813 } 1814 // update the end point 1815 canonUpdateEndPoint(endpt); 1816 } 1817 1818 1819 void DWELL(double seconds) 1820 { 1821 EMC_TRAJ_DELAY delayMsg; 1822 1823 flush_segments(); 1824 1825 delayMsg.delay = seconds; 1826 1827 interp_list.append(delayMsg); 1828 } 1829 1830 /* Spindle Functions */ 1831 void SPINDLE_RETRACT_TRAVERSE() 1832 { 1833 /*! \todo FIXME-- unimplemented */ 1834 } 1835 1836 void SET_SPINDLE_MODE(int spindle, double css_max) { 1837 canon.spindle[spindle].css_maximum = fabs(css_max); 1838 } 1839 1840 void START_SPINDLE_CLOCKWISE(int s, int wait_for_atspeed) 1841 { 1842 EMC_SPINDLE_ON emc_spindle_on_msg; 1843 1844 flush_segments(); 1845 canon.spindle[s].dir = 1; 1846 emc_spindle_on_msg.spindle = s; 1847 if(canon.spindle[s].css_maximum) { 1848 if(canon.lengthUnits == CANON_UNITS_INCHES){ 1849 canon.spindle[s].css_factor = 12 / (2 * M_PI) * canon.spindle[s].speed * TO_EXT_LEN(25.4); 1850 } else { 1851 canon.spindle[s].css_factor = 1000 / (2 * M_PI) * canon.spindle[s].speed * TO_EXT_LEN(1); 1852 } 1853 emc_spindle_on_msg.speed = canon.spindle[s].dir * canon.spindle[s].css_maximum; 1854 emc_spindle_on_msg.factor = canon.spindle[s].dir * canon.spindle[s].css_factor; 1855 emc_spindle_on_msg.xoffset = TO_EXT_LEN(canon.g5xOffset.x + canon.g92Offset.x + canon.toolOffset.tran.x); 1856 } else { 1857 emc_spindle_on_msg.speed = canon.spindle[s].dir * canon.spindle[s].speed; 1858 // canon.css_numerator = 0; FIXME: Do we need this? 1859 } 1860 emc_spindle_on_msg.wait_for_spindle_at_speed = wait_for_atspeed; 1861 interp_list.append(emc_spindle_on_msg); 1862 } 1863 1864 void START_SPINDLE_COUNTERCLOCKWISE(int s, int wait_for_atspeed) 1865 { 1866 EMC_SPINDLE_ON emc_spindle_on_msg; 1867 1868 flush_segments(); 1869 canon.spindle[s].dir = -1; 1870 emc_spindle_on_msg.spindle = s; 1871 if(canon.spindle[s].css_maximum) { 1872 if(canon.lengthUnits == CANON_UNITS_INCHES){ 1873 canon.spindle[s].css_factor = 12 / (2 * M_PI) * canon.spindle[s].speed * TO_EXT_LEN(25.4); 1874 } else { 1875 canon.spindle[s].css_factor = 1000 / (2 * M_PI) * canon.spindle[s].speed * TO_EXT_LEN(1); 1876 } 1877 emc_spindle_on_msg.speed = canon.spindle[s].dir * canon.spindle[s].css_maximum; 1878 emc_spindle_on_msg.factor = canon.spindle[s].dir * canon.spindle[s].css_factor; 1879 emc_spindle_on_msg.xoffset = TO_EXT_LEN(canon.g5xOffset.x + canon.g92Offset.x + canon.toolOffset.tran.x); 1880 } else { 1881 emc_spindle_on_msg.speed = canon.spindle[s].dir * canon.spindle[s].speed; 1882 // canon.css_numerator = 0; FIXME: Do we need this? 1883 } 1884 emc_spindle_on_msg.wait_for_spindle_at_speed = wait_for_atspeed; 1885 interp_list.append(emc_spindle_on_msg); 1886 } 1887 1888 void SET_SPINDLE_SPEED(int s, double r) 1889 { 1890 // speed is in RPMs everywhere 1891 1892 canon.spindle[s].speed = fabs(r); // interp will never send negative anyway ... 1893 1894 EMC_SPINDLE_SPEED emc_spindle_speed_msg; 1895 1896 flush_segments(); 1897 1898 emc_spindle_speed_msg.spindle = s; 1899 if(canon.spindle[s].css_maximum) { 1900 if(canon.lengthUnits == CANON_UNITS_INCHES){ 1901 canon.spindle[s].css_factor = 12 / (2 * M_PI) * canon.spindle[s].speed * TO_EXT_LEN(25.4); 1902 } else { 1903 canon.spindle[s].css_factor = 1000 / (2 * M_PI) * canon.spindle[s].speed * TO_EXT_LEN(1); 1904 } 1905 emc_spindle_speed_msg.speed = canon.spindle[s].dir * canon.spindle[s].css_maximum; 1906 emc_spindle_speed_msg.factor = canon.spindle[s].dir * canon.spindle[s].css_factor; 1907 emc_spindle_speed_msg.xoffset = TO_EXT_LEN(canon.g5xOffset.x + canon.g92Offset.x + canon.toolOffset.tran.x); 1908 } else { 1909 emc_spindle_speed_msg.speed = canon.spindle[s].dir * canon.spindle[s].speed; 1910 // canon.css_numerator = 0; FIXME: Do we need this? 1911 } 1912 interp_list.append(emc_spindle_speed_msg); 1913 } 1914 1915 void STOP_SPINDLE_TURNING(int s) 1916 { 1917 EMC_SPINDLE_OFF emc_spindle_off_msg; 1918 1919 flush_segments(); 1920 emc_spindle_off_msg.spindle = s; 1921 interp_list.append(emc_spindle_off_msg); 1922 // Added by atp 6/1/18 not sure this is right. There is a problem that the _second_ S word starts the spindle without M3/M4 1923 canon.spindle[s].dir = 0; 1924 } 1925 1926 void SPINDLE_RETRACT() 1927 { 1928 /*! \todo FIXME-- unimplemented */ 1929 } 1930 1931 void ORIENT_SPINDLE(int s, double orientation, int mode) 1932 { 1933 EMC_SPINDLE_ORIENT o; 1934 1935 flush_segments(); 1936 o.spindle = s; 1937 o.orientation = orientation; 1938 o.mode = mode; 1939 interp_list.append(o); 1940 } 1941 1942 void WAIT_SPINDLE_ORIENT_COMPLETE(int s, double timeout) 1943 { 1944 EMC_SPINDLE_WAIT_ORIENT_COMPLETE o; 1945 1946 flush_segments(); 1947 o.spindle = s; 1948 o.timeout = timeout; 1949 interp_list.append(o); 1950 } 1951 1952 void USE_SPINDLE_FORCE(void) 1953 { 1954 /*! \todo FIXME-- unimplemented */ 1955 } 1956 1957 void LOCK_SPINDLE_Z(void) 1958 { 1959 /*! \todo FIXME-- unimplemented */ 1960 } 1961 1962 void USE_NO_SPINDLE_FORCE(void) 1963 { 1964 /*! \todo FIXME-- unimplemented */ 1965 } 1966 1967 /* Tool Functions */ 1968 1969 /* this is called with distances in external (machine) units */ 1970 void SET_TOOL_TABLE_ENTRY(int pocket, int toolno, EmcPose offset, double diameter, 1971 double frontangle, double backangle, int orientation) { 1972 EMC_TOOL_SET_OFFSET o; 1973 flush_segments(); 1974 o.pocket = pocket; 1975 o.toolno = toolno; 1976 o.offset = offset; 1977 o.diameter = diameter; 1978 o.frontangle = frontangle; 1979 o.backangle = backangle; 1980 o.orientation = orientation; 1981 interp_list.append(o); 1982 } 1983 1984 /* 1985 EMC has no tool length offset. To implement it, we save it here, 1986 and apply it when necessary 1987 */ 1988 void USE_TOOL_LENGTH_OFFSET(EmcPose offset) 1989 { 1990 EMC_TRAJ_SET_OFFSET set_offset_msg; 1991 1992 flush_segments(); 1993 1994 /* convert to mm units for internal canonical use */ 1995 canon.toolOffset.tran.x = FROM_PROG_LEN(offset.tran.x); 1996 canon.toolOffset.tran.y = FROM_PROG_LEN(offset.tran.y); 1997 canon.toolOffset.tran.z = FROM_PROG_LEN(offset.tran.z); 1998 canon.toolOffset.a = FROM_PROG_ANG(offset.a); 1999 canon.toolOffset.b = FROM_PROG_ANG(offset.b); 2000 canon.toolOffset.c = FROM_PROG_ANG(offset.c); 2001 canon.toolOffset.u = FROM_PROG_LEN(offset.u); 2002 canon.toolOffset.v = FROM_PROG_LEN(offset.v); 2003 canon.toolOffset.w = FROM_PROG_LEN(offset.w); 2004 2005 /* append it to interp list so it gets updated at the right time, not at 2006 read-ahead time */ 2007 set_offset_msg.offset.tran.x = TO_EXT_LEN(canon.toolOffset.tran.x); 2008 set_offset_msg.offset.tran.y = TO_EXT_LEN(canon.toolOffset.tran.y); 2009 set_offset_msg.offset.tran.z = TO_EXT_LEN(canon.toolOffset.tran.z); 2010 set_offset_msg.offset.a = TO_EXT_ANG(canon.toolOffset.a); 2011 set_offset_msg.offset.b = TO_EXT_ANG(canon.toolOffset.b); 2012 set_offset_msg.offset.c = TO_EXT_ANG(canon.toolOffset.c); 2013 set_offset_msg.offset.u = TO_EXT_LEN(canon.toolOffset.u); 2014 set_offset_msg.offset.v = TO_EXT_LEN(canon.toolOffset.v); 2015 set_offset_msg.offset.w = TO_EXT_LEN(canon.toolOffset.w); 2016 2017 for (int s = 0; s < emcStatus->motion.traj.spindles; s++){ 2018 if(canon.spindle[s].css_maximum) { 2019 SET_SPINDLE_SPEED(s, canon.spindle[s].speed); 2020 } 2021 } 2022 interp_list.append(set_offset_msg); 2023 } 2024 2025 /* issued at very start of an M6 command. Notification. */ 2026 void START_CHANGE() 2027 { 2028 EMC_TOOL_START_CHANGE emc_start_change_msg; 2029 2030 flush_segments(); 2031 2032 interp_list.append(emc_start_change_msg); 2033 } 2034 2035 /* CHANGE_TOOL results from M6 */ 2036 void CHANGE_TOOL(int slot) 2037 { 2038 EMC_TRAJ_LINEAR_MOVE linearMoveMsg; 2039 linearMoveMsg.feed_mode = canon.feed_mode; 2040 EMC_TOOL_LOAD load_tool_msg; 2041 2042 flush_segments(); 2043 2044 /* optional move to tool change position. This 2045 * is a mess because we really want a configurable chain 2046 * of events to happen when a tool change is called for. 2047 * Since they'll probably involve motion, we can't just 2048 * do it in HAL. This is basic support for making one 2049 * move to a particular coordinate before the tool change 2050 * is called. */ 2051 2052 if (have_tool_change_position) { 2053 double vel, acc, x, y, z, a, b, c, u, v, w; 2054 2055 x = FROM_EXT_LEN(tool_change_position.tran.x); 2056 y = FROM_EXT_LEN(tool_change_position.tran.y); 2057 z = FROM_EXT_LEN(tool_change_position.tran.z); 2058 a = canon.endPoint.a; 2059 b = canon.endPoint.b; 2060 c = canon.endPoint.c; 2061 u = canon.endPoint.u; 2062 v = canon.endPoint.v; 2063 w = canon.endPoint.w; 2064 2065 if (have_tool_change_position > 3) { 2066 a = FROM_EXT_ANG(tool_change_position.a); 2067 b = FROM_EXT_ANG(tool_change_position.b); 2068 c = FROM_EXT_ANG(tool_change_position.c); 2069 } 2070 2071 if (have_tool_change_position > 6) { 2072 u = FROM_EXT_LEN(tool_change_position.u); 2073 v = FROM_EXT_LEN(tool_change_position.v); 2074 w = FROM_EXT_LEN(tool_change_position.w); 2075 } 2076 2077 VelData veldata = getStraightVelocity(x, y, z, a, b, c, u, v, w); 2078 AccelData accdata = getStraightAcceleration(x, y, z, a, b, c, u, v, w); 2079 vel = veldata.vel; 2080 acc = accdata.acc; 2081 2082 linearMoveMsg.end = to_ext_pose(x, y, z, a, b, c, u, v, w); 2083 2084 linearMoveMsg.vel = linearMoveMsg.ini_maxvel = toExtVel(vel); 2085 linearMoveMsg.acc = toExtAcc(acc); 2086 linearMoveMsg.type = EMC_MOTION_TYPE_TOOLCHANGE; 2087 linearMoveMsg.feed_mode = 0; 2088 linearMoveMsg.indexer_jnum = -1; 2089 2090 int old_feed_mode = canon.feed_mode; 2091 if(canon.feed_mode) 2092 STOP_SPEED_FEED_SYNCH(); 2093 2094 if(vel && acc) 2095 interp_list.append(linearMoveMsg); 2096 2097 if(old_feed_mode) 2098 START_SPEED_FEED_SYNCH(canon.spindle_num, canon.linearFeedRate, 1); 2099 2100 canonUpdateEndPoint(x, y, z, a, b, c, u, v, w); 2101 } 2102 2103 /* regardless of optional moves above, we'll always send a load tool 2104 message */ 2105 interp_list.append(load_tool_msg); 2106 } 2107 2108 /* SELECT_POCKET results from Tn */ 2109 void SELECT_POCKET(int slot , int tool) 2110 { 2111 EMC_TOOL_PREPARE prep_for_tool_msg; 2112 2113 prep_for_tool_msg.pocket = slot; 2114 prep_for_tool_msg.tool = tool; 2115 2116 interp_list.append(prep_for_tool_msg); 2117 } 2118 2119 /* CHANGE_TOOL_NUMBER results from M61 */ 2120 void CHANGE_TOOL_NUMBER(int pocket_number) 2121 { 2122 EMC_TOOL_SET_NUMBER emc_tool_set_number_msg; 2123 2124 emc_tool_set_number_msg.tool = pocket_number; 2125 2126 interp_list.append(emc_tool_set_number_msg); 2127 } 2128 2129 2130 /* Misc Functions */ 2131 2132 void CLAMP_AXIS(CANON_AXIS axis) 2133 { 2134 /*! \todo FIXME-- unimplemented */ 2135 } 2136 2137 /* 2138 setString and addString initializes or adds src to dst, never exceeding 2139 dst's maxlen chars. 2140 */ 2141 2142 static char *setString(char *dst, const char *src, int maxlen) 2143 { 2144 dst[0] = 0; 2145 strncat(dst, src, maxlen - 1); 2146 dst[maxlen - 1] = 0; 2147 return dst; 2148 } 2149 2150 static char *addString(char *dst, const char *src, int maxlen) 2151 { 2152 int dstlen = strlen(dst); 2153 int srclen = strlen(src); 2154 int actlen; 2155 2156 if (srclen >= maxlen - dstlen) { 2157 actlen = maxlen - dstlen - 1; 2158 dst[maxlen - 1] = 0; 2159 } else { 2160 actlen = srclen; 2161 } 2162 2163 strncat(dst, src, actlen); 2164 2165 return dst; 2166 } 2167 2168 /* 2169 The probe file is opened with a hot-comment (PROBEOPEN <filename>), 2170 and the results of each probed point are written to that file. 2171 The file is closed with a (PROBECLOSE) comment. 2172 */ 2173 2174 static FILE *probefile = NULL; 2175 2176 void COMMENT(const char *comment) 2177 { 2178 // nothing need be done here, but you can play tricks with hot comments 2179 2180 char msg[LINELEN]; 2181 char probefilename[LINELEN]; 2182 const char *ptr; 2183 2184 // set RPY orientation for subsequent moves 2185 if (!strncmp(comment, "RPY", strlen("RPY"))) { 2186 PM_RPY rpy; 2187 // it's RPY <R> <P> <Y> 2188 if (3 != 2189 sscanf(comment, "%*s %lf %lf %lf", &rpy.r, &rpy.p, &rpy.y)) { 2190 // print current orientation 2191 printf("rpy = %f %f %f, quat = %f %f %f %f\n", 2192 rpy.r, rpy.p, rpy.y, quat.s, quat.x, quat.y, quat.z); 2193 } else { 2194 // set and print orientation 2195 quat = rpy; 2196 printf("rpy = %f %f %f, quat = %f %f %f %f\n", 2197 rpy.r, rpy.p, rpy.y, quat.s, quat.x, quat.y, quat.z); 2198 } 2199 return; 2200 } 2201 // open probe output file 2202 if (!strncmp(comment, "PROBEOPEN", strlen("PROBEOPEN"))) { 2203 // position ptr to first char after PROBEOPEN 2204 ptr = &comment[strlen("PROBEOPEN")]; 2205 // and step over white space to name, or NULL 2206 while (isspace(*ptr)) { 2207 ptr++; 2208 } 2209 setString(probefilename, ptr, LINELEN); 2210 if (NULL == (probefile = fopen(probefilename, "wt"))) { 2211 // pop up a warning message 2212 setString(msg, "can't open probe file ", LINELEN); 2213 addString(msg, probefilename, LINELEN); 2214 MESSAGE(msg); 2215 probefile = NULL; 2216 } 2217 return; 2218 } 2219 // close probe output file 2220 if (!strncmp(comment, "PROBECLOSE", strlen("PROBECLOSE"))) { 2221 if (probefile != NULL) { 2222 fclose(probefile); 2223 probefile = NULL; 2224 } 2225 return; 2226 } 2227 2228 return; 2229 } 2230 2231 // refers to feed rate 2232 void DISABLE_FEED_OVERRIDE() 2233 { 2234 EMC_TRAJ_SET_FO_ENABLE set_fo_enable_msg; 2235 flush_segments(); 2236 2237 set_fo_enable_msg.mode = 0; 2238 interp_list.append(set_fo_enable_msg); 2239 } 2240 2241 void ENABLE_FEED_OVERRIDE() 2242 { 2243 EMC_TRAJ_SET_FO_ENABLE set_fo_enable_msg; 2244 flush_segments(); 2245 2246 set_fo_enable_msg.mode = 1; 2247 interp_list.append(set_fo_enable_msg); 2248 } 2249 2250 2251 //refers to adaptive feed override (HAL input, useful for EDM for example) 2252 void DISABLE_ADAPTIVE_FEED() 2253 { 2254 EMC_MOTION_ADAPTIVE emcmotAdaptiveMsg; 2255 flush_segments(); 2256 2257 emcmotAdaptiveMsg.status = 0; 2258 interp_list.append(emcmotAdaptiveMsg); 2259 } 2260 2261 void ENABLE_ADAPTIVE_FEED() 2262 { 2263 EMC_MOTION_ADAPTIVE emcmotAdaptiveMsg; 2264 flush_segments(); 2265 2266 emcmotAdaptiveMsg.status = 1; 2267 interp_list.append(emcmotAdaptiveMsg); 2268 } 2269 2270 //refers to spindle speed 2271 void DISABLE_SPEED_OVERRIDE(int spindle) 2272 { 2273 EMC_TRAJ_SET_SO_ENABLE set_so_enable_msg; 2274 flush_segments(); 2275 2276 set_so_enable_msg.mode = 0; 2277 set_so_enable_msg.spindle = spindle; 2278 interp_list.append(set_so_enable_msg); 2279 } 2280 2281 2282 void ENABLE_SPEED_OVERRIDE(int spindle) 2283 { 2284 EMC_TRAJ_SET_SO_ENABLE set_so_enable_msg; 2285 flush_segments(); 2286 2287 set_so_enable_msg.mode = 1; 2288 set_so_enable_msg.spindle = spindle; 2289 interp_list.append(set_so_enable_msg); 2290 } 2291 2292 void ENABLE_FEED_HOLD() 2293 { 2294 EMC_TRAJ_SET_FH_ENABLE set_feed_hold_msg; 2295 flush_segments(); 2296 2297 set_feed_hold_msg.mode = 1; 2298 interp_list.append(set_feed_hold_msg); 2299 } 2300 2301 void DISABLE_FEED_HOLD() 2302 { 2303 EMC_TRAJ_SET_FH_ENABLE set_feed_hold_msg; 2304 flush_segments(); 2305 2306 set_feed_hold_msg.mode = 0; 2307 interp_list.append(set_feed_hold_msg); 2308 } 2309 2310 void FLOOD_OFF() 2311 { 2312 EMC_COOLANT_FLOOD_OFF flood_off_msg; 2313 2314 flush_segments(); 2315 2316 interp_list.append(flood_off_msg); 2317 } 2318 2319 void FLOOD_ON() 2320 { 2321 EMC_COOLANT_FLOOD_ON flood_on_msg; 2322 2323 flush_segments(); 2324 2325 interp_list.append(flood_on_msg); 2326 } 2327 2328 void MESSAGE(char *s) 2329 { 2330 EMC_OPERATOR_DISPLAY operator_display_msg; 2331 2332 flush_segments(); 2333 operator_display_msg.id = 0; 2334 strncpy(operator_display_msg.display, s, LINELEN); 2335 operator_display_msg.display[LINELEN - 1] = 0; 2336 interp_list.append(operator_display_msg); 2337 } 2338 2339 static FILE *logfile = NULL; 2340 2341 void LOG(char *s) { 2342 flush_segments(); 2343 if(logfile) { fprintf(logfile, "%s\n", s); fflush(logfile); } 2344 fprintf(stderr, "LOG(%s)\n", s); 2345 2346 } 2347 2348 void LOGOPEN(char *name) { 2349 if(logfile) fclose(logfile); 2350 logfile = fopen(name, "wt"); 2351 fprintf(stderr, "LOGOPEN(%s) -> %p\n", name, logfile); 2352 } 2353 2354 void LOGAPPEND(char *name) { 2355 if(logfile) fclose(logfile); 2356 logfile = fopen(name, "at"); 2357 fprintf(stderr, "LOGAPPEND(%s) -> %p\n", name, logfile); 2358 } 2359 2360 2361 void LOGCLOSE() { 2362 if(logfile) fclose(logfile); 2363 logfile = NULL; 2364 fprintf(stderr, "LOGCLOSE()\n"); 2365 } 2366 2367 void MIST_OFF() 2368 { 2369 EMC_COOLANT_MIST_OFF mist_off_msg; 2370 2371 flush_segments(); 2372 2373 interp_list.append(mist_off_msg); 2374 } 2375 2376 void MIST_ON() 2377 { 2378 EMC_COOLANT_MIST_ON mist_on_msg; 2379 2380 flush_segments(); 2381 2382 interp_list.append(mist_on_msg); 2383 } 2384 2385 void PALLET_SHUTTLE() 2386 { 2387 /*! \todo FIXME-- unimplemented */ 2388 } 2389 2390 void TURN_PROBE_OFF() 2391 { 2392 // don't do anything-- this is called when the probing is done 2393 } 2394 2395 void TURN_PROBE_ON() 2396 { 2397 EMC_TRAJ_CLEAR_PROBE_TRIPPED_FLAG clearMsg; 2398 2399 interp_list.append(clearMsg); 2400 } 2401 2402 void UNCLAMP_AXIS(CANON_AXIS axis) 2403 { 2404 /*! \todo FIXME-- unimplemented */ 2405 } 2406 2407 /* Program Functions */ 2408 2409 void PROGRAM_STOP() 2410 { 2411 /* 2412 implement this as a pause. A resume will cause motion to proceed. */ 2413 EMC_TASK_PLAN_PAUSE pauseMsg; 2414 2415 flush_segments(); 2416 2417 interp_list.append(pauseMsg); 2418 } 2419 2420 void SET_BLOCK_DELETE(bool state) 2421 { 2422 canon.block_delete = state; //state == ON, means we don't interpret lines starting with "/" 2423 } 2424 2425 bool GET_BLOCK_DELETE() 2426 { 2427 return canon.block_delete; //state == ON, means we don't interpret lines starting with "/" 2428 } 2429 2430 2431 void SET_OPTIONAL_PROGRAM_STOP(bool state) 2432 { 2433 canon.optional_program_stop = state; //state == ON, means we stop 2434 } 2435 2436 bool GET_OPTIONAL_PROGRAM_STOP() 2437 { 2438 return canon.optional_program_stop; //state == ON, means we stop 2439 } 2440 2441 void OPTIONAL_PROGRAM_STOP() 2442 { 2443 EMC_TASK_PLAN_OPTIONAL_STOP stopMsg; 2444 2445 flush_segments(); 2446 2447 interp_list.append(stopMsg); 2448 } 2449 2450 void PROGRAM_END() 2451 { 2452 flush_segments(); 2453 2454 EMC_TASK_PLAN_END endMsg; 2455 2456 interp_list.append(endMsg); 2457 } 2458 2459 double GET_EXTERNAL_TOOL_LENGTH_XOFFSET() 2460 { 2461 return TO_PROG_LEN(canon.toolOffset.tran.x); 2462 } 2463 2464 double GET_EXTERNAL_TOOL_LENGTH_YOFFSET() 2465 { 2466 return TO_PROG_LEN(canon.toolOffset.tran.y); 2467 } 2468 2469 double GET_EXTERNAL_TOOL_LENGTH_ZOFFSET() 2470 { 2471 return TO_PROG_LEN(canon.toolOffset.tran.z); 2472 } 2473 2474 double GET_EXTERNAL_TOOL_LENGTH_AOFFSET() 2475 { 2476 return TO_PROG_ANG(canon.toolOffset.a); 2477 } 2478 2479 double GET_EXTERNAL_TOOL_LENGTH_BOFFSET() 2480 { 2481 return TO_PROG_ANG(canon.toolOffset.b); 2482 } 2483 2484 double GET_EXTERNAL_TOOL_LENGTH_COFFSET() 2485 { 2486 return TO_PROG_ANG(canon.toolOffset.c); 2487 } 2488 2489 double GET_EXTERNAL_TOOL_LENGTH_UOFFSET() 2490 { 2491 return TO_PROG_LEN(canon.toolOffset.u); 2492 } 2493 2494 double GET_EXTERNAL_TOOL_LENGTH_VOFFSET() 2495 { 2496 return TO_PROG_LEN(canon.toolOffset.v); 2497 } 2498 2499 double GET_EXTERNAL_TOOL_LENGTH_WOFFSET() 2500 { 2501 return TO_PROG_LEN(canon.toolOffset.w); 2502 } 2503 2504 /* 2505 INIT_CANON() 2506 Initialize canonical local variables to defaults 2507 */ 2508 void INIT_CANON() 2509 { 2510 double units; 2511 2512 chained_points.clear(); 2513 2514 // initialize locals to original values 2515 canon.xy_rotation = 0.0; 2516 canon.rotary_unlock_for_traverse = -1; 2517 canon.feed_mode = 0; 2518 canon.g5xOffset.x = 0.0; 2519 canon.g5xOffset.y = 0.0; 2520 canon.g5xOffset.z = 0.0; 2521 canon.g5xOffset.a = 0.0; 2522 canon.g5xOffset.b = 0.0; 2523 canon.g5xOffset.c = 0.0; 2524 canon.g5xOffset.u = 0.0; 2525 canon.g5xOffset.v = 0.0; 2526 canon.g5xOffset.w = 0.0; 2527 canon.g92Offset.x = 0.0; 2528 canon.g92Offset.y = 0.0; 2529 canon.g92Offset.z = 0.0; 2530 canon.g92Offset.a = 0.0; 2531 canon.g92Offset.b = 0.0; 2532 canon.g92Offset.c = 0.0; 2533 canon.g92Offset.u = 0.0; 2534 canon.g92Offset.v = 0.0; 2535 canon.g92Offset.w = 0.0; 2536 SELECT_PLANE(CANON_PLANE_XY); 2537 canonUpdateEndPoint(0, 0, 0, 0, 0, 0, 0, 0, 0); 2538 SET_MOTION_CONTROL_MODE(CANON_CONTINUOUS, 0); 2539 SET_NAIVECAM_TOLERANCE(0); 2540 for (int s = 0; s < EMCMOT_MAX_SPINDLES; s++) { 2541 canon.spindle[s].speed = 0.0; 2542 canon.spindle[s].synched = 0; 2543 } 2544 canon.optional_program_stop = ON; //set enabled by default (previous EMC behaviour) 2545 canon.block_delete = ON; //set enabled by default (previous EMC behaviour) 2546 canon.cartesian_move = 0; 2547 canon.angular_move = 0; 2548 canon.linearFeedRate = 0.0; 2549 canon.angularFeedRate = 0.0; 2550 ZERO_EMC_POSE(canon.toolOffset); 2551 2552 /* 2553 to set the units, note that GET_EXTERNAL_LENGTH_UNITS() returns 2554 traj->linearUnits, which is already set from the .ini file in 2555 iniTraj(). This is a floating point number, in user units per mm. We 2556 can compare this against known values and set the symbolic values 2557 accordingly. If it doesn't match, we have an error. */ 2558 units = GET_EXTERNAL_LENGTH_UNITS(); 2559 if (fabs(units - 1.0 / 25.4) < 1.0e-3) { 2560 canon.lengthUnits = CANON_UNITS_INCHES; 2561 } else if (fabs(units - 1.0) < 1.0e-3) { 2562 canon.lengthUnits = CANON_UNITS_MM; 2563 } else { 2564 CANON_ERROR 2565 ("non-standard length units, setting interpreter to mm"); 2566 canon.lengthUnits = CANON_UNITS_MM; 2567 } 2568 } 2569 2570 /* Sends error message */ 2571 void CANON_ERROR(const char *fmt, ...) 2572 { 2573 va_list ap; 2574 EMC_OPERATOR_ERROR operator_error_msg; 2575 2576 flush_segments(); 2577 2578 operator_error_msg.id = 0; 2579 if (fmt != NULL) { 2580 va_start(ap, fmt); 2581 vsnprintf(operator_error_msg.error,sizeof(operator_error_msg.error), fmt, ap); 2582 va_end(ap); 2583 } else { 2584 operator_error_msg.error[0] = 0; 2585 } 2586 2587 interp_list.append(operator_error_msg); 2588 } 2589 2590 /* 2591 GET_EXTERNAL_TOOL_TABLE(int pocket) 2592 2593 Returns the tool table structure associated with pocket. Note that 2594 pocket can run from 0 (by definition, the spindle), to pocket CANON_POCKETS_MAX - 1. 2595 2596 Tool table is always in machine units. 2597 2598 */ 2599 CANON_TOOL_TABLE GET_EXTERNAL_TOOL_TABLE(int pocket) 2600 { 2601 CANON_TOOL_TABLE retval; 2602 2603 if (pocket < 0 || pocket >= CANON_POCKETS_MAX) { 2604 retval.toolno = -1; 2605 ZERO_EMC_POSE(retval.offset); 2606 retval.frontangle = 0.0; 2607 retval.backangle = 0.0; 2608 retval.diameter = 0.0; 2609 retval.orientation = 0; 2610 } else { 2611 retval = emcStatus->io.tool.toolTable[pocket]; 2612 } 2613 2614 return retval; 2615 } 2616 2617 CANON_POSITION GET_EXTERNAL_POSITION() 2618 { 2619 CANON_POSITION position; 2620 EmcPose pos; 2621 2622 drop_segments(); 2623 2624 pos = emcStatus->motion.traj.position; 2625 2626 if (GET_EXTERNAL_OFFSET_APPLIED() ) { 2627 EmcPose eoffset = GET_EXTERNAL_OFFSETS(); 2628 pos.tran.x -= eoffset.tran.x; 2629 pos.tran.y -= eoffset.tran.y; 2630 pos.tran.z -= eoffset.tran.z; 2631 pos.a -= eoffset.a; 2632 pos.b -= eoffset.b; 2633 pos.c -= eoffset.c; 2634 pos.u -= eoffset.u; 2635 pos.v -= eoffset.v; 2636 pos.w -= eoffset.w; 2637 } 2638 2639 // first update internal record of last position 2640 canonUpdateEndPoint(FROM_EXT_LEN(pos.tran.x), FROM_EXT_LEN(pos.tran.y), FROM_EXT_LEN(pos.tran.z), 2641 FROM_EXT_ANG(pos.a), FROM_EXT_ANG(pos.b), FROM_EXT_ANG(pos.c), 2642 FROM_EXT_LEN(pos.u), FROM_EXT_LEN(pos.v), FROM_EXT_LEN(pos.w)); 2643 2644 // now calculate position in program units, for interpreter 2645 position = unoffset_and_unrotate_pos(canon.endPoint); 2646 to_prog(position); 2647 2648 return position; 2649 } 2650 2651 CANON_POSITION GET_EXTERNAL_PROBE_POSITION() 2652 { 2653 CANON_POSITION position; 2654 EmcPose pos; 2655 static CANON_POSITION last_probed_position; 2656 2657 flush_segments(); 2658 2659 pos = emcStatus->motion.traj.probedPosition; 2660 2661 // first update internal record of last position 2662 pos.tran.x = FROM_EXT_LEN(pos.tran.x); 2663 pos.tran.y = FROM_EXT_LEN(pos.tran.y); 2664 pos.tran.z = FROM_EXT_LEN(pos.tran.z); 2665 2666 pos.a = FROM_EXT_ANG(pos.a); 2667 pos.b = FROM_EXT_ANG(pos.b); 2668 pos.c = FROM_EXT_ANG(pos.c); 2669 2670 pos.u = FROM_EXT_LEN(pos.u); 2671 pos.v = FROM_EXT_LEN(pos.v); 2672 pos.w = FROM_EXT_LEN(pos.w); 2673 2674 // now calculate position in program units, for interpreter 2675 position = unoffset_and_unrotate_pos(pos); 2676 to_prog(position); 2677 2678 if (probefile != NULL) { 2679 if (last_probed_position != position) { 2680 fprintf(probefile, "%f %f %f %f %f %f %f %f %f\n", 2681 position.x, position.y, position.z, 2682 position.a, position.b, position.c, 2683 position.u, position.v, position.w); 2684 last_probed_position = position; 2685 } 2686 } 2687 2688 return position; 2689 } 2690 2691 int GET_EXTERNAL_PROBE_TRIPPED_VALUE() 2692 { 2693 return emcStatus->motion.traj.probe_tripped; 2694 } 2695 2696 double GET_EXTERNAL_PROBE_VALUE() 2697 { 2698 // only for analog non-contact probe, so force a 0 2699 return 0.0; 2700 } 2701 2702 // feed rate wanted is in program units per minute 2703 double GET_EXTERNAL_FEED_RATE() 2704 { 2705 double feed; 2706 2707 if (canon.feed_mode) { 2708 // We're in G95 "Units per Revolution" mode, so linearFeedRate 2709 // is the FPR and we should just return it, unchanged. 2710 feed = canon.linearFeedRate; 2711 } else { 2712 // We're in G94 "Units per Minute" mode so unhork linearFeedRate 2713 // before returning it, by converting from internal to program 2714 // units, and from "per second" to "per minute". 2715 feed = TO_PROG_LEN(canon.linearFeedRate); 2716 feed *= 60.0; 2717 } 2718 2719 return feed; 2720 } 2721 2722 // traverse rate wanted is in program units per minute 2723 double GET_EXTERNAL_TRAVERSE_RATE() 2724 { 2725 double traverse; 2726 2727 // convert from external to program units 2728 traverse = 2729 TO_PROG_LEN(FROM_EXT_LEN(emcStatus->motion.traj.maxVelocity)); 2730 2731 // now convert from per-sec to per-minute 2732 traverse *= 60.0; 2733 2734 return traverse; 2735 } 2736 2737 double GET_EXTERNAL_LENGTH_UNITS(void) 2738 { 2739 double u; 2740 2741 u = emcStatus->motion.traj.linearUnits; 2742 2743 if (u == 0) { 2744 CANON_ERROR("external length units are zero"); 2745 return 1.0; 2746 } else { 2747 return u; 2748 } 2749 } 2750 2751 double GET_EXTERNAL_ANGLE_UNITS(void) 2752 { 2753 double u; 2754 2755 u = emcStatus->motion.traj.angularUnits; 2756 2757 if (u == 0) { 2758 CANON_ERROR("external angle units are zero"); 2759 return 1.0; 2760 } else { 2761 return u; 2762 } 2763 } 2764 2765 int GET_EXTERNAL_MIST() 2766 { 2767 return emcStatus->io.coolant.mist; 2768 } 2769 2770 int GET_EXTERNAL_FLOOD() 2771 { 2772 return emcStatus->io.coolant.flood; 2773 } 2774 2775 double GET_EXTERNAL_SPEED(int spindle) 2776 { 2777 // speed is in RPMs everywhere 2778 return canon.spindle[spindle].speed; 2779 } 2780 2781 CANON_DIRECTION GET_EXTERNAL_SPINDLE(int spindle) 2782 { 2783 if (emcStatus->motion.spindle[spindle].speed == 0) { 2784 return CANON_STOPPED; 2785 } 2786 2787 if (emcStatus->motion.spindle[spindle].speed >= 0.0) { 2788 return CANON_CLOCKWISE; 2789 } 2790 2791 return CANON_COUNTERCLOCKWISE; 2792 } 2793 2794 int GET_EXTERNAL_POCKETS_MAX() 2795 { 2796 return CANON_POCKETS_MAX; 2797 } 2798 2799 static char _parameter_file_name[LINELEN]; 2800 2801 void SET_PARAMETER_FILE_NAME(const char *name) 2802 { 2803 strncpy(_parameter_file_name, name, PARAMETER_FILE_NAME_LENGTH); 2804 } 2805 2806 void GET_EXTERNAL_PARAMETER_FILE_NAME(char *file_name, /* string: to copy 2807 file name into */ 2808 int max_size) 2809 { /* maximum number of characters to copy */ 2810 // Paranoid checks 2811 if (0 == file_name) 2812 return; 2813 2814 if (max_size < 0) 2815 return; 2816 2817 if (strlen(_parameter_file_name) < ((size_t) max_size)) 2818 strcpy(file_name, _parameter_file_name); 2819 else 2820 file_name[0] = 0; 2821 } 2822 2823 double GET_EXTERNAL_POSITION_X(void) 2824 { 2825 CANON_POSITION position; 2826 position = GET_EXTERNAL_POSITION(); 2827 return position.x; 2828 } 2829 2830 double GET_EXTERNAL_POSITION_Y(void) 2831 { 2832 CANON_POSITION position; 2833 position = GET_EXTERNAL_POSITION(); 2834 return position.y; 2835 } 2836 2837 double GET_EXTERNAL_POSITION_Z(void) 2838 { 2839 CANON_POSITION position; 2840 position = GET_EXTERNAL_POSITION(); 2841 return position.z; 2842 } 2843 2844 double GET_EXTERNAL_POSITION_A(void) 2845 { 2846 CANON_POSITION position; 2847 position = GET_EXTERNAL_POSITION(); 2848 return position.a; 2849 } 2850 2851 double GET_EXTERNAL_POSITION_B(void) 2852 { 2853 CANON_POSITION position; 2854 position = GET_EXTERNAL_POSITION(); 2855 return position.b; 2856 } 2857 2858 double GET_EXTERNAL_POSITION_C(void) 2859 { 2860 CANON_POSITION position; 2861 position = GET_EXTERNAL_POSITION(); 2862 return position.c; 2863 } 2864 2865 double GET_EXTERNAL_POSITION_U(void) 2866 { 2867 CANON_POSITION position; 2868 position = GET_EXTERNAL_POSITION(); 2869 return position.u; 2870 } 2871 2872 double GET_EXTERNAL_POSITION_V(void) 2873 { 2874 CANON_POSITION position; 2875 position = GET_EXTERNAL_POSITION(); 2876 return position.v; 2877 } 2878 2879 double GET_EXTERNAL_POSITION_W(void) 2880 { 2881 CANON_POSITION position; 2882 position = GET_EXTERNAL_POSITION(); 2883 return position.w; 2884 } 2885 2886 double GET_EXTERNAL_PROBE_POSITION_X(void) 2887 { 2888 CANON_POSITION position; 2889 position = GET_EXTERNAL_PROBE_POSITION(); 2890 return position.x; 2891 } 2892 2893 double GET_EXTERNAL_PROBE_POSITION_Y(void) 2894 { 2895 CANON_POSITION position; 2896 position = GET_EXTERNAL_PROBE_POSITION(); 2897 return position.y; 2898 } 2899 2900 double GET_EXTERNAL_PROBE_POSITION_Z(void) 2901 { 2902 CANON_POSITION position; 2903 position = GET_EXTERNAL_PROBE_POSITION(); 2904 return position.z; 2905 } 2906 2907 double GET_EXTERNAL_PROBE_POSITION_A(void) 2908 { 2909 CANON_POSITION position; 2910 position = GET_EXTERNAL_PROBE_POSITION(); 2911 return position.a; 2912 } 2913 2914 double GET_EXTERNAL_PROBE_POSITION_B(void) 2915 { 2916 CANON_POSITION position; 2917 position = GET_EXTERNAL_PROBE_POSITION(); 2918 return position.b; 2919 } 2920 2921 double GET_EXTERNAL_PROBE_POSITION_C(void) 2922 { 2923 CANON_POSITION position; 2924 position = GET_EXTERNAL_PROBE_POSITION(); 2925 return position.c; 2926 } 2927 2928 double GET_EXTERNAL_PROBE_POSITION_U(void) 2929 { 2930 CANON_POSITION position; 2931 position = GET_EXTERNAL_PROBE_POSITION(); 2932 return position.u; 2933 } 2934 2935 double GET_EXTERNAL_PROBE_POSITION_V(void) 2936 { 2937 CANON_POSITION position; 2938 position = GET_EXTERNAL_PROBE_POSITION(); 2939 return position.v; 2940 } 2941 2942 double GET_EXTERNAL_PROBE_POSITION_W(void) 2943 { 2944 CANON_POSITION position; 2945 position = GET_EXTERNAL_PROBE_POSITION(); 2946 return position.w; 2947 } 2948 2949 CANON_MOTION_MODE GET_EXTERNAL_MOTION_CONTROL_MODE() 2950 { 2951 return canon.motionMode; 2952 } 2953 2954 double GET_EXTERNAL_MOTION_CONTROL_TOLERANCE() 2955 { 2956 return TO_PROG_LEN(canon.motionTolerance); 2957 } 2958 2959 2960 CANON_UNITS GET_EXTERNAL_LENGTH_UNIT_TYPE() 2961 { 2962 return canon.lengthUnits; 2963 } 2964 2965 int GET_EXTERNAL_QUEUE_EMPTY(void) 2966 { 2967 flush_segments(); 2968 2969 return emcStatus->motion.traj.queue == 0 ? 1 : 0; 2970 } 2971 2972 // Returns the "home pocket" of the tool currently in the spindle, ie the 2973 // pocket that the current tool was loaded from. Returns 0 if there is no 2974 // tool in the spindle. 2975 int GET_EXTERNAL_TOOL_SLOT() 2976 { 2977 int toolno = emcStatus->io.tool.toolInSpindle; 2978 int pocket; 2979 2980 for (pocket = 1; pocket < CANON_POCKETS_MAX; pocket++) { 2981 if (emcStatus->io.tool.toolTable[pocket].toolno == toolno) { 2982 return pocket; 2983 } 2984 } 2985 2986 return 0; // no tool in spindle 2987 } 2988 2989 // If the tool changer has prepped a pocket (after a Txxx command) and is 2990 // ready to perform a tool change, return the currently prepped pocket 2991 // number. If the tool changer is idle (because no Txxx command has been 2992 // run, or because an M6 tool change has completed), return -1. 2993 int GET_EXTERNAL_SELECTED_TOOL_SLOT() 2994 { 2995 return emcStatus->io.tool.pocketPrepped; 2996 } 2997 2998 int GET_EXTERNAL_TC_FAULT() 2999 { 3000 return emcStatus->io.fault; 3001 } 3002 3003 int GET_EXTERNAL_TC_REASON() 3004 { 3005 return emcStatus->io.reason; 3006 } 3007 3008 int GET_EXTERNAL_FEED_OVERRIDE_ENABLE() 3009 { 3010 return emcStatus->motion.traj.feed_override_enabled; 3011 } 3012 3013 int GET_EXTERNAL_SPINDLE_OVERRIDE_ENABLE(int spindle) 3014 { 3015 return emcStatus->motion.spindle[spindle].spindle_override_enabled; 3016 } 3017 3018 int GET_EXTERNAL_ADAPTIVE_FEED_ENABLE() 3019 { 3020 return emcStatus->motion.traj.adaptive_feed_enabled; 3021 } 3022 3023 int GET_EXTERNAL_FEED_HOLD_ENABLE() 3024 { 3025 return emcStatus->motion.traj.feed_hold_enabled; 3026 } 3027 3028 int GET_EXTERNAL_AXIS_MASK() { 3029 return emcStatus->motion.traj.axis_mask; 3030 } 3031 3032 int GET_EXTERNAL_OFFSET_APPLIED(void) { 3033 return emcGetExternalOffsetApplied(); 3034 } 3035 3036 EmcPose GET_EXTERNAL_OFFSETS() { 3037 return emcGetExternalOffsets(); 3038 } 3039 3040 CANON_PLANE GET_EXTERNAL_PLANE() 3041 { 3042 return canon.activePlane; 3043 } 3044 3045 /* returns current value of the digital input selected by index.*/ 3046 int GET_EXTERNAL_DIGITAL_INPUT(int index, int def) 3047 { 3048 if ((index < 0) || (index >= EMCMOT_MAX_DIO)) 3049 return -1; 3050 3051 if (emcStatus->task.input_timeout == 1) 3052 return -1; 3053 3054 #ifdef INPUT_DEBUG 3055 printf("GET_EXTERNAL_DIGITAL_INPUT called\n di[%d]=%d \n timeout=%d \n",index,emcStatus->motion.synch_di[index],emcStatus->task.input_timeout); 3056 #endif 3057 return (emcStatus->motion.synch_di[index] != 0) ? 1 : 0; 3058 } 3059 3060 double GET_EXTERNAL_ANALOG_INPUT(int index, double def) 3061 { 3062 /* returns current value of the analog input selected by index.*/ 3063 #ifdef INPUT_DEBUG 3064 printf("GET_EXTERNAL_ANALOG_INPUT called\n ai[%d]=%g \n timeout=%d \n",index,emcStatus->motion.analog_input[index],emcStatus->task.input_timeout); 3065 #endif 3066 if ((index < 0) || (index >= EMCMOT_MAX_AIO)) 3067 return -1; 3068 3069 if (emcStatus->task.input_timeout == 1) 3070 return -1; 3071 3072 return emcStatus->motion.analog_input[index]; 3073 } 3074 3075 3076 USER_DEFINED_FUNCTION_TYPE USER_DEFINED_FUNCTION[USER_DEFINED_FUNCTION_NUM] 3077 = { 0 }; 3078 3079 int USER_DEFINED_FUNCTION_ADD(USER_DEFINED_FUNCTION_TYPE func, int num) 3080 { 3081 if (num < 0 || num >= USER_DEFINED_FUNCTION_NUM) { 3082 return -1; 3083 } 3084 3085 USER_DEFINED_FUNCTION[num] = func; 3086 3087 return 0; 3088 } 3089 3090 /*! \function SET_MOTION_OUTPUT_BIT 3091 3092 sets a DIO pin 3093 this message goes to task, then to motion which sets the DIO 3094 when the first motion starts. 3095 The pin gets set with value 1 at the begin of motion, and stays 1 at the end of motion 3096 (this behaviour can be changed if needed) 3097 3098 warning: setting more then one for a motion segment will clear out the previous ones 3099 (the TP doesn't implement a queue of these), 3100 use SET_AUX_OUTPUT_BIT instead, that allows to set the value right away 3101 */ 3102 void SET_MOTION_OUTPUT_BIT(int index) 3103 { 3104 EMC_MOTION_SET_DOUT dout_msg; 3105 3106 flush_segments(); 3107 3108 dout_msg.index = index; 3109 dout_msg.start = 1; // startvalue = 1 3110 dout_msg.end = 1; // endvalue = 1, means it doesn't get reset after current motion 3111 dout_msg.now = 0; // not immediate, but synched with motion (goes to the TP) 3112 3113 interp_list.append(dout_msg); 3114 3115 return; 3116 } 3117 3118 /*! \function CLEAR_MOTION_OUTPUT_BIT 3119 3120 clears a DIO pin 3121 this message goes to task, then to motion which clears the DIO 3122 when the first motion starts. 3123 The pin gets set with value 0 at the begin of motion, and stays 0 at the end of motion 3124 (this behaviour can be changed if needed) 3125 3126 warning: setting more then one for a motion segment will clear out the previous ones 3127 (the TP doesn't implement a queue of these), 3128 use CLEAR_AUX_OUTPUT_BIT instead, that allows to set the value right away 3129 */ 3130 void CLEAR_MOTION_OUTPUT_BIT(int index) 3131 { 3132 EMC_MOTION_SET_DOUT dout_msg; 3133 3134 flush_segments(); 3135 3136 dout_msg.index = index; 3137 dout_msg.start = 0; // startvalue = 1 3138 dout_msg.end = 0; // endvalue = 0, means it stays 0 after current motion 3139 dout_msg.now = 0; // not immediate, but synched with motion (goes to the TP) 3140 3141 interp_list.append(dout_msg); 3142 3143 return; 3144 } 3145 3146 /*! \function SET_AUX_OUTPUT_BIT 3147 3148 sets a DIO pin 3149 this message goes to task, then to motion which sets the DIO 3150 right away. 3151 The pin gets set with value 1 at the begin of motion, and stays 1 at the end of motion 3152 (this behaviour can be changed if needed) 3153 you can use any number of these, as the effect is imediate 3154 */ 3155 void SET_AUX_OUTPUT_BIT(int index) 3156 { 3157 3158 EMC_MOTION_SET_DOUT dout_msg; 3159 3160 flush_segments(); 3161 3162 dout_msg.index = index; 3163 dout_msg.start = 1; // startvalue = 1 3164 dout_msg.end = 1; // endvalue = 1, means it doesn't get reset after current motion 3165 dout_msg.now = 1; // immediate, we don't care about synching for AUX 3166 3167 interp_list.append(dout_msg); 3168 3169 return; 3170 } 3171 3172 /*! \function CLEAR_AUX_OUTPUT_BIT 3173 3174 clears a DIO pin 3175 this message goes to task, then to motion which clears the DIO 3176 right away. 3177 The pin gets set with value 0 at the begin of motion, and stays 0 at the end of motion 3178 (this behaviour can be changed if needed) 3179 you can use any number of these, as the effect is imediate 3180 */ 3181 void CLEAR_AUX_OUTPUT_BIT(int index) 3182 { 3183 EMC_MOTION_SET_DOUT dout_msg; 3184 3185 flush_segments(); 3186 3187 dout_msg.index = index; 3188 dout_msg.start = 0; // startvalue = 1 3189 dout_msg.end = 0; // endvalue = 0, means it stays 0 after current motion 3190 dout_msg.now = 1; // immediate, we don't care about synching for AUX 3191 3192 interp_list.append(dout_msg); 3193 3194 return; 3195 } 3196 3197 /*! \function SET_MOTION_OUTPUT_VALUE 3198 3199 sets a AIO value, not used by the RS274 Interp, 3200 not fully implemented in the motion controller either 3201 */ 3202 void SET_MOTION_OUTPUT_VALUE(int index, double value) 3203 { 3204 EMC_MOTION_SET_AOUT aout_msg; 3205 3206 flush_segments(); 3207 3208 aout_msg.index = index; // which output 3209 aout_msg.start = value; // start value 3210 aout_msg.end = value; // end value 3211 aout_msg.now = 0; // immediate=1, or synched when motion start=0 3212 3213 interp_list.append(aout_msg); 3214 3215 return; 3216 } 3217 3218 /*! \function SET_AUX_OUTPUT_VALUE 3219 3220 sets a AIO value, not used by the RS274 Interp, 3221 not fully implemented in the motion controller either 3222 */ 3223 void SET_AUX_OUTPUT_VALUE(int index, double value) 3224 { 3225 EMC_MOTION_SET_AOUT aout_msg; 3226 3227 flush_segments(); 3228 3229 aout_msg.index = index; // which output 3230 aout_msg.start = value; // start value 3231 aout_msg.end = value; // end value 3232 aout_msg.now = 1; // immediate=1, or synched when motion start=0 3233 3234 interp_list.append(aout_msg); 3235 3236 return; 3237 } 3238 3239 /*! \function WAIT 3240 program execution and interpreting is stopped until the input selected by 3241 index changed to the needed state (specified by wait_type). 3242 Return value: either wait_type if timeout didn't occur, or -1 otherwise. */ 3243 3244 int WAIT(int index, /* index of the motion exported input */ 3245 int input_type, /*DIGITAL_INPUT or ANALOG_INPUT */ 3246 int wait_type, /* 0 - immediate, 1 - rise, 2 - fall, 3 - be high, 4 - be low */ 3247 double timeout) /* time to wait [in seconds], if the input didn't change the value -1 is returned */ 3248 { 3249 if (input_type == DIGITAL_INPUT) { 3250 if ((index < 0) || (index >= EMCMOT_MAX_DIO)) 3251 return -1; 3252 } else if (input_type == ANALOG_INPUT) { 3253 if ((index < 0) || (index >= EMCMOT_MAX_AIO)) 3254 return -1; 3255 } 3256 3257 EMC_AUX_INPUT_WAIT wait_msg; 3258 3259 flush_segments(); 3260 3261 wait_msg.index = index; 3262 wait_msg.input_type = input_type; 3263 wait_msg.wait_type = wait_type; 3264 wait_msg.timeout = timeout; 3265 3266 interp_list.append(wait_msg); 3267 return 0; 3268 } 3269 3270 int UNLOCK_ROTARY(int line_number, int joint_num) { 3271 EMC_TRAJ_LINEAR_MOVE m; 3272 // first, set up a zero length move to interrupt blending and get to final position 3273 m.type = EMC_MOTION_TYPE_TRAVERSE; 3274 m.feed_mode = 0; 3275 m.end = to_ext_pose(canon.endPoint.x, canon.endPoint.y, canon.endPoint.z, 3276 canon.endPoint.a, canon.endPoint.b, canon.endPoint.c, 3277 canon.endPoint.u, canon.endPoint.v, canon.endPoint.w); 3278 m.vel = m.acc = 1; // nonzero but otherwise doesn't matter 3279 m.indexer_jnum = -1; 3280 3281 // issue it 3282 int old_feed_mode = canon.feed_mode; 3283 if(canon.feed_mode) 3284 STOP_SPEED_FEED_SYNCH(); 3285 interp_list.set_line_number(line_number); 3286 interp_list.append(m); 3287 // no need to update endpoint 3288 if(old_feed_mode) 3289 START_SPEED_FEED_SYNCH(canon.spindle_num, canon.linearFeedRate, 1); 3290 3291 // now, the next move is the real indexing move, so be ready 3292 canon.rotary_unlock_for_traverse = joint_num; 3293 return 0; 3294 } 3295 3296 int LOCK_ROTARY(int line_number, int joint_num) { 3297 canon.rotary_unlock_for_traverse = -1; 3298 return 0; 3299 } 3300 3301 /* PLUGIN_CALL queues a Python tuple for execution by task 3302 * the tuple is expected to be already pickled 3303 * The tuple format is: (callable,tupleargs,keywordargs) 3304 */ 3305 void PLUGIN_CALL(int len, const char *call) 3306 { 3307 EMC_EXEC_PLUGIN_CALL call_msg; 3308 if (len > (int) sizeof(call_msg.call)) { 3309 // really should call it quits here, this is going to fail 3310 printf("PLUGIN_CALL: message size exceeded actual=%d max=%zd\n",len,sizeof(call_msg.call)); 3311 } 3312 memset(call_msg.call, 0, sizeof(call_msg.call)); 3313 memcpy(call_msg.call, call, len > (int) sizeof(call_msg.call) ? sizeof(call_msg.call) : len); 3314 call_msg.len = len; 3315 3316 printf("canon: PLUGIN_CALL(arglen=%zd)\n",strlen(call)); 3317 3318 interp_list.append(call_msg); 3319 } 3320 3321 void IO_PLUGIN_CALL(int len, const char *call) 3322 { 3323 EMC_IO_PLUGIN_CALL call_msg; 3324 if (len > (int) sizeof(call_msg.call)) { 3325 // really should call it quits here, this is going to fail 3326 printf("IO_PLUGIN_CALL: message size exceeded actual=%d max=%zd\n",len,sizeof(call_msg.call)); 3327 } 3328 memset(call_msg.call, 0, sizeof(call_msg.call)); 3329 memcpy(call_msg.call, call, len > (int) sizeof(call_msg.call) ? sizeof(call_msg.call) : len); 3330 call_msg.len = len; 3331 3332 printf("canon: IO_PLUGIN_CALL(arglen=%d)\n",len); 3333 3334 interp_list.append(call_msg); 3335 }