emcsh.cc

Source code for an Numeric Cmputer

/********************************************************************
* Description: emcsh.cc
*   Extended-Tcl-based EMC automatic test interface
*
*   Derived from a work by Fred Proctor & Will Shackleford
*
* Author:
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2004 All rights reserved.
*
* Last change:
* $Revision: 1.15 $
* $Author: jmkasunich $
* $Date: 2005/11/24 03:41:40 $
********************************************************************/

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <signal.h>
#include <math.h>
#include "tcl.h"
#include "tk.h"

#include "rcs.hh"
#include "posemath.h"		// PM_POSE, TO_RAD
#include "emc.hh"		// EMC NML
#include "canon.hh"		// CANON_UNITS, CANON_UNITS_INCHES,MM,CM
#include "emcglb.h"		// EMC_NMLFILE, TRAJ_MAX_VELOCITY, etc.
#include "emccfg.h"		// DEFAULT_TRAJ_MAX_VELOCITY
#include "inifile.hh"		// INIFILE

/*
  Using emcsh:

  emcsh {<filename>} {-- -ini <ini file>}

  With filename, it opens NML buffers to the EMC, runs the script, closes
  the buffers, and quits.

  With -- -ini <inifile>, uses inifile instead of emc.ini. Note that
  the two dashes prevents Tcl from looking at the remaining args, which
  would otherwise trigger a Tcl error that it doesn't understand what
  -ini means.

  Without filename, it runs interactively.

  The files (or manual input) are Tcl scripts, extended with EMC-specific
  commands. These commands are all prefixed with "emc_", which makes them
  somewhat inconvenient for typing but avoids name conflicts, e.g., open.

  Some commands take 0 or more arguments. 0 arguments means they return
  the associated value; the argument would be to set the value.

  Commands are sent to the EMC, and control resumes immediately. You can
  call a timed wait until the command got there, or a timed wait until the
  command completed, or not wait at all.

  EMC commands:

  EMC_INIFILE
  Exported values of the EMC global of the same name

  emc_plat
  Returns the platform for which this was compiled, e.g., linux_2_0_36

  emc_ini <var> <section>
  Returns the string value of <var> in section <section>, in EMC_INIFILE

  emc_debug {<new value>}
  With no arg, returns the integer value of EMC_DEBUG, in the EMC. Note that
  it may not be true that the local EMC_DEBUG variable here (in emcsh and
  the GUIs that use it) is the same as the EMC_DEBUG value in the EMC. This
  can happen if the EMC is started from one .ini file, and the GUI is started
  with another that has a different value for DEBUG.
  With an arg, sends a command to the EMC to set the new debug level,
  and sets the EMC_DEBUG global here to the same value. This will make
  the two values the same, since they really ought to be the same.

  emc_set_wait none | received | done
  Set the wait for commands to return to be right away (none), after the
  command was sent and received (received), or after the command was
  done (done).

  emc_wait received | done
  Force a wait for the previous command to be received, or done. This lets
  you wait in the event that "emc_set_wait none" is in effect.

  emc_set_timeout <timeout>
  Set the timeout for commands to return to <timeout>, in seconds. Timeout
  is a real number. If it's <= 0.0, it means wait forever. Default is 0.0,
  wait forever.

  emc_update (none) | none | auto
  With no arg, forces an update of the EMC status. With "none", doesn't
  cause an automatic update of status with other emc_ words. With "auto",
  makes emc_ words automatically update status before they return values.

  emc_error
  Returns the current EMC error string, or "ok" if no error.

  emc_operator_display
  Returns the current EMC operator display string, or "ok" if none.

  emc_operator_text
  Returns the current EMC operator text string, or "ok" if none.

  emc_time
  Returns the time, in seconds, from the start of the epoch. This starting
  time depends on the platform.

  emc_estop (none) | on | off
  With no arg, returns the estop setting as "on" or "off". Otherwise,
  sends an estop on or off command.

  emc_machine (none) | on | off
  With no arg, returns the machine setting as "on" or "off". Otherwise,
  sends a machine on or off command.

  emc_mode (none) | manual | auto | mdi
  With no arg, returns the mode setting as "manual", "auto", or "mdi".
  Otherwise, sends a mode manual, auto, or mdi command.

  emc_mist (none) | on | off
  With no arg, returns the mist setting as "on" or "off". Otherwise,
  sends a mist on or off command.

  emc_flood (none) | on | off
  With no arg, returns the flood setting as "on" or "off". Otherwise,
  sends a flood on or off command.

  emc_lube (none) | on | off
  With no arg, returns the lubricant pump setting as "on" or "off".
  Otherwise, sends a lube on or off command.

  emc_lube_level
  Returns the lubricant level sensor reading as "ok" or "low".

  emc_spindle (none) | forward | reverse | increase | decrease | constant | off
  With no arg, returns the value of the spindle state as "forward",
  "reverse", "increase", "decrease", or "off". With arg, sends the spindle
  command. Note that "increase" and "decrease" will cause a speed change in
  the corresponding direction until a "constant" command is sent.

  emc_brake (none) | on | off
  With no arg, returns the brake setting. Otherwise sets the brake.

  emc_tool
  Returns the id of the currently loaded tool

  emc_tool_offset
  Returns the currently applied tool length offset

  emc_load_tool_table <file>
  Loads the tool table specified by <file>

  emc_home 0 | 1 | 2 | ...
  Homes the indicated axis.

  emc_jog_stop 0 | 1 | 2 | ...
  Stop the axis jog

  emc_jog 0 | 1 | 2 | ... <speed>
  Jog the indicated axis at <speed>; sign of speed is direction

  emc_jog_incr 0 | 1 | 2 | ... <speed> <incr>
  Jog the indicated axis by increment <incr> at the <speed>; sign of
  speed is direction

  emc_feed_override {<percent>}
  With no args, returns the current feed override, as a percent. With
  argument, set the feed override to be the percent value

  emc_abs_cmd_pos 0 | 1 | ...
  Returns double obj containing the XYZ-SXYZ commanded pos in abs coords,
  at given index, 0 = X, etc.

  emc_abs_act_pos
  Returns double objs containing the XYZ-SXYZ actual pos in abs coords

  emc_rel_cmd_pos 0 | 1 | ...
  Returns double obj containing the XYZ-SXYZ commanded pos in rel coords,
  at given index, 0 = X, etc., including tool length offset

  emc_rel_act_pos
  Returns double objs containing the XYZ-SXYZ actual pos in rel coords,
  including tool length offset

  emc_joint_pos
  Returns double objs containing the actual pos in absolute coords of individual
  joint/slider positions, excludes tool length offset

  emc_pos_offset X | Y | Z | R | P | W
  Returns the position offset associated with the world coordinate provided

  emc_joint_limit 0 | 1 | ...
  Returns "ok", "minsoft", "minhard", "maxsoft", "maxhard"

  emc_joint_fault 0 | 1 | ...
  Returns "ok" or "fault"

  emc_joint_homed 0 | 1 | ...
  Returns "homed", "not"

  emc_mdi <string>
  Sends the <string> as an MDI command

  emc_task_plan_init
  Initializes the program interpreter

  emc_open <filename>
  Opens the named file

  emc_run {<start line>}
  Without start line, runs the opened program from the beginning. With
  start line, runs from that line. A start line of -1 runs in verify mode.

  emc_pause
  Pause program execution

  emc_resume
  Resume program execution

  emc_step
  Step the program one line

  emc_program
  Returns the name of the currently opened program, or "none"

  emc_program_line
  Returns the currently executing line of the program

  emc_program_status
  Returns "idle", "running", or "paused"

  emc_program_codes
  Returns the string for the currently active program codes

  emc_joint_type <joint>
  Returns "linear", "angular", or "custom" for the type of the specified joint

  emc_joint_units <joint>
  Returns "inch", "mm", "cm", or "deg", "rad", "grad", or "custom",
  for the corresponding native units of the specified axis. The type
  of the axis (linear or angular) is used to resolve which type of units
  are returned. The units are obtained heuristically, based on the
  EMC_AXIS_STAT::units numerical value of user units per mm or deg.
  For linear joints, something close to 0.03937 is deemed "inch",
  1.000 is "mm", 0.1 is "cm", otherwise it's "custom".
  For angular joints, something close to 1.000 is deemed "deg",
  PI/180 is "rad", 100/90 is "grad", otherwise it's "custom".
 
  emc_program_units
  emc_program_linear_units
  Returns "inch", "mm", "cm", or "none", for the corresponding linear 
  units that are active in the program interpreter.

  emc_program_angular_units
  Returns "deg", "rad", "grad", or "none" for the corresponding angular
  units that are active in the program interpreter.

  emc_user_linear_units
  Returns "inch", "mm", "cm", or "custom", for the
  corresponding native user linear units of the EMC trajectory
  level. This is obtained heuristically, based on the
  EMC_TRAJ_STAT::linearUnits numerical value of user units per mm.
  Something close to 0.03937 is deemed "inch", 1.000 is "mm", 0.1 is
  "cm", otherwise it's "custom".

  emc_user_angular_units
  Returns "deg", "rad", "grad", or "custom" for the corresponding native
  user angular units of the EMC trajectory level. Like with linear units,
  this is obtained heuristically.

  emc_display_linear_units
  emc_display_angular_units
  Returns "inch", "mm", "cm", or "deg", "rad", "grad", or "custom",
  for the linear or angular units that are active in the display. 
  This is effectively the value of linearUnitConversion or
  angularUnitConversion, resp.

  emc_linear_unit_conversion {inch | mm | cm | auto}
  With no args, returns the unit conversion active. With arg, sets the
  units to be displayed. If it's "auto", the units to be displayed match
  the program units.
 
  emc_angular_unit_conversion {deg | rad | grad | auto}
  With no args, returns the unit conversion active. With arg, sets the
  units to be displayed. If it's "auto", the units to be displayed match
  the program units.

  emc_probe_index
  Which wire is the probe on or which bit of digital IO to use? (No args
  gets it, one arg sets it.)

  emc_probe_polarity
  Value to look for for probe tripped? (0 args gets it, one arg sets it.)

  emc_probe_clear
  Clear the probe tripped flag.

  emc_probe_tripped
  Has the probe been tripped since the last clear.

  emc_probe_value
  Value of current probe signal. (read-only)

  emc_probe
  Move toward a certain location. If the probe is tripped on the way stop
  motion, record the position and raise the probe tripped flag.

  emc_teleop_enable
  Should motion run in teleop mode? (No args
  gets it, one arg sets it.)

  emc_kinematics_type
  returns the type of kinematics functions used identity=1, serial=2,
  parallel=3, custom=4
*/

// the NML channels to the EMC task
static RCS_CMD_CHANNEL *emcCommandBuffer = 0;
static RCS_STAT_CHANNEL *emcStatusBuffer = 0;
EMC_STAT *emcStatus = 0;

// the NML channel for errors
static NML *emcErrorBuffer = 0;
static char error_string[LINELEN] = "";
static char operator_text_string[LINELEN] = "";
static char operator_display_string[LINELEN] = "";

// the current command numbers, set up updateStatus(), used in main()
static int emcCommandSerialNumber = 0;
static int saveEmcCommandSerialNumber = 0;

// default value for timeout, 0 means wait forever
static double emcTimeout = 0.0;

static enum {
    EMC_WAIT_NONE = 1,
    EMC_WAIT_RECEIVED,
    EMC_WAIT_DONE
} emcWaitType = EMC_WAIT_DONE;

static enum {
    EMC_UPDATE_NONE = 1,
    EMC_UPDATE_AUTO
} emcUpdateType = EMC_UPDATE_AUTO;

static int emcTaskNmlGet()
{
    int retval = 0;

    // try to connect to EMC cmd
    if (emcCommandBuffer == 0) {
	emcCommandBuffer =
	    new RCS_CMD_CHANNEL(emcFormat, "emcCommand", "xemc",
				EMC_NMLFILE);
	if (!emcCommandBuffer->valid()) {
	    delete emcCommandBuffer;
	    emcCommandBuffer = 0;
	    retval = -1;
	}
    }
    // try to connect to EMC status
    if (emcStatusBuffer == 0) {
	emcStatusBuffer =
	    new RCS_STAT_CHANNEL(emcFormat, "emcStatus", "xemc",
				 EMC_NMLFILE);
	if (!emcStatusBuffer->valid()
	    || EMC_STAT_TYPE != emcStatusBuffer->peek()) {
	    delete emcStatusBuffer;
	    emcStatusBuffer = 0;
	    emcStatus = 0;
	    retval = -1;
	} else {
	    emcStatus = (EMC_STAT *) emcStatusBuffer->get_address();
	}
    }

    return retval;
}

static int emcErrorNmlGet()
{
    int retval = 0;

    if (emcErrorBuffer == 0) {
	emcErrorBuffer =
	    new NML(nmlErrorFormat, "emcError", "xemc", EMC_NMLFILE);
	if (!emcErrorBuffer->valid()) {
	    delete emcErrorBuffer;
	    emcErrorBuffer = 0;
	    retval = -1;
	}
    }

    return retval;
}

static int tryNml()
{
    double end;
    int good;
#define RETRY_TIME 10.0		// seconds to wait for subsystems to come up
#define RETRY_INTERVAL 1.0	// seconds between wait tries for a subsystem

    if (EMC_DEBUG & EMC_DEBUG_NML == 0) {
	set_rcs_print_destination(RCS_PRINT_TO_NULL);	// inhibit diag
	// messages
    }
    end = RETRY_TIME;
    good = 0;
    do {
	if (0 == emcTaskNmlGet()) {
	    good = 1;
	    break;
	}
	esleep(RETRY_INTERVAL);
	end -= RETRY_INTERVAL;
    } while (end > 0.0);
    if (EMC_DEBUG & EMC_DEBUG_NML == 0) {
	set_rcs_print_destination(RCS_PRINT_TO_STDOUT);	// inhibit diag
	// messages
    }
    if (!good) {
	return -1;
    }

    if (EMC_DEBUG & EMC_DEBUG_NML == 0) {
	set_rcs_print_destination(RCS_PRINT_TO_NULL);	// inhibit diag
	// messages
    }
    end = RETRY_TIME;
    good = 0;
    do {
	if (0 == emcErrorNmlGet()) {
	    good = 1;
	    break;
	}
	esleep(RETRY_INTERVAL);
	end -= RETRY_INTERVAL;
    } while (end > 0.0);
    if (EMC_DEBUG & EMC_DEBUG_NML == 0) {
	set_rcs_print_destination(RCS_PRINT_TO_STDOUT);	// inhibit diag
	// messages
    }
    if (!good) {
	return -1;
    }

    return 0;

#undef RETRY_TIME
#undef RETRY_INTERVAL
}

static int updateStatus()
{
    NMLTYPE type;

    if (0 == emcStatus || 0 == emcStatusBuffer
	|| !emcStatusBuffer->valid()) {
	return -1;
    }

    switch (type = emcStatusBuffer->peek()) {
    case -1:
	// error on CMS channel
	return -1;
	break;

    case 0:			// no new data
    case EMC_STAT_TYPE:	// new data
	// new data
	break;

    default:
	return -1;
	break;
    }

    return 0;
}

/*
  updateError() updates "errors," which are true errors and also
  operator display and text messages.
*/
static int updateError()
{
    NMLTYPE type;

    if (0 == emcErrorBuffer || !emcErrorBuffer->valid()) {