/********************************************************************* Description: emctaskmain.cc*   Main program for EMC task level**   Derived from a work by Fred Proctor & Will Shackleford** Author:* License: GPL Version 2* System: Linux*    * Copyright (c) 2004 All rights reserved.*********************************************************************//*  Principles of operation:  1.  The main program calls emcTaskPlan() and emcTaskExecute() cyclically.  2.  emcTaskPlan() reads the new command, and decides what to do with  it based on the mode (manual, auto, mdi) or state (estop, on) of the  machine. Many of the commands just go out immediately to the  subsystems (motion and IO). In auto mode, the interpreter is called  and as a result the interp_list is appended with NML commands.  3.  emcTaskExecute() executes a big switch on execState. If it's done,  it gets the next item off the interp_list, and sets execState to the  preconditions for that. These preconditions include waiting for motion,  waiting for IO, etc. Once they are satisfied, it issues the command, and  sets execState to the postconditions. Once those are satisfied, it gets  the next item off the interp_list, and so on.  4.  preconditions and postconditions are only looked at in conjunction  with commands on the interp_list. Immediate commands won't have any  pre- or postconditions associated with them looked at.  5.  At this point, nothing in this file adds anything to the interp_list.  This could change, for example, when defining pre- and postconditions for  jog or home commands. If this is done, make sure that the corresponding  abort command clears out the interp_list.  6. Single-stepping is handled in checkPreconditions() as the first  condition. If we're in single-stepping mode, as indicated by the  variable 'stepping', we set the state to waiting-for-step. This  polls on the variable 'steppingWait' which is reset to zero when a  step command is received, and set to one when the command is  issued.  */extern "C" {#include <stdio.h>		// vsprintf()#include <string.h>		// strcpy()#include <stdarg.h>		// va_start()#include <stdlib.h>		// exit()#include <signal.h>		// signal(), SIGINT#include <float.h>		// DBL_MAX#include <sys/types.h>		// pid_t#include <unistd.h>		// fork()#include <sys/wait.h>		// waitpid(), WNOHANG, WIFEXITED#include <ctype.h>		// isspace()#include <libintl.h>}#include "rcs.hh"		// NML classes, nmlErrorFormat()#include "emc.hh"		// EMC NML#include "canon.hh"		// CANON_TOOL_TABLE stuff#include "inifile.hh"		// INIFILE#include "interpl.hh"		// NML_INTERP_LIST, interp_list#include "emcglb.h"		// EMC_INIFILE,NMLFILE, EMC_TASK_CYCLE_TIME#include "interp_return.hh"	// public interpreter return values#ifdef USE_NLS#define _(string) gettext(string)#else#define _(string) (string)#endif// command line args-- global so that other modules can access int Argc;char **Argv;// NML channelsstatic RCS_CMD_CHANNEL *emcCommandBuffer = 0;static RCS_STAT_CHANNEL *emcStatusBuffer = 0;static NML *emcErrorBuffer = 0;// NML command channel data pointerstatic RCS_CMD_MSG *emcCommand = 0;// global EMC statusEMC_STAT *emcStatus = 0;// timer stuffstatic RCS_TIMER *timer = 0;// flag signifying that ini file [TASK] CYCLE_TIME is <= 0.0, so// we should not delay at all between cycles. This means also that// the EMC_TASK_CYCLE_TIME global will be set to the measured cycle// time each cycle, in case other code references this.static int emcTaskNoDelay = 0;static double EMC_TASK_CYCLE_TIME_ORIG = 0.0;// delay counterstatic double taskExecDelayTimeout = 0.0;// emcTaskQueueCommand puts cmd on interp_liststatic int emcTaskQueueCommand(NMLmsg * cmd);// emcTaskIssueCommand issues command immediatelystatic int emcTaskIssueCommand(NMLmsg * cmd);// pending command to be sent out by emcTaskExecute()static NMLmsg *emcTaskCommand = 0;// signal handling code to stop main loopstatic int done;static int emctask_shutdown(void);static int pseudoMdiLineNumber = -1;static void emctask_quit(int sig){    // set main's done flag    done = 1;    // restore signal handler    signal(sig, emctask_quit);}// implementation of EMC error loggerint emcOperatorError(int id, const char *fmt, ...){    EMC_OPERATOR_ERROR error_msg;    va_list ap;    // check channel for validity    if (emcErrorBuffer == NULL)	return -1;    if (!emcErrorBuffer->valid())	return -1;    if (NULL == fmt) {	return -1;    }    if (0 == *fmt) {	return -1;    }    // prepend error code, leave off 0 ad-hoc code    error_msg.error[0] = 0;    if (0 != id) {	sprintf(error_msg.error, "[%d] ", id);    }    // append error string    va_start(ap, fmt);    vsprintf(&error_msg.error[strlen(error_msg.error)], fmt, ap);    va_end(ap);    // force a NULL at the end for safety    error_msg.error[LINELEN - 1] = 0;    // write it    rcs_print("%s\n", error_msg.error);    return emcErrorBuffer->write(error_msg);}int emcOperatorText(int id, const char *fmt, ...){    EMC_OPERATOR_TEXT text_msg;    va_list ap;    // check channel for validity    if (emcErrorBuffer == NULL)	return -1;    if (!emcErrorBuffer->valid())	return -1;    // write args to NML message (ignore int text code)    va_start(ap, fmt);    vsprintf(text_msg.text, fmt, ap);    va_end(ap);    // force a NULL at the end for safety    text_msg.text[LINELEN - 1] = 0;    // write it    return emcErrorBuffer->write(text_msg);}int emcOperatorDisplay(int id, const char *fmt, ...){    EMC_OPERATOR_DISPLAY display_msg;    va_list ap;    // check channel for validity    if (emcErrorBuffer == NULL)	return -1;    if (!emcErrorBuffer->valid())	return -1;    // write args to NML message (ignore int display code)    va_start(ap, fmt);    vsprintf(display_msg.display, fmt, ap);    va_end(ap);    // force a NULL at the end for safety    display_msg.display[LINELEN - 1] = 0;    // write it    return emcErrorBuffer->write(display_msg);}/*  handling of EMC_SYSTEM_CMD *//* convert string to arg/argv set */static int argvize(const char *src, char *dst, char *argv[], int len){    char *bufptr;    int argvix;    char inquote;    char looking;    strncpy(dst, src, len);    dst[len - 1] = 0;    bufptr = dst;    inquote = 0;    argvix = 0;    looking = 1;    while (0 != *bufptr) {	if (*bufptr == '"') {	    *bufptr = 0;	    if (inquote) {		inquote = 0;		looking = 1;	    } else {		inquote = 1;	    }	} else if (isspace(*bufptr) && !inquote) {	    looking = 1;	    *bufptr = 0;	} else if (looking) {	    looking = 0;	    argv[argvix] = bufptr;	    argvix++;	}	bufptr++;    }    argv[argvix] = 0;		// null-terminate the argv list    return argvix;}static pid_t emcSystemCmdPid = 0;int emcSystemCmd(char *s){    char buffer[EMC_SYSTEM_CMD_LEN];    char *argv[EMC_SYSTEM_CMD_LEN / 2 + 1];    if (0 != emcSystemCmdPid) {	// something's already running, and we can only handle one	if (EMC_DEBUG & EMC_DEBUG_TASK_ISSUE) {	    rcs_print		("emcSystemCmd: abandoning process %d, running ``%s''\n",		 emcSystemCmdPid, s);	}    }    emcSystemCmdPid = fork();    if (-1 == emcSystemCmdPid) {	// we're still the parent, with no child created	if (EMC_DEBUG & EMC_DEBUG_TASK_ISSUE) {	    rcs_print("system command ``%s'' can't be executed\n", s);	}	return -1;    }    if (0 == emcSystemCmdPid) {	// we're the child	// convert string to argc/argv	argvize(s, buffer, argv, EMC_SYSTEM_CMD_LEN);	// drop any setuid privileges	setuid(getuid());	execvp(argv[0], argv);	// if we get here, we didn't exec	if (EMC_DEBUG & EMC_DEBUG_TASK_ISSUE) {	    rcs_print("emcSystemCmd: can't execute ``%s''\n", s);	}	return -1;    }    // else we're the parent    return 0;}// shorthand typecasting ptrsstatic EMC_AXIS_HALT *axis_halt_msg;static EMC_AXIS_DISABLE *disable_msg;static EMC_AXIS_ENABLE *enable_msg;static EMC_AXIS_HOME *home_msg;static EMC_AXIS_JOG *jog_msg;static EMC_AXIS_ABORT *axis_abort_msg;static EMC_AXIS_INCR_JOG *incr_jog_msg;static EMC_AXIS_ABS_JOG *abs_jog_msg;//static EMC_AXIS_SET_CYCLE_TIME *set_cycle_time_msg;//static EMC_AXIS_SET_GAINS *set_gains_msg;static EMC_AXIS_SET_BACKLASH *set_backlash_msg;static EMC_AXIS_SET_HOMING_PARAMS *set_homing_params_msg;//static EMC_AXIS_SET_INPUT_SCALE *set_input_scale_msg;//static EMC_AXIS_SET_OUTPUT_SCALE *set_output_scale_msg;static EMC_AXIS_SET_FERROR *set_ferror_msg;static EMC_AXIS_SET_MIN_FERROR *set_min_ferror_msg;static EMC_AXIS_SET_MAX_POSITION_LIMIT *set_max_limit_msg;static EMC_AXIS_SET_MIN_POSITION_LIMIT *set_min_limit_msg;static EMC_AXIS_OVERRIDE_LIMITS *axis_lim_msg;//static EMC_AXIS_SET_OUTPUT *axis_output_msg;static EMC_AXIS_LOAD_COMP *axis_load_comp_msg;static EMC_AXIS_ALTER *axis_alter_msg;//static EMC_AXIS_SET_STEP_PARAMS *set_step_params_msg;static EMC_TRAJ_SET_SCALE *emcTrajSetScaleMsg;static EMC_TRAJ_SET_VELOCITY *emcTrajSetVelocityMsg;static EMC_TRAJ_SET_ACCELERATION *emcTrajSetAccelerationMsg;static EMC_TRAJ_LINEAR_MOVE *emcTrajLinearMoveMsg;static EMC_TRAJ_CIRCULAR_MOVE *emcTrajCircularMoveMsg;static EMC_TRAJ_DELAY *emcTrajDelayMsg;static EMC_TRAJ_SET_TERM_COND *emcTrajSetTermCondMsg;static EMC_TRAJ_SET_SPINDLESYNC *emcTrajSetSpindlesyncMsg;// These classes are commented out because the compiler// complains that they are "defined but not used".//static EMC_MOTION_SET_AOUT *emcMotionSetAoutMsg;//static EMC_MOTION_SET_DOUT *emcMotionSetDoutMsg;static EMC_SPINDLE_ON *spindle_on_msg;static EMC_TOOL_PREPARE *tool_prepare_msg;static EMC_TOOL_LOAD_TOOL_TABLE *load_tool_table_msg;static EMC_TOOL_SET_OFFSET *emc_tool_set_offset_msg;static EMC_TASK_SET_MODE *mode_msg;static EMC_TASK_SET_STATE *state_msg;static EMC_TASK_PLAN_RUN *run_msg;static EMC_TASK_PLAN_EXECUTE *execute_msg;static EMC_TASK_PLAN_OPEN *open_msg;// commands we compose herestatic EMC_TASK_PLAN_RUN taskPlanRunCmd;	// 16-Aug-1999 FMPstatic EMC_TASK_PLAN_INIT taskPlanInitCmd;static EMC_TASK_PLAN_SYNCH taskPlanSynchCmd;static int interpResumeState = EMC_TASK_INTERP_IDLE;static int programStartLine = 0;	// which line to run program from// how long the interp list can be/*! \todo FIXME-- make an ini file global */#define EMC_TASK_INTERP_MAX_LEN 1000static int stepping = 0;static int steppingWait = 0;static int steppedLine = 0;/*  checkInterpList(NML_INTERP_LIST *il, EMC_STAT *stat) takes a pointer  to an interpreter list and a pointer to the EMC status, pops each NML  message off the list, and checks it against limits, resource availability,  etc. in the status.  It returns 0 if all messages check out, -1 if any of them fail. If one  fails, the rest of the list is not checked. */static int checkInterpList(NML_INTERP_LIST * il, EMC_STAT * stat){    NMLmsg *cmd = 0;    // let's create some shortcuts to casts at compile time#define operator_error_msg ((EMC_OPERATOR_ERROR *) cmd)#define linear_move ((EMC_TRAJ_LINEAR_MOVE *) cmd)#define circular_move ((EMC_TRAJ_CIRCULAR_MOVE *) cmd)    while (il->len() > 0) {	cmd = il->get();	switch (cmd->type) {	case EMC_OPERATOR_ERROR_TYPE:	    emcOperatorError(operator_error_msg->id,			     operator_error_msg->error);	    break;	case EMC_TRAJ_LINEAR_MOVE_TYPE:	    if (linear_move->end.tran.x >		stat->motion.axis[0].maxPositionLimit) {		emcOperatorError(0, "%s\n%s", stat->task.command,				 _("exceeds +X limit"));		return -1;	    }	    if (linear_move->end.tran.y >		stat->motion.axis[1].maxPositionLimit) {		emcOperatorError(0, "%s\n%s", stat->task.command,				 _("exceeds +Y limit"));		return -1;	    }	    if (linear_move->end.tran.z >		stat->motion.axis[2].maxPositionLimit) {		emcOperatorError(0, "%s\n%s", stat->task.command,				 _("exceeds +Z limit"));		return -1;	    }	    if (linear_move->end.tran.x <		stat->motion.axis[0].minPositionLimit) {		emcOperatorError(0, "%s\n%s", stat->task.command,				 _("exceeds -X limit"));		return -1;	    }	    if (linear_move->end.tran.y <		stat->motion.axis[1].minPositionLimit) {		emcOperatorError(0, "%s\n%s", stat->task.command,				 _("exceeds -Y limit"));		return -1;	    }	    if (linear_move->end.tran.z <		stat->motion.axis[2].minPositionLimit) {		emcOperatorError(0, "%s\n%s", stat->task.command,				 _("exceeds -Z limit"));		return -1;	    }	    break;	case EMC_TRAJ_CIRCULAR_MOVE_TYPE:	    if (circular_move->end.tran.x >		stat->motion.axis[0].maxPositionLimit) {		emcOperatorError(0, "%s\n%s", stat->task.command,				 _("exceeds +X limit"));		return -1;	    }	    if (circular_move->end.tran.y >		stat->motion.axis[1].maxPositionLimit) {		emcOperatorError(0, "%s\n%s", stat->task.command,				 _("exceeds +Y limit"));		return -1;	    }	    if (circular_move->end.tran.z >		stat->motion.axis[2].maxPositionLimit) {		emcOperatorError(0, "%s\n%s", stat->task.command,				 _("exceeds +Z limit"));		return -1;	    }	    if (circular_move->end.tran.x <		stat->motion.axis[0].minPositionLimit) {		emcOperatorError(0, "%s\n%s", stat->task.command,				 _("exceeds -X limit"));		return -1;	    }	    if (circular_move->end.tran.y <		stat->motion.axis[1].minPositionLimit) {		emcOperatorError(0, "%s\n%s", stat->task.command,				 _("exceeds -Y limit"));		return -1;	    }	    if (circular_move->end.tran.z <		stat->motion.axis[2].minPositionLimit) {		emcOperatorError(0, "%s\n%s", stat->task.command,				 _("exceeds -Z limit"));		return -1;	    }	    break;