shcom.cc
1 /******************************************************************** 2 * Description: shcom.cc 3 * Common functions for NML calls 4 * 5 * Derived from a work by Fred Proctor & Will Shackleford 6 * Further derived from work by jmkasunich, Alex Joni 7 * 8 * Author: Eric H. Johnson 9 * License: GPL Version 2 10 * System: Linux 11 * 12 * Copyright (c) 2006 All rights reserved. 13 * 14 * Last change: 15 ********************************************************************/ 16 17 18 #define __STDC_FORMAT_MACROS 19 #include <stdio.h> 20 #include <string.h> 21 #include <stdlib.h> 22 #include <signal.h> 23 #include <unistd.h> 24 #include <ctype.h> 25 #include <math.h> 26 #include <sys/types.h> 27 #include <inttypes.h> 28 29 #include "rcs.hh" 30 #include "posemath.h" // PM_POSE, TO_RAD 31 #include "emc.hh" // EMC NML 32 #include "emc_nml.hh" 33 #include "canon.hh" // CANON_UNITS, CANON_UNITS_INCHES,MM,CM 34 #include "emcglb.h" // EMC_NMLFILE, TRAJ_MAX_VELOCITY, etc. 35 #include "emccfg.h" // DEFAULT_TRAJ_MAX_VELOCITY 36 #include "inifile.hh" // INIFILE 37 #include "nml_oi.hh" // nmlErrorFormat, NML_ERROR, etc 38 #include "rcs_print.hh" 39 #include "timer.hh" // esleep 40 #include "shcom.hh" // Common NML communications functions 41 42 LINEAR_UNIT_CONVERSION linearUnitConversion; 43 ANGULAR_UNIT_CONVERSION angularUnitConversion; 44 45 static int num_joints = EMCMOT_MAX_JOINTS; 46 47 int emcCommandSerialNumber; 48 49 // the NML channels to the EMC task 50 RCS_CMD_CHANNEL *emcCommandBuffer; 51 RCS_STAT_CHANNEL *emcStatusBuffer; 52 EMC_STAT *emcStatus; 53 54 // the NML channel for errors 55 NML *emcErrorBuffer; 56 char error_string[NML_ERROR_LEN]; 57 char operator_text_string[NML_TEXT_LEN]; 58 char operator_display_string[NML_DISPLAY_LEN]; 59 char defaultPath[80] = DEFAULT_PATH; 60 // default value for timeout, 0 means wait forever 61 double emcTimeout; 62 int programStartLine; 63 64 EMC_UPDATE_TYPE emcUpdateType; 65 EMC_WAIT_TYPE emcWaitType; 66 67 void strupr(char *s) 68 { 69 int i; 70 71 for (i = 0; i < (int)strlen(s); i++) 72 if (s[i] > 96 && s[i] <= 'z') 73 s[i] -= 32; 74 } 75 76 int emcTaskNmlGet() 77 { 78 int retval = 0; 79 80 // try to connect to EMC cmd 81 if (emcCommandBuffer == 0) { 82 emcCommandBuffer = 83 new RCS_CMD_CHANNEL(emcFormat, "emcCommand", "xemc", 84 emc_nmlfile); 85 if (!emcCommandBuffer->valid()) { 86 delete emcCommandBuffer; 87 emcCommandBuffer = 0; 88 retval = -1; 89 } 90 } 91 // try to connect to EMC status 92 if (emcStatusBuffer == 0) { 93 emcStatusBuffer = 94 new RCS_STAT_CHANNEL(emcFormat, "emcStatus", "xemc", 95 emc_nmlfile); 96 if (!emcStatusBuffer->valid() 97 || EMC_STAT_TYPE != emcStatusBuffer->peek()) { 98 delete emcStatusBuffer; 99 emcStatusBuffer = 0; 100 emcStatus = 0; 101 retval = -1; 102 } else { 103 emcStatus = (EMC_STAT *) emcStatusBuffer->get_address(); 104 } 105 } 106 107 return retval; 108 } 109 110 int emcErrorNmlGet() 111 { 112 int retval = 0; 113 114 if (emcErrorBuffer == 0) { 115 emcErrorBuffer = 116 new NML(nmlErrorFormat, "emcError", "xemc", emc_nmlfile); 117 if (!emcErrorBuffer->valid()) { 118 delete emcErrorBuffer; 119 emcErrorBuffer = 0; 120 retval = -1; 121 } 122 } 123 124 return retval; 125 } 126 127 int tryNml(double retry_time, double retry_interval) 128 { 129 double end; 130 int good; 131 132 if ((emc_debug & EMC_DEBUG_NML) == 0) { 133 set_rcs_print_destination(RCS_PRINT_TO_NULL); // inhibit diag 134 // messages 135 } 136 end = retry_time; 137 good = 0; 138 do { 139 if (0 == emcTaskNmlGet()) { 140 good = 1; 141 break; 142 } 143 esleep(retry_interval); 144 end -= retry_interval; 145 } while (end > 0.0); 146 if ((emc_debug & EMC_DEBUG_NML) == 0) { 147 set_rcs_print_destination(RCS_PRINT_TO_STDOUT); // inhibit diag 148 // messages 149 } 150 if (!good) { 151 return -1; 152 } 153 154 if ((emc_debug & EMC_DEBUG_NML) == 0) { 155 set_rcs_print_destination(RCS_PRINT_TO_NULL); // inhibit diag 156 // messages 157 } 158 end = retry_time; 159 good = 0; 160 do { 161 if (0 == emcErrorNmlGet()) { 162 good = 1; 163 break; 164 } 165 esleep(retry_interval); 166 end -= retry_interval; 167 } while (end > 0.0); 168 if ((emc_debug & EMC_DEBUG_NML) == 0) { 169 set_rcs_print_destination(RCS_PRINT_TO_STDOUT); // inhibit diag 170 // messages 171 } 172 if (!good) { 173 return -1; 174 } 175 176 return 0; 177 } 178 179 int updateStatus() 180 { 181 NMLTYPE type; 182 183 if (0 == emcStatus || 0 == emcStatusBuffer 184 || !emcStatusBuffer->valid()) { 185 return -1; 186 } 187 188 switch (type = emcStatusBuffer->peek()) { 189 case -1: 190 // error on CMS channel 191 return -1; 192 break; 193 194 case 0: // no new data 195 case EMC_STAT_TYPE: // new data 196 // new data 197 break; 198 199 default: 200 return -1; 201 break; 202 } 203 204 return 0; 205 } 206 207 /* 208 updateError() updates "errors," which are true errors and also 209 operator display and text messages. 210 */ 211 int updateError() 212 { 213 NMLTYPE type; 214 215 if (0 == emcErrorBuffer || !emcErrorBuffer->valid()) { 216 return -1; 217 } 218 219 switch (type = emcErrorBuffer->read()) { 220 case -1: 221 // error reading channel 222 return -1; 223 break; 224 225 case 0: 226 // nothing new 227 break; 228 229 case EMC_OPERATOR_ERROR_TYPE: 230 strncpy(error_string, 231 ((EMC_OPERATOR_ERROR *) (emcErrorBuffer->get_address()))-> 232 error, LINELEN - 1); 233 error_string[NML_ERROR_LEN - 1] = 0; 234 break; 235 236 case EMC_OPERATOR_TEXT_TYPE: 237 strncpy(operator_text_string, 238 ((EMC_OPERATOR_TEXT *) (emcErrorBuffer->get_address()))-> 239 text, LINELEN - 1); 240 operator_text_string[NML_TEXT_LEN - 1] = 0; 241 break; 242 243 case EMC_OPERATOR_DISPLAY_TYPE: 244 strncpy(operator_display_string, 245 ((EMC_OPERATOR_DISPLAY *) (emcErrorBuffer-> 246 get_address()))->display, 247 LINELEN - 1); 248 operator_display_string[NML_DISPLAY_LEN - 1] = 0; 249 break; 250 251 case NML_ERROR_TYPE: 252 strncpy(error_string, 253 ((NML_ERROR *) (emcErrorBuffer->get_address()))->error, 254 NML_ERROR_LEN - 1); 255 error_string[NML_ERROR_LEN - 1] = 0; 256 break; 257 258 case NML_TEXT_TYPE: 259 strncpy(operator_text_string, 260 ((NML_TEXT *) (emcErrorBuffer->get_address()))->text, 261 NML_TEXT_LEN - 1); 262 operator_text_string[NML_TEXT_LEN - 1] = 0; 263 break; 264 265 case NML_DISPLAY_TYPE: 266 strncpy(operator_display_string, 267 ((NML_DISPLAY *) (emcErrorBuffer->get_address()))->display, 268 NML_DISPLAY_LEN - 1); 269 operator_display_string[NML_DISPLAY_LEN - 1] = 0; 270 break; 271 272 default: 273 // if not recognized, set the error string 274 sprintf(error_string, "unrecognized error %" PRId32, type); 275 return -1; 276 break; 277 } 278 279 return 0; 280 } 281 282 #define EMC_COMMAND_DELAY 0.1 // how long to sleep between checks 283 284 int emcCommandWaitDone() 285 { 286 double end; 287 for (end = 0.0; emcTimeout <= 0.0 || end < emcTimeout; end += EMC_COMMAND_DELAY) { 288 updateStatus(); 289 int serial_diff = emcStatus->echo_serial_number - emcCommandSerialNumber; 290 if (serial_diff < 0) { 291 continue; 292 } 293 294 if (serial_diff > 0) { 295 return 0; 296 } 297 298 if (emcStatus->status == RCS_DONE) { 299 return 0; 300 } 301 302 if (emcStatus->status == RCS_ERROR) { 303 return -1; 304 } 305 306 esleep(EMC_COMMAND_DELAY); 307 } 308 309 return -1; 310 } 311 312 int emcCommandWaitReceived() 313 { 314 double end; 315 for (end = 0.0; emcTimeout <= 0.0 || end < emcTimeout; end += EMC_COMMAND_DELAY) { 316 updateStatus(); 317 318 int serial_diff = emcStatus->echo_serial_number - emcCommandSerialNumber; 319 if (serial_diff >= 0) { 320 return 0; 321 } 322 323 esleep(EMC_COMMAND_DELAY); 324 } 325 326 return -1; 327 } 328 329 int emcCommandSend(RCS_CMD_MSG & cmd) 330 { 331 // write command 332 if (emcCommandBuffer->write(&cmd)) { 333 return -1; 334 } 335 emcCommandSerialNumber = cmd.serial_number; 336 return 0; 337 } 338 339 340 /* 341 Unit conversion 342 343 Length and angle units in the EMC status buffer are in user units, as 344 defined in the INI file in [TRAJ] LINEAR,ANGULAR_UNITS. These may differ 345 from the program units, and when they are the display is confusing. 346 347 It may be desirable to synchronize the display units with the program 348 units automatically, and also to break this sync and allow independent 349 display of position values. 350 351 The global variable "linearUnitConversion" is set by the Tcl commands 352 emc_linear_unit_conversion to correspond to either "inch", 353 "mm", "cm", "auto", or "custom". This forces numbers to be returned in the 354 units specified, in program units when "auto" is set, or not converted 355 at all if "custom" is specified. 356 357 Ditto for "angularUnitConversion", set by emc_angular_unit_conversion 358 to "deg", "rad", "grad", "auto", or "custom". 359 360 With no args, emc_linear/angular_unit_conversion return the setting. 361 362 The functions convertLinearUnits and convertAngularUnits take a length 363 or angle value, typically from the emcStatus structure, and convert it 364 as indicated by linearUnitConversion and angularUnitConversion, resp. 365 */ 366 367 368 /* 369 to convert linear units, values are converted to mm, then to desired 370 units 371 */ 372 double convertLinearUnits(double u) 373 { 374 double in_mm; 375 376 /* convert u to mm */ 377 in_mm = u / emcStatus->motion.traj.linearUnits; 378 379 /* convert u to display units */ 380 switch (linearUnitConversion) { 381 case LINEAR_UNITS_MM: 382 return in_mm; 383 break; 384 case LINEAR_UNITS_INCH: 385 return in_mm * INCH_PER_MM; 386 break; 387 case LINEAR_UNITS_CM: 388 return in_mm * CM_PER_MM; 389 break; 390 case LINEAR_UNITS_AUTO: 391 switch (emcStatus->task.programUnits) { 392 case CANON_UNITS_MM: 393 return in_mm; 394 break; 395 case CANON_UNITS_INCHES: 396 return in_mm * INCH_PER_MM; 397 break; 398 case CANON_UNITS_CM: 399 return in_mm * CM_PER_MM; 400 break; 401 } 402 break; 403 404 case LINEAR_UNITS_CUSTOM: 405 return u; 406 break; 407 } 408 409 // If it ever gets here we have an error. 410 411 return u; 412 } 413 414 double convertAngularUnits(double u) 415 { 416 // Angular units are always degrees 417 return u; 418 } 419 420 // polarities for joint jogging, from ini file 421 static int jogPol[EMCMOT_MAX_JOINTS]; 422 423 int sendDebug(int level) 424 { 425 EMC_SET_DEBUG debug_msg; 426 427 debug_msg.debug = level; 428 emcCommandSend(debug_msg); 429 if (emcWaitType == EMC_WAIT_RECEIVED) { 430 return emcCommandWaitReceived(); 431 } else if (emcWaitType == EMC_WAIT_DONE) { 432 return emcCommandWaitDone(); 433 } 434 435 return 0; 436 } 437 438 int sendEstop() 439 { 440 EMC_TASK_SET_STATE state_msg; 441 442 state_msg.state = EMC_TASK_STATE_ESTOP; 443 emcCommandSend(state_msg); 444 if (emcWaitType == EMC_WAIT_RECEIVED) { 445 return emcCommandWaitReceived(); 446 } else if (emcWaitType == EMC_WAIT_DONE) { 447 return emcCommandWaitDone(); 448 } 449 450 return 0; 451 } 452 453 int sendEstopReset() 454 { 455 EMC_TASK_SET_STATE state_msg; 456 457 state_msg.state = EMC_TASK_STATE_ESTOP_RESET; 458 emcCommandSend(state_msg); 459 if (emcWaitType == EMC_WAIT_RECEIVED) { 460 return emcCommandWaitReceived(); 461 } else if (emcWaitType == EMC_WAIT_DONE) { 462 return emcCommandWaitDone(); 463 } 464 465 return 0; 466 } 467 468 int sendMachineOn() 469 { 470 EMC_TASK_SET_STATE state_msg; 471 472 state_msg.state = EMC_TASK_STATE_ON; 473 emcCommandSend(state_msg); 474 if (emcWaitType == EMC_WAIT_RECEIVED) { 475 return emcCommandWaitReceived(); 476 } else if (emcWaitType == EMC_WAIT_DONE) { 477 return emcCommandWaitDone(); 478 } 479 480 return 0; 481 } 482 483 int sendMachineOff() 484 { 485 EMC_TASK_SET_STATE state_msg; 486 487 state_msg.state = EMC_TASK_STATE_OFF; 488 emcCommandSend(state_msg); 489 if (emcWaitType == EMC_WAIT_RECEIVED) { 490 return emcCommandWaitReceived(); 491 } else if (emcWaitType == EMC_WAIT_DONE) { 492 return emcCommandWaitDone(); 493 } 494 495 return 0; 496 } 497 498 int sendManual() 499 { 500 EMC_TASK_SET_MODE mode_msg; 501 502 mode_msg.mode = EMC_TASK_MODE_MANUAL; 503 emcCommandSend(mode_msg); 504 if (emcWaitType == EMC_WAIT_RECEIVED) { 505 return emcCommandWaitReceived(); 506 } else if (emcWaitType == EMC_WAIT_DONE) { 507 return emcCommandWaitDone(); 508 } 509 510 return 0; 511 } 512 513 int sendAuto() 514 { 515 EMC_TASK_SET_MODE mode_msg; 516 517 mode_msg.mode = EMC_TASK_MODE_AUTO; 518 emcCommandSend(mode_msg); 519 if (emcWaitType == EMC_WAIT_RECEIVED) { 520 return emcCommandWaitReceived(); 521 } else if (emcWaitType == EMC_WAIT_DONE) { 522 return emcCommandWaitDone(); 523 } 524 525 return 0; 526 } 527 528 int sendMdi() 529 { 530 EMC_TASK_SET_MODE mode_msg; 531 532 mode_msg.mode = EMC_TASK_MODE_MDI; 533 emcCommandSend(mode_msg); 534 if (emcWaitType == EMC_WAIT_RECEIVED) { 535 return emcCommandWaitReceived(); 536 } else if (emcWaitType == EMC_WAIT_DONE) { 537 return emcCommandWaitDone(); 538 } 539 540 return 0; 541 } 542 543 int sendOverrideLimits(int joint) 544 { 545 EMC_JOINT_OVERRIDE_LIMITS lim_msg; 546 547 lim_msg.joint = joint; // neg means off, else on for all 548 emcCommandSend(lim_msg); 549 if (emcWaitType == EMC_WAIT_RECEIVED) { 550 return emcCommandWaitReceived(); 551 } else if (emcWaitType == EMC_WAIT_DONE) { 552 return emcCommandWaitDone(); 553 } 554 555 return 0; 556 } 557 558 int sendJogStop(int ja, int jjogmode) 559 { 560 EMC_JOG_STOP emc_jog_stop_msg; 561 562 if ( ( (jjogmode == JOGJOINT) 563 && (emcStatus->motion.traj.mode == EMC_TRAJ_MODE_TELEOP) ) 564 || ( (jjogmode == JOGTELEOP ) 565 && (emcStatus->motion.traj.mode != EMC_TRAJ_MODE_TELEOP) ) 566 ) { 567 return -1; 568 } 569 570 if ( jjogmode && (ja < 0 || ja >= num_joints)) { 571 fprintf(stderr,"shcom.cc: unexpected_1 %d\n",ja); return -1; 572 } 573 if ( !jjogmode && (ja < 0)) { 574 fprintf(stderr,"shcom.cc: unexpected_2 %d\n",ja); return -1; 575 } 576 577 emc_jog_stop_msg.jjogmode = jjogmode; 578 emc_jog_stop_msg.joint_or_axis = ja; 579 emcCommandSend(emc_jog_stop_msg); 580 return 0; 581 } 582 583 int sendJogCont(int ja, int jjogmode, double speed) 584 { 585 EMC_JOG_CONT emc_jog_cont_msg; 586 587 if (emcStatus->task.state != EMC_TASK_STATE_ON) { return -1; } 588 if ( ( (jjogmode == JOGJOINT) 589 && (emcStatus->motion.traj.mode == EMC_TRAJ_MODE_TELEOP) ) 590 || ( (jjogmode == JOGTELEOP ) 591 && (emcStatus->motion.traj.mode != EMC_TRAJ_MODE_TELEOP) ) 592 ) { 593 return -1; 594 } 595 596 if ( jjogmode && (ja < 0 || ja >= num_joints)) { 597 fprintf(stderr,"shcom.cc: unexpected_3 %d\n",ja); return -1; 598 } 599 if ( !jjogmode && (ja < 0)) { 600 fprintf(stderr,"shcom.cc: unexpected_4 %d\n",ja); return -1; 601 } 602 603 emc_jog_cont_msg.jjogmode = jjogmode; 604 emc_jog_cont_msg.joint_or_axis = ja; 605 emc_jog_cont_msg.vel = speed / 60.0; 606 607 emcCommandSend(emc_jog_cont_msg); 608 609 return 0; 610 } 611 612 int sendJogIncr(int ja, int jjogmode, double speed, double incr) 613 { 614 EMC_JOG_INCR emc_jog_incr_msg; 615 616 if (emcStatus->task.state != EMC_TASK_STATE_ON) { return -1; } 617 if ( ( (jjogmode == JOGJOINT) 618 && ( emcStatus->motion.traj.mode == EMC_TRAJ_MODE_TELEOP) ) 619 || ( (jjogmode == JOGTELEOP ) 620 && ( emcStatus->motion.traj.mode != EMC_TRAJ_MODE_TELEOP) ) 621 ) { 622 return -1; 623 } 624 625 if ( jjogmode && (ja < 0 || ja >= num_joints)) { 626 fprintf(stderr,"shcom.cc: unexpected_5 %d\n",ja); return -1; 627 } 628 if ( !jjogmode && (ja < 0)) { 629 fprintf(stderr,"shcom.cc: unexpected_6 %d\n",ja); return -1; 630 } 631 632 emc_jog_incr_msg.jjogmode = jjogmode; 633 emc_jog_incr_msg.joint_or_axis = ja; 634 emc_jog_incr_msg.vel = speed / 60.0; 635 emc_jog_incr_msg.incr = incr; 636 637 emcCommandSend(emc_jog_incr_msg); 638 639 return 0; 640 } 641 642 int sendMistOn() 643 { 644 EMC_COOLANT_MIST_ON emc_coolant_mist_on_msg; 645 646 emcCommandSend(emc_coolant_mist_on_msg); 647 if (emcWaitType == EMC_WAIT_RECEIVED) { 648 return emcCommandWaitReceived(); 649 } else if (emcWaitType == EMC_WAIT_DONE) { 650 return emcCommandWaitDone(); 651 } 652 653 return 0; 654 } 655 656 int sendMistOff() 657 { 658 EMC_COOLANT_MIST_OFF emc_coolant_mist_off_msg; 659 660 emcCommandSend(emc_coolant_mist_off_msg); 661 if (emcWaitType == EMC_WAIT_RECEIVED) { 662 return emcCommandWaitReceived(); 663 } else if (emcWaitType == EMC_WAIT_DONE) { 664 return emcCommandWaitDone(); 665 } 666 667 return 0; 668 } 669 670 int sendFloodOn() 671 { 672 EMC_COOLANT_FLOOD_ON emc_coolant_flood_on_msg; 673 674 emcCommandSend(emc_coolant_flood_on_msg); 675 if (emcWaitType == EMC_WAIT_RECEIVED) { 676 return emcCommandWaitReceived(); 677 } else if (emcWaitType == EMC_WAIT_DONE) { 678 return emcCommandWaitDone(); 679 } 680 681 return 0; 682 } 683 684 int sendFloodOff() 685 { 686 EMC_COOLANT_FLOOD_OFF emc_coolant_flood_off_msg; 687 688 emcCommandSend(emc_coolant_flood_off_msg); 689 if (emcWaitType == EMC_WAIT_RECEIVED) { 690 return emcCommandWaitReceived(); 691 } else if (emcWaitType == EMC_WAIT_DONE) { 692 return emcCommandWaitDone(); 693 } 694 695 return 0; 696 } 697 698 int sendLubeOn() 699 { 700 EMC_LUBE_ON emc_lube_on_msg; 701 702 emcCommandSend(emc_lube_on_msg); 703 if (emcWaitType == EMC_WAIT_RECEIVED) { 704 return emcCommandWaitReceived(); 705 } else if (emcWaitType == EMC_WAIT_DONE) { 706 return emcCommandWaitDone(); 707 } 708 709 return 0; 710 } 711 712 int sendLubeOff() 713 { 714 EMC_LUBE_OFF emc_lube_off_msg; 715 716 emcCommandSend(emc_lube_off_msg); 717 if (emcWaitType == EMC_WAIT_RECEIVED) { 718 return emcCommandWaitReceived(); 719 } else if (emcWaitType == EMC_WAIT_DONE) { 720 return emcCommandWaitDone(); 721 } 722 723 return 0; 724 } 725 726 int sendSpindleForward(int spindle) 727 { 728 EMC_SPINDLE_ON emc_spindle_on_msg; 729 emc_spindle_on_msg.spindle = spindle; 730 if (emcStatus->task.activeSettings[2] != 0) { 731 emc_spindle_on_msg.speed = fabs(emcStatus->task.activeSettings[2]); 732 } else { 733 emc_spindle_on_msg.speed = +500; 734 } 735 emcCommandSend(emc_spindle_on_msg); 736 if (emcWaitType == EMC_WAIT_RECEIVED) { 737 return emcCommandWaitReceived(); 738 } else if (emcWaitType == EMC_WAIT_DONE) { 739 return emcCommandWaitDone(); 740 } 741 742 return 0; 743 } 744 745 int sendSpindleReverse(int spindle) 746 { 747 EMC_SPINDLE_ON emc_spindle_on_msg; 748 emc_spindle_on_msg.spindle = spindle; 749 if (emcStatus->task.activeSettings[2] != 0) { 750 emc_spindle_on_msg.speed = 751 -1 * fabs(emcStatus->task.activeSettings[2]); 752 } else { 753 emc_spindle_on_msg.speed = -500; 754 } 755 emcCommandSend(emc_spindle_on_msg); 756 if (emcWaitType == EMC_WAIT_RECEIVED) { 757 return emcCommandWaitReceived(); 758 } else if (emcWaitType == EMC_WAIT_DONE) { 759 return emcCommandWaitDone(); 760 } 761 762 return 0; 763 } 764 765 int sendSpindleOff(int spindle) 766 { 767 EMC_SPINDLE_OFF emc_spindle_off_msg; 768 emc_spindle_off_msg.spindle = spindle; 769 emcCommandSend(emc_spindle_off_msg); 770 if (emcWaitType == EMC_WAIT_RECEIVED) { 771 return emcCommandWaitReceived(); 772 } else if (emcWaitType == EMC_WAIT_DONE) { 773 return emcCommandWaitDone(); 774 } 775 776 return 0; 777 } 778 779 int sendSpindleIncrease(int spindle) 780 { 781 EMC_SPINDLE_INCREASE emc_spindle_increase_msg; 782 emc_spindle_increase_msg.spindle = spindle; 783 emcCommandSend(emc_spindle_increase_msg); 784 if (emcWaitType == EMC_WAIT_RECEIVED) { 785 return emcCommandWaitReceived(); 786 } else if (emcWaitType == EMC_WAIT_DONE) { 787 return emcCommandWaitDone(); 788 } 789 790 return 0; 791 } 792 793 int sendSpindleDecrease(int spindle) 794 { 795 EMC_SPINDLE_DECREASE emc_spindle_decrease_msg; 796 emc_spindle_decrease_msg.spindle = spindle; 797 emcCommandSend(emc_spindle_decrease_msg); 798 if (emcWaitType == EMC_WAIT_RECEIVED) { 799 return emcCommandWaitReceived(); 800 } else if (emcWaitType == EMC_WAIT_DONE) { 801 return emcCommandWaitDone(); 802 } 803 804 return 0; 805 } 806 807 int sendSpindleConstant(int spindle) 808 { 809 EMC_SPINDLE_CONSTANT emc_spindle_constant_msg; 810 emc_spindle_constant_msg.spindle = spindle; 811 emcCommandSend(emc_spindle_constant_msg); 812 if (emcWaitType == EMC_WAIT_RECEIVED) { 813 return emcCommandWaitReceived(); 814 } else if (emcWaitType == EMC_WAIT_DONE) { 815 return emcCommandWaitDone(); 816 } 817 818 return 0; 819 } 820 821 int sendBrakeEngage(int spindle) 822 { 823 EMC_SPINDLE_BRAKE_ENGAGE emc_spindle_brake_engage_msg; 824 825 emc_spindle_brake_engage_msg.spindle = spindle; 826 emcCommandSend(emc_spindle_brake_engage_msg); 827 if (emcWaitType == EMC_WAIT_RECEIVED) { 828 return emcCommandWaitReceived(); 829 } else if (emcWaitType == EMC_WAIT_DONE) { 830 return emcCommandWaitDone(); 831 } 832 833 return 0; 834 } 835 836 int sendBrakeRelease(int spindle) 837 { 838 EMC_SPINDLE_BRAKE_RELEASE emc_spindle_brake_release_msg; 839 840 emc_spindle_brake_release_msg.spindle = spindle; 841 emcCommandSend(emc_spindle_brake_release_msg); 842 if (emcWaitType == EMC_WAIT_RECEIVED) { 843 return emcCommandWaitReceived(); 844 } else if (emcWaitType == EMC_WAIT_DONE) { 845 return emcCommandWaitDone(); 846 } 847 848 return 0; 849 } 850 851 int sendAbort() 852 { 853 EMC_TASK_ABORT task_abort_msg; 854 855 emcCommandSend(task_abort_msg); 856 if (emcWaitType == EMC_WAIT_RECEIVED) { 857 return emcCommandWaitReceived(); 858 } else if (emcWaitType == EMC_WAIT_DONE) { 859 return emcCommandWaitDone(); 860 } 861 862 return 0; 863 } 864 865 int sendHome(int joint) 866 { 867 EMC_JOINT_HOME emc_joint_home_msg; 868 869 emc_joint_home_msg.joint = joint; 870 emcCommandSend(emc_joint_home_msg); 871 if (emcWaitType == EMC_WAIT_RECEIVED) { 872 return emcCommandWaitReceived(); 873 } else if (emcWaitType == EMC_WAIT_DONE) { 874 return emcCommandWaitDone(); 875 } 876 877 return 0; 878 } 879 880 int sendUnHome(int joint) 881 { 882 EMC_JOINT_UNHOME emc_joint_home_msg; 883 884 emc_joint_home_msg.joint = joint; 885 emcCommandSend(emc_joint_home_msg); 886 if (emcWaitType == EMC_WAIT_RECEIVED) { 887 return emcCommandWaitReceived(); 888 } else if (emcWaitType == EMC_WAIT_DONE) { 889 return emcCommandWaitDone(); 890 } 891 892 return 0; 893 } 894 895 int sendFeedOverride(double override) 896 { 897 EMC_TRAJ_SET_SCALE emc_traj_set_scale_msg; 898 899 if (override < 0.0) { 900 override = 0.0; 901 } 902 903 emc_traj_set_scale_msg.scale = override; 904 emcCommandSend(emc_traj_set_scale_msg); 905 if (emcWaitType == EMC_WAIT_RECEIVED) { 906 return emcCommandWaitReceived(); 907 } else if (emcWaitType == EMC_WAIT_DONE) { 908 return emcCommandWaitDone(); 909 } 910 911 return 0; 912 } 913 914 int sendRapidOverride(double override) 915 { 916 EMC_TRAJ_SET_RAPID_SCALE emc_traj_set_scale_msg; 917 918 if (override < 0.0) { 919 override = 0.0; 920 } 921 922 if (override > 1.0) { 923 override = 1.0; 924 } 925 926 emc_traj_set_scale_msg.scale = override; 927 emcCommandSend(emc_traj_set_scale_msg); 928 if (emcWaitType == EMC_WAIT_RECEIVED) { 929 return emcCommandWaitReceived(); 930 } else if (emcWaitType == EMC_WAIT_DONE) { 931 return emcCommandWaitDone(); 932 } 933 934 return 0; 935 } 936 937 938 int sendSpindleOverride(int spindle, double override) 939 { 940 EMC_TRAJ_SET_SPINDLE_SCALE emc_traj_set_spindle_scale_msg; 941 942 if (override < 0.0) { 943 override = 0.0; 944 } 945 946 emc_traj_set_spindle_scale_msg.spindle = spindle; 947 emc_traj_set_spindle_scale_msg.scale = override; 948 emcCommandSend(emc_traj_set_spindle_scale_msg); 949 if (emcWaitType == EMC_WAIT_RECEIVED) { 950 return emcCommandWaitReceived(); 951 } else if (emcWaitType == EMC_WAIT_DONE) { 952 return emcCommandWaitDone(); 953 } 954 955 return 0; 956 } 957 958 int sendTaskPlanInit() 959 { 960 EMC_TASK_PLAN_INIT task_plan_init_msg; 961 962 emcCommandSend(task_plan_init_msg); 963 if (emcWaitType == EMC_WAIT_RECEIVED) { 964 return emcCommandWaitReceived(); 965 } else if (emcWaitType == EMC_WAIT_DONE) { 966 return emcCommandWaitDone(); 967 } 968 969 return 0; 970 } 971 972 // saved value of last program opened 973 static char lastProgramFile[LINELEN] = ""; 974 975 int sendProgramOpen(char *program) 976 { 977 EMC_TASK_PLAN_OPEN emc_task_plan_open_msg; 978 979 // save this to run again 980 strcpy(lastProgramFile, program); 981 982 strcpy(emc_task_plan_open_msg.file, program); 983 emcCommandSend(emc_task_plan_open_msg); 984 if (emcWaitType == EMC_WAIT_RECEIVED) { 985 return emcCommandWaitReceived(); 986 } else if (emcWaitType == EMC_WAIT_DONE) { 987 return emcCommandWaitDone(); 988 } 989 990 return 0; 991 } 992 993 int sendProgramRun(int line) 994 { 995 EMC_TASK_PLAN_RUN emc_task_plan_run_msg; 996 997 if (emcUpdateType == EMC_UPDATE_AUTO) { 998 updateStatus(); 999 } 1000 // first reopen program if it's not open 1001 if (0 == emcStatus->task.file[0]) { 1002 // send a request to open last one 1003 sendProgramOpen(lastProgramFile); 1004 } 1005 // save the start line, to compare against active line later 1006 programStartLine = line; 1007 1008 emc_task_plan_run_msg.line = line; 1009 emcCommandSend(emc_task_plan_run_msg); 1010 if (emcWaitType == EMC_WAIT_RECEIVED) { 1011 return emcCommandWaitReceived(); 1012 } else if (emcWaitType == EMC_WAIT_DONE) { 1013 return emcCommandWaitDone(); 1014 } 1015 1016 return 0; 1017 } 1018 1019 int sendProgramPause() 1020 { 1021 EMC_TASK_PLAN_PAUSE emc_task_plan_pause_msg; 1022 1023 emcCommandSend(emc_task_plan_pause_msg); 1024 if (emcWaitType == EMC_WAIT_RECEIVED) { 1025 return emcCommandWaitReceived(); 1026 } else if (emcWaitType == EMC_WAIT_DONE) { 1027 return emcCommandWaitDone(); 1028 } 1029 1030 return 0; 1031 } 1032 1033 int sendProgramResume() 1034 { 1035 EMC_TASK_PLAN_RESUME emc_task_plan_resume_msg; 1036 1037 emcCommandSend(emc_task_plan_resume_msg); 1038 if (emcWaitType == EMC_WAIT_RECEIVED) { 1039 return emcCommandWaitReceived(); 1040 } else if (emcWaitType == EMC_WAIT_DONE) { 1041 return emcCommandWaitDone(); 1042 } 1043 1044 return 0; 1045 } 1046 1047 int sendSetOptionalStop(bool state) 1048 { 1049 EMC_TASK_PLAN_SET_OPTIONAL_STOP emc_task_plan_set_optional_stop_msg; 1050 1051 emc_task_plan_set_optional_stop_msg.state = state; 1052 emcCommandSend(emc_task_plan_set_optional_stop_msg); 1053 if (emcWaitType == EMC_WAIT_RECEIVED) { 1054 return emcCommandWaitReceived(); 1055 } else if (emcWaitType == EMC_WAIT_DONE) { 1056 return emcCommandWaitDone(); 1057 } 1058 1059 return 0; 1060 } 1061 1062 1063 int sendProgramStep() 1064 { 1065 EMC_TASK_PLAN_STEP emc_task_plan_step_msg; 1066 1067 // clear out start line, if we had a verify before it would be -1 1068 programStartLine = 0; 1069 1070 emcCommandSend(emc_task_plan_step_msg); 1071 if (emcWaitType == EMC_WAIT_RECEIVED) { 1072 return emcCommandWaitReceived(); 1073 } else if (emcWaitType == EMC_WAIT_DONE) { 1074 return emcCommandWaitDone(); 1075 } 1076 1077 return 0; 1078 } 1079 1080 int sendMdiCmd(const char *mdi) 1081 { 1082 EMC_TASK_PLAN_EXECUTE emc_task_plan_execute_msg; 1083 1084 strcpy(emc_task_plan_execute_msg.command, mdi); 1085 emcCommandSend(emc_task_plan_execute_msg); 1086 if (emcWaitType == EMC_WAIT_RECEIVED) { 1087 return emcCommandWaitReceived(); 1088 } else if (emcWaitType == EMC_WAIT_DONE) { 1089 return emcCommandWaitDone(); 1090 } 1091 1092 return 0; 1093 } 1094 1095 int sendLoadToolTable(const char *file) 1096 { 1097 EMC_TOOL_LOAD_TOOL_TABLE emc_tool_load_tool_table_msg; 1098 1099 strcpy(emc_tool_load_tool_table_msg.file, file); 1100 emcCommandSend(emc_tool_load_tool_table_msg); 1101 if (emcWaitType == EMC_WAIT_RECEIVED) { 1102 return emcCommandWaitReceived(); 1103 } else if (emcWaitType == EMC_WAIT_DONE) { 1104 return emcCommandWaitDone(); 1105 } 1106 1107 return 0; 1108 } 1109 1110 int sendToolSetOffset(int toolno, double zoffset, double diameter) 1111 { 1112 EMC_TOOL_SET_OFFSET emc_tool_set_offset_msg; 1113 1114 emc_tool_set_offset_msg.toolno = toolno; 1115 emc_tool_set_offset_msg.offset.tran.z = zoffset; 1116 emc_tool_set_offset_msg.diameter = diameter; 1117 emc_tool_set_offset_msg.orientation = 0; // mill style tool table 1118 1119 emcCommandSend(emc_tool_set_offset_msg); 1120 if (emcWaitType == EMC_WAIT_RECEIVED) { 1121 return emcCommandWaitReceived(); 1122 } else if (emcWaitType == EMC_WAIT_DONE) { 1123 return emcCommandWaitDone(); 1124 } 1125 1126 return 0; 1127 } 1128 1129 int sendToolSetOffset(int toolno, double zoffset, double xoffset, 1130 double diameter, double frontangle, double backangle, 1131 int orientation) 1132 { 1133 EMC_TOOL_SET_OFFSET emc_tool_set_offset_msg; 1134 1135 emc_tool_set_offset_msg.toolno = toolno; 1136 emc_tool_set_offset_msg.offset.tran.z = zoffset; 1137 emc_tool_set_offset_msg.offset.tran.x = xoffset; 1138 emc_tool_set_offset_msg.diameter = diameter; 1139 emc_tool_set_offset_msg.frontangle = frontangle; 1140 emc_tool_set_offset_msg.backangle = backangle; 1141 emc_tool_set_offset_msg.orientation = orientation; 1142 1143 emcCommandSend(emc_tool_set_offset_msg); 1144 if (emcWaitType == EMC_WAIT_RECEIVED) { 1145 return emcCommandWaitReceived(); 1146 } else if (emcWaitType == EMC_WAIT_DONE) { 1147 return emcCommandWaitDone(); 1148 } 1149 1150 return 0; 1151 } 1152 1153 int sendJointSetBacklash(int joint, double backlash) 1154 { 1155 EMC_JOINT_SET_BACKLASH emc_joint_set_backlash_msg; 1156 1157 emc_joint_set_backlash_msg.joint = joint; 1158 emc_joint_set_backlash_msg.backlash = backlash; 1159 emcCommandSend(emc_joint_set_backlash_msg); 1160 if (emcWaitType == EMC_WAIT_RECEIVED) { 1161 return emcCommandWaitReceived(); 1162 } else if (emcWaitType == EMC_WAIT_DONE) { 1163 return emcCommandWaitDone(); 1164 } 1165 1166 return 0; 1167 } 1168 1169 int sendJointEnable(int joint, int val) 1170 { 1171 EMC_JOINT_ENABLE emc_joint_enable_msg; 1172 EMC_JOINT_DISABLE emc_joint_disable_msg; 1173 1174 if (val) { 1175 emc_joint_enable_msg.joint = joint; 1176 emcCommandSend(emc_joint_enable_msg); 1177 } else { 1178 emc_joint_disable_msg.joint = joint; 1179 emcCommandSend(emc_joint_disable_msg); 1180 } 1181 if (emcWaitType == EMC_WAIT_RECEIVED) { 1182 return emcCommandWaitReceived(); 1183 } else if (emcWaitType == EMC_WAIT_DONE) { 1184 return emcCommandWaitDone(); 1185 } 1186 1187 return 0; 1188 } 1189 1190 int sendJointLoadComp(int joint, const char *file, int type) 1191 { 1192 EMC_JOINT_LOAD_COMP emc_joint_load_comp_msg; 1193 1194 strcpy(emc_joint_load_comp_msg.file, file); 1195 emc_joint_load_comp_msg.type = type; 1196 emcCommandSend(emc_joint_load_comp_msg); 1197 if (emcWaitType == EMC_WAIT_RECEIVED) { 1198 return emcCommandWaitReceived(); 1199 } else if (emcWaitType == EMC_WAIT_DONE) { 1200 return emcCommandWaitDone(); 1201 } 1202 1203 return 0; 1204 } 1205 1206 int sendSetTeleopEnable(int enable) 1207 { 1208 EMC_TRAJ_SET_TELEOP_ENABLE emc_set_teleop_enable_msg; 1209 1210 emc_set_teleop_enable_msg.enable = enable; 1211 emcCommandSend(emc_set_teleop_enable_msg); 1212 if (emcWaitType == EMC_WAIT_RECEIVED) { 1213 return emcCommandWaitReceived(); 1214 } else if (emcWaitType == EMC_WAIT_DONE) { 1215 return emcCommandWaitDone(); 1216 } 1217 1218 return 0; 1219 } 1220 1221 int sendClearProbeTrippedFlag() 1222 { 1223 EMC_TRAJ_CLEAR_PROBE_TRIPPED_FLAG emc_clear_probe_tripped_flag_msg; 1224 1225 emc_clear_probe_tripped_flag_msg.serial_number = 1226 emcCommandSend(emc_clear_probe_tripped_flag_msg); 1227 if (emcWaitType == EMC_WAIT_RECEIVED) { 1228 return emcCommandWaitReceived(); 1229 } else if (emcWaitType == EMC_WAIT_DONE) { 1230 return emcCommandWaitDone(); 1231 } 1232 1233 return 0; 1234 } 1235 1236 int sendProbe(double x, double y, double z) 1237 { 1238 EMC_TRAJ_PROBE emc_probe_msg; 1239 1240 emc_probe_msg.pos.tran.x = x; 1241 emc_probe_msg.pos.tran.y = y; 1242 emc_probe_msg.pos.tran.z = z; 1243 1244 emcCommandSend(emc_probe_msg); 1245 if (emcWaitType == EMC_WAIT_RECEIVED) { 1246 return emcCommandWaitReceived(); 1247 } else if (emcWaitType == EMC_WAIT_DONE) { 1248 return emcCommandWaitDone(); 1249 } 1250 1251 return 0; 1252 } 1253 1254 int iniLoad(const char *filename) 1255 { 1256 IniFile inifile; 1257 const char *inistring; 1258 char displayString[LINELEN] = ""; 1259 int t; 1260 int i; 1261 1262 // open it 1263 if (inifile.Open(filename) == false) { 1264 return -1; 1265 } 1266 1267 if (NULL != (inistring = inifile.Find("DEBUG", "EMC"))) { 1268 // copy to global 1269 if (1 != sscanf(inistring, "%i", &emc_debug)) { 1270 emc_debug = 0; 1271 } 1272 } else { 1273 // not found, use default 1274 emc_debug = 0; 1275 } 1276 1277 if (NULL != (inistring = inifile.Find("NML_FILE", "EMC"))) { 1278 // copy to global 1279 strcpy(emc_nmlfile, inistring); 1280 } else { 1281 // not found, use default 1282 } 1283 1284 for (t = 0; t < EMCMOT_MAX_JOINTS; t++) { 1285 jogPol[t] = 1; // set to default 1286 sprintf(displayString, "JOINT_%d", t); 1287 if (NULL != (inistring = 1288 inifile.Find("JOGGING_POLARITY", displayString)) && 1289 1 == sscanf(inistring, "%d", &i) && i == 0) { 1290 // it read as 0, so override default 1291 jogPol[t] = 0; 1292 } 1293 } 1294 1295 if (NULL != (inistring = inifile.Find("LINEAR_UNITS", "DISPLAY"))) { 1296 if (!strcmp(inistring, "AUTO")) { 1297 linearUnitConversion = LINEAR_UNITS_AUTO; 1298 } else if (!strcmp(inistring, "INCH")) { 1299 linearUnitConversion = LINEAR_UNITS_INCH; 1300 } else if (!strcmp(inistring, "MM")) { 1301 linearUnitConversion = LINEAR_UNITS_MM; 1302 } else if (!strcmp(inistring, "CM")) { 1303 linearUnitConversion = LINEAR_UNITS_CM; 1304 } 1305 } else { 1306 // not found, leave default alone 1307 } 1308 1309 if (NULL != (inistring = inifile.Find("ANGULAR_UNITS", "DISPLAY"))) { 1310 if (!strcmp(inistring, "AUTO")) { 1311 angularUnitConversion = ANGULAR_UNITS_AUTO; 1312 } else if (!strcmp(inistring, "DEG")) { 1313 angularUnitConversion = ANGULAR_UNITS_DEG; 1314 } else if (!strcmp(inistring, "RAD")) { 1315 angularUnitConversion = ANGULAR_UNITS_RAD; 1316 } else if (!strcmp(inistring, "GRAD")) { 1317 angularUnitConversion = ANGULAR_UNITS_GRAD; 1318 } 1319 } else { 1320 // not found, leave default alone 1321 } 1322 1323 // close it 1324 inifile.Close(); 1325 1326 return 0; 1327 } 1328 1329 int checkStatus () 1330 { 1331 if (emcStatus) return 1; 1332 return 0; 1333 } 1334 1335 1336