emctask.cc
1 /******************************************************************** 2 * Description: emctask.cc 3 * Mode and state management for EMC_TASK class 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 * Last change: 14 ********************************************************************/ 15 16 #include <stdlib.h> 17 #include <string.h> // strncpy() 18 #include <sys/stat.h> // struct stat 19 #include <unistd.h> // stat() 20 #include <limits.h> // PATH_MAX 21 #include <dlfcn.h> 22 23 #include "rcs.hh" // INIFILE 24 #include "emc.hh" // EMC NML 25 #include "emc_nml.hh" 26 #include "emcglb.h" // EMC_INIFILE 27 #include "interpl.hh" // NML_INTERP_LIST, interp_list 28 #include "canon.hh" // CANON_VECTOR, GET_PROGRAM_ORIGIN() 29 #include "rs274ngc_interp.hh" // the interpreter 30 #include "interp_return.hh" // INTERP_FILE_NOT_OPEN 31 #include "inifile.hh" 32 #include "rcs_print.hh" 33 #include "task.hh" // emcTaskCommand etc 34 #include "python_plugin.hh" 35 #include "taskclass.hh" 36 #include "motion.h" 37 38 #define USER_DEFINED_FUNCTION_MAX_DIRS 5 39 #define MAX_M_DIRS (USER_DEFINED_FUNCTION_MAX_DIRS+1) 40 //note:the +1 is for the PROGRAM_PREFIX or default directory==nc_files 41 42 /* flag for how we want to interpret traj coord mode, as mdi or auto */ 43 static int mdiOrAuto = EMC_TASK_MODE_AUTO; 44 45 InterpBase *pinterp=0; 46 #define interp (*pinterp) 47 setup_pointer _is = 0; // helper for gdb hardware watchpoints FIXME 48 49 50 /* 51 format string for user-defined programs, e.g., "programs/M1%02d" means 52 user-defined programs are in the programs/ directory and are named 53 M1XX, where XX is a two-digit string. 54 */ 55 static char user_defined_fmt[MAX_M_DIRS][EMC_SYSTEM_CMD_LEN]; // ex: "dirname/M1%02d" 56 57 // index to directory for each user defined function: 58 static int user_defined_function_dirindex[USER_DEFINED_FUNCTION_NUM]; 59 60 static void user_defined_add_m_code(int num, double arg1, double arg2) 61 { 62 // num is the m_code number, typically 00-99 corresponding to M100-M199 63 char fmt[EMC_SYSTEM_CMD_LEN]; 64 EMC_SYSTEM_CMD system_cmd; 65 66 //we call FINISH() to flush any linked motions before the M1xx call, 67 //otherwise they would mix badly 68 FINISH(); 69 strcpy(fmt, user_defined_fmt[user_defined_function_dirindex[num]]); 70 strcat(fmt, " %f %f"); 71 snprintf(system_cmd.string, sizeof(system_cmd.string), fmt, num, arg1, arg2); 72 interp_list.append(system_cmd); 73 } 74 75 int emcTaskInit() 76 { 77 char mdir[MAX_M_DIRS][PATH_MAX+1]; 78 int num,dct,dmax; 79 char path[EMC_SYSTEM_CMD_LEN]; 80 struct stat buf; 81 IniFile inifile; 82 const char *inistring; 83 84 inifile.Open(emc_inifile); 85 86 // Identify user_defined_function directories 87 if (NULL != (inistring = inifile.Find("PROGRAM_PREFIX", "DISPLAY"))) { 88 strncpy(mdir[0],inistring, sizeof(mdir[0])); 89 if (mdir[0][sizeof(mdir[0])-1] != '\0') { 90 rcs_print("[DISPLAY]PROGRAM_PREFIX too long (max len %zu)\n", sizeof(mdir[0])); 91 return -1; 92 } 93 } else { 94 // default dir if no PROGRAM_PREFIX 95 strncpy(mdir[0],"nc_files", sizeof(mdir[0])); 96 if (mdir[0][sizeof(mdir[0])-1] != '\0') { 97 rcs_print("default nc_files too long (max len %zu)\n", sizeof(mdir[0])); 98 return -1; 99 } 100 } 101 dmax = 1; //one directory mdir[0], USER_M_PATH specifies additional dirs 102 103 // user can specify a list of directories for user defined functions 104 // with a colon (:) separated list 105 if (NULL != (inistring = inifile.Find("USER_M_PATH", "RS274NGC"))) { 106 char* nextdir; 107 char tmpdirs[PATH_MAX]; 108 109 for (dct=1; dct < MAX_M_DIRS; dct++) mdir[dct][0] = 0; 110 111 strncpy(tmpdirs,inistring, sizeof(tmpdirs)); 112 if (tmpdirs[sizeof(tmpdirs)-1] != '\0') { 113 rcs_print("[RS274NGC]USER_M_PATH too long (max len %zu)\n", sizeof(tmpdirs)); 114 return -1; 115 } 116 117 nextdir = strtok(tmpdirs,":"); // first token 118 dct = 1; 119 while (dct < MAX_M_DIRS) { 120 if (nextdir == NULL) break; // no more tokens 121 strncpy(mdir[dct],nextdir, sizeof(mdir[dct])); 122 if (mdir[dct][sizeof(mdir[dct])-1] != '\0') { 123 rcs_print("[RS274NGC]USER_M_PATH component (%s) too long (max len %zu)\n", nextdir, sizeof(mdir[dct])); 124 return -1; 125 } 126 nextdir = strtok(NULL,":"); 127 dct++; 128 } 129 dmax=dct; 130 } 131 inifile.Close(); 132 133 /* check for programs named programs/M100 .. programs/M199 and add 134 any to the user defined functions list */ 135 for (num = 0; num < USER_DEFINED_FUNCTION_NUM; num++) { 136 for (dct=0; dct < dmax; dct++) { 137 char expanddir[LINELEN]; 138 if (!mdir[dct][0]) continue; 139 if (inifile.TildeExpansion(mdir[dct],expanddir,sizeof(expanddir))) { 140 rcs_print("emcTaskInit: TildeExpansion failed for %s, ignoring\n", 141 mdir[dct]); 142 } 143 snprintf(path, sizeof(path), "%s/M1%02d",expanddir,num); 144 if (0 == stat(path, &buf)) { 145 if (buf.st_mode & S_IXUSR) { 146 // set the user_defined_fmt string with dirname 147 // note the %%02d means 2 digits after the M code 148 // and we need two % to get the literal % 149 snprintf(user_defined_fmt[dct], sizeof(user_defined_fmt[0]), 150 "%s/M1%%02d", expanddir); // update global 151 USER_DEFINED_FUNCTION_ADD(user_defined_add_m_code,num); 152 if (emc_debug & EMC_DEBUG_CONFIG) { 153 rcs_print("emcTaskInit: adding user-defined function %s\n", 154 path); 155 } 156 user_defined_function_dirindex[num] = dct; 157 break; // use first occurrence found for num 158 } else { 159 if (emc_debug & EMC_DEBUG_CONFIG) { 160 rcs_print("emcTaskInit: user-defined function %s found, but not executable, so ignoring\n", 161 path); 162 } 163 } 164 } 165 } 166 } 167 168 return 0; 169 } 170 171 int emcTaskHalt() 172 { 173 return 0; 174 } 175 176 int emcTaskAbort() 177 { 178 emcMotionAbort(); 179 180 // clear out the pending command 181 emcTaskCommand = 0; 182 interp_list.clear(); 183 184 // clear out the interpreter state 185 emcStatus->task.interpState = EMC_TASK_INTERP_IDLE; 186 emcStatus->task.execState = EMC_TASK_EXEC_DONE; 187 emcStatus->task.task_paused = 0; 188 emcStatus->task.motionLine = 0; 189 emcStatus->task.readLine = 0; 190 emcStatus->task.command[0] = 0; 191 emcStatus->task.callLevel = 0; 192 193 stepping = 0; 194 steppingWait = 0; 195 196 // now queue up command to resynch interpreter 197 EMC_TASK_PLAN_SYNCH taskPlanSynchCmd; 198 emcTaskQueueCommand(&taskPlanSynchCmd); 199 200 // without emcTaskPlanClose(), a new run command resumes at 201 // aborted line-- feature that may be considered later 202 { 203 int was_open = taskplanopen; 204 emcTaskPlanClose(); 205 if (emc_debug & EMC_DEBUG_INTERP && was_open) { 206 rcs_print("emcTaskPlanClose() called at %s:%d\n", __FILE__, 207 __LINE__); 208 } 209 } 210 211 return 0; 212 } 213 214 int emcTaskSetMode(int mode) 215 { 216 int retval = 0; 217 218 switch (mode) { 219 case EMC_TASK_MODE_MANUAL: 220 // go to manual mode 221 if (all_homed()) { 222 emcTrajSetMode(EMC_TRAJ_MODE_TELEOP); 223 } else { 224 emcTrajSetMode(EMC_TRAJ_MODE_FREE); 225 } 226 mdiOrAuto = EMC_TASK_MODE_AUTO; // we'll default back to here 227 break; 228 229 case EMC_TASK_MODE_MDI: 230 // go to mdi mode 231 emcTrajSetMode(EMC_TRAJ_MODE_COORD); 232 emcTaskAbort(); 233 emcTaskPlanSynch(); 234 mdiOrAuto = EMC_TASK_MODE_MDI; 235 break; 236 237 case EMC_TASK_MODE_AUTO: 238 // go to auto mode 239 emcTrajSetMode(EMC_TRAJ_MODE_COORD); 240 emcTaskAbort(); 241 emcTaskPlanSynch(); 242 mdiOrAuto = EMC_TASK_MODE_AUTO; 243 break; 244 245 default: 246 retval = -1; 247 break; 248 } 249 250 return retval; 251 } 252 253 int emcTaskSetState(int state) 254 { 255 int t; 256 int retval = 0; 257 258 switch (state) { 259 case EMC_TASK_STATE_OFF: 260 emcMotionAbort(); 261 // turn the machine servos off-- go into READY state 262 for (t = 0; t < emcStatus->motion.traj.spindles; t++) emcSpindleAbort(t); 263 for (t = 0; t < emcStatus->motion.traj.joints; t++) { 264 emcJointDisable(t); 265 } 266 emcTrajDisable(); 267 emcIoAbort(EMC_ABORT_TASK_STATE_OFF); 268 emcLubeOff(); 269 emcTaskAbort(); 270 emcJointUnhome(-2); // only those joints which are volatile_home 271 emcAbortCleanup(EMC_ABORT_TASK_STATE_OFF); 272 emcTaskPlanSynch(); 273 break; 274 275 case EMC_TASK_STATE_ON: 276 // turn the machine servos on 277 emcTrajEnable(); 278 for (t = 0; t < emcStatus->motion.traj.joints; t++){ 279 emcJointEnable(t); 280 } 281 emcLubeOn(); 282 break; 283 284 case EMC_TASK_STATE_ESTOP_RESET: 285 // reset the estop 286 emcAuxEstopOff(); 287 emcLubeOff(); 288 emcTaskAbort(); 289 emcIoAbort(EMC_ABORT_TASK_STATE_ESTOP_RESET); 290 for (t = 0; t < emcStatus->motion.traj.spindles; t++) emcSpindleAbort(t); 291 emcAbortCleanup(EMC_ABORT_TASK_STATE_ESTOP_RESET); 292 emcTaskPlanSynch(); 293 break; 294 295 case EMC_TASK_STATE_ESTOP: 296 emcMotionAbort(); 297 for (t = 0; t < emcStatus->motion.traj.spindles; t++) emcSpindleAbort(t); 298 // go into estop-- do both IO estop and machine servos off 299 emcAuxEstopOn(); 300 for (t = 0; t < emcStatus->motion.traj.joints; t++) { 301 emcJointDisable(t); 302 } 303 emcTrajDisable(); 304 emcLubeOff(); 305 emcTaskAbort(); 306 emcIoAbort(EMC_ABORT_TASK_STATE_ESTOP); 307 for (t = 0; t < emcStatus->motion.traj.spindles; t++) emcSpindleAbort(t); 308 emcJointUnhome(-2); // only those joints which are volatile_home 309 emcAbortCleanup(EMC_ABORT_TASK_STATE_ESTOP); 310 emcTaskPlanSynch(); 311 break; 312 313 default: 314 retval = -1; 315 break; 316 } 317 318 return retval; 319 } 320 321 // WM access functions 322 323 /* 324 determineMode() 325 326 Looks at mode of subsystems, and returns associated mode 327 328 Depends on traj mode, and mdiOrAuto flag 329 330 traj mode mdiOrAuto task mode 331 --------- --------- --------- 332 FREE XXX MANUAL 333 TELEOP XXX MANUAL 334 COORD MDI MDI 335 COORD AUTO AUTO 336 */ 337 static int determineMode() 338 { 339 if (emcStatus->motion.traj.mode == EMC_TRAJ_MODE_FREE) { 340 return EMC_TASK_MODE_MANUAL; 341 } 342 if (emcStatus->motion.traj.mode == EMC_TRAJ_MODE_TELEOP) { 343 return EMC_TASK_MODE_MANUAL; 344 } 345 // for EMC_TRAJ_MODE_COORD 346 return mdiOrAuto; 347 } 348 349 /* 350 determineState() 351 352 Looks at state of subsystems, and returns associated state 353 354 Depends on traj enabled, io estop, and desired task state 355 356 traj enabled io estop state 357 ------------ -------- ----- 358 DISABLED ESTOP ESTOP 359 ENABLED ESTOP ESTOP 360 DISABLED OUT OF ESTOP ESTOP_RESET 361 ENABLED OUT OF ESTOP ON 362 */ 363 static int determineState() 364 { 365 if (emcStatus->io.aux.estop) { 366 return EMC_TASK_STATE_ESTOP; 367 } 368 369 if (!emcStatus->motion.traj.enabled) { 370 return EMC_TASK_STATE_ESTOP_RESET; 371 } 372 373 return EMC_TASK_STATE_ON; 374 } 375 376 static int waitFlag = 0; 377 378 static char interp_error_text_buf[LINELEN]; 379 static char interp_stack_buf[LINELEN]; 380 381 static void print_interp_error(int retval) 382 { 383 int index = 0; 384 if (retval == 0) { 385 return; 386 } 387 388 if (0 != emcStatus) { 389 emcStatus->task.interpreter_errcode = retval; 390 } 391 392 interp_error_text_buf[0] = 0; 393 interp.error_text(retval, interp_error_text_buf, LINELEN); 394 if (0 != interp_error_text_buf[0]) { 395 rcs_print_error("interp_error: %s\n", interp_error_text_buf); 396 } 397 emcOperatorError(0, "%s", interp_error_text_buf); 398 index = 0; 399 if (emc_debug & EMC_DEBUG_INTERP) { 400 rcs_print("Interpreter stack: \t"); 401 while (index < 5) { 402 interp_stack_buf[0] = 0; 403 interp.stack_name(index, interp_stack_buf, LINELEN); 404 if (0 == interp_stack_buf[0]) { 405 break; 406 } 407 rcs_print(" - %s ", interp_stack_buf); 408 index++; 409 } 410 rcs_print("\n"); 411 } 412 } 413 414 int emcTaskPlanInit() 415 { 416 if(!pinterp) { 417 IniFile inifile; 418 const char *inistring; 419 inifile.Open(emc_inifile); 420 if((inistring = inifile.Find("INTERPRETER", "TASK"))) { 421 pinterp = interp_from_shlib(inistring); 422 fprintf(stderr, "interp_from_shlib() -> %p\n", pinterp); 423 } 424 inifile.Close(); 425 } 426 if(!pinterp) { 427 pinterp = new Interp; 428 } 429 430 Interp *i = dynamic_cast<Interp*>(pinterp); 431 if(i) _is = &i->_setup; // FIXME 432 else _is = 0; 433 interp.ini_load(emc_inifile); 434 waitFlag = 0; 435 436 int retval = interp.init(); 437 // In task, enable M99 main program endless looping 438 interp.set_loop_on_main_m99(true); 439 if (retval > INTERP_MIN_ERROR) { // I'd think this should be fatal. 440 print_interp_error(retval); 441 } else { 442 if (0 != rs274ngc_startup_code[0]) { 443 retval = interp.execute(rs274ngc_startup_code); 444 while (retval == INTERP_EXECUTE_FINISH) { 445 retval = interp.execute(0); 446 } 447 if (retval > INTERP_MIN_ERROR) { 448 print_interp_error(retval); 449 } 450 } 451 } 452 453 if (emc_debug & EMC_DEBUG_INTERP) { 454 rcs_print("emcTaskPlanInit() returned %d\n", retval); 455 } 456 457 return retval; 458 } 459 460 int emcTaskPlanSetWait() 461 { 462 waitFlag = 1; 463 464 if (emc_debug & EMC_DEBUG_INTERP) { 465 rcs_print("emcTaskPlanSetWait() called\n"); 466 } 467 468 return 0; 469 } 470 471 int emcTaskPlanIsWait() 472 { 473 return waitFlag; 474 } 475 476 int emcTaskPlanClearWait() 477 { 478 waitFlag = 0; 479 480 if (emc_debug & EMC_DEBUG_INTERP) { 481 rcs_print("emcTaskPlanClearWait() called\n"); 482 } 483 484 return 0; 485 } 486 487 int emcTaskPlanSetOptionalStop(bool state) 488 { 489 SET_OPTIONAL_PROGRAM_STOP(state); 490 return 0; 491 } 492 493 int emcTaskPlanSetBlockDelete(bool state) 494 { 495 SET_BLOCK_DELETE(state); 496 return 0; 497 } 498 499 500 int emcTaskPlanSynch() 501 { 502 int retval = interp.synch(); 503 if (retval == INTERP_ERROR) { 504 emcTaskAbort(); 505 } 506 507 if (emc_debug & EMC_DEBUG_INTERP) { 508 rcs_print("emcTaskPlanSynch() returned %d\n", retval); 509 } 510 511 return retval; 512 } 513 514 void emcTaskPlanExit() 515 { 516 if (pinterp != NULL) { 517 interp.exit(); 518 } 519 } 520 521 int emcTaskPlanOpen(const char *file) 522 { 523 if (emcStatus != 0) { 524 emcStatus->task.motionLine = 0; 525 emcStatus->task.currentLine = 0; 526 emcStatus->task.readLine = 0; 527 } 528 529 int retval = interp.open(file); 530 if (retval > INTERP_MIN_ERROR) { 531 print_interp_error(retval); 532 return retval; 533 } 534 taskplanopen = 1; 535 536 if (emc_debug & EMC_DEBUG_INTERP) { 537 rcs_print("emcTaskPlanOpen(%s) returned %d\n", file, retval); 538 } 539 540 return retval; 541 } 542 543 544 int emcTaskPlanRead() 545 { 546 int retval = interp.read(); 547 if (retval == INTERP_FILE_NOT_OPEN) { 548 if (emcStatus->task.file[0] != 0) { 549 retval = interp.open(emcStatus->task.file); 550 if (retval > INTERP_MIN_ERROR) { 551 print_interp_error(retval); 552 } 553 retval = interp.read(); 554 } 555 } 556 if (retval > INTERP_MIN_ERROR) { 557 print_interp_error(retval); 558 } 559 560 if (emc_debug & EMC_DEBUG_INTERP) { 561 rcs_print("emcTaskPlanRead() returned %d\n", retval); 562 } 563 564 return retval; 565 } 566 567 int emcTaskPlanExecute(const char *command) 568 { 569 int inpos = emcStatus->motion.traj.inpos; // 1 if in position, 0 if not. 570 571 if (command != 0) { // Command is 0 if in AUTO mode, non-null if in MDI mode. 572 // Don't sync if not in position. 573 if ((*command != 0) && (inpos)) { 574 interp.synch(); 575 } 576 } 577 int retval = interp.execute(command); 578 if (retval > INTERP_MIN_ERROR) { 579 print_interp_error(retval); 580 } 581 if(command != 0) { 582 FINISH(); 583 } 584 585 if (emc_debug & EMC_DEBUG_INTERP) { 586 rcs_print("emcTaskPlanExecute(0) return %d\n", retval); 587 } 588 589 return retval; 590 } 591 592 int emcTaskPlanExecute(const char *command, int line_number) 593 { 594 int retval = interp.execute(command, line_number); 595 if (retval > INTERP_MIN_ERROR) { 596 print_interp_error(retval); 597 } 598 if(command != 0) { // this means MDI 599 FINISH(); 600 } 601 602 if (emc_debug & EMC_DEBUG_INTERP) { 603 rcs_print("emcTaskPlanExecute(%s) returned %d\n", command, retval); 604 } 605 606 return retval; 607 } 608 609 int emcTaskPlanClose() 610 { 611 int retval = interp.close(); 612 if (retval > INTERP_MIN_ERROR) { 613 print_interp_error(retval); 614 } 615 616 taskplanopen = 0; 617 return retval; 618 } 619 620 int emcTaskPlanReset() 621 { 622 int retval = interp.reset(); 623 if (retval > INTERP_MIN_ERROR) { 624 print_interp_error(retval); 625 } 626 627 return retval; 628 } 629 630 int emcTaskPlanLine() 631 { 632 int retval = interp.line(); 633 634 if (emc_debug & EMC_DEBUG_INTERP) { 635 rcs_print("emcTaskPlanLine() returned %d\n", retval); 636 } 637 638 return retval; 639 } 640 641 int emcTaskPlanLevel() 642 { 643 int retval = interp.call_level(); 644 645 if (emc_debug & EMC_DEBUG_INTERP) { 646 rcs_print("emcTaskPlanLevel() returned %d\n", retval); 647 } 648 649 return retval; 650 } 651 652 int emcTaskPlanCommand(char *cmd) 653 { 654 char buf[LINELEN]; 655 656 strcpy(cmd, interp.command(buf, LINELEN)); 657 658 if (emc_debug & EMC_DEBUG_INTERP) { 659 rcs_print("emcTaskPlanCommand(%s) called. (line_number=%d)\n", 660 cmd, emcStatus->task.readLine); 661 } 662 663 return 0; 664 } 665 666 int emcTaskUpdate(EMC_TASK_STAT * stat) 667 { 668 stat->mode = (enum EMC_TASK_MODE_ENUM) determineMode(); 669 int oldstate = stat->state; 670 stat->state = (enum EMC_TASK_STATE_ENUM) determineState(); 671 672 if(oldstate == EMC_TASK_STATE_ON && oldstate != stat->state) { 673 emcTaskAbort(); 674 for (int s = 0; s < emcStatus->motion.traj.spindles; s++) emcSpindleAbort(s); 675 emcIoAbort(EMC_ABORT_TASK_STATE_NOT_ON); 676 emcAbortCleanup(EMC_ABORT_TASK_STATE_NOT_ON); 677 } 678 679 // execState set in main 680 // interpState set in main 681 if (emcStatus->motion.traj.id > 0) { 682 stat->motionLine = emcStatus->motion.traj.id; 683 } 684 // currentLine set in main 685 // readLine set in main 686 687 char buf[LINELEN]; 688 strcpy(stat->file, interp.file(buf, LINELEN)); 689 // command set in main 690 691 // update active G and M codes 692 interp.active_g_codes(&stat->activeGCodes[0]); 693 interp.active_m_codes(&stat->activeMCodes[0]); 694 interp.active_settings(&stat->activeSettings[0]); 695 696 //update state of optional stop 697 stat->optional_stop_state = GET_OPTIONAL_PROGRAM_STOP(); 698 699 //update state of block delete 700 stat->block_delete_state = GET_BLOCK_DELETE(); 701 702 stat->heartbeat++; 703 704 return 0; 705 } 706 707 int emcAbortCleanup(int reason, const char *message) 708 { 709 int status = interp.on_abort(reason,message); 710 if (status > INTERP_MIN_ERROR) 711 print_interp_error(status); 712 return status; 713 } 714