iosh.cc

来自「CNC 的开放码,EMC2 V2.2.8版」· CC 代码 · 共 1,639 行 · 第 1/3 页

/********************************************************************
* Description: iosh.cc
*   Extended-Tcl-based general IO programming shell with NML connectivity
*
*   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.23 $
* $Author: cradek $
* $Date: 2007/04/01 04:16:33 $
********************************************************************/
/*! \todo 
  FIXME

  add tool_in_spindle status
*/

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <signal.h>

#include "tcl.h"
#include "tk.h"

#include "rcs.hh"		// etime()
#include "posemath.h"		// PM_POSE
#include "emc.hh"		// EMC NML
#include "emc_nml.hh"
#include "emcglb.h"		// EMC_NMLFILE, TRAJ_MAX_VELOCITY, etc.
#include "emccfg.h"		// DEFAULT_TRAJ_MAX_VELOCITY
#include "inifile.hh"		// INIFILE
#include "rcs_print.hh"
#include "nml_oi.hh"            // nmlErrorFormat
#include "timer.hh"             // esleep
#include "motion.h"		// emc struct and commands
#include "motion_debug.h"
#include "motion_struct.h"
#include "usrmotintf.h"		// usrmot interface

/*
  Using iosh:

  iosh {<script>} {-- -ini <ini file>}

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

  Without filename, it runs interactively.

  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.

  EMC IO commands:

  emc_io_connect
  emc_io_disconnect
  Open or close the NML buffers to the command in, status out, and error out.
  Returns 0 if OK, or -1 if not.

  emc_io_read_command
  Peek the NML command buffer. Returns 0 if OK, -1 if not.

  emc_io_get_command
  Puts the command string, e.g., "emc_aux_estop_off", or "none". Returns 0.

  emc_io_get_command_type
  Puts the command NML number. Returns 0.

  emc_io_get_serial_number
  Puts the command serial number. Returns 0.

  emc_io_write_status
  Write the EMC_IO_STAT structure out to NML. Returns 0 if OK, -1 if error.

  emc_io_write_error
  Write the error string to the error NML buffer. Returns 0 if OK, -1 if error.

  emc_io_load_tool_table
  Loads the tool table specified in the ini file

  IO status, sets associated field in the NML status structure

  emc_io_status_heartbeat <number>
  emc_io_status_echo_serial_number <number>
  emc_io_status_status done | exec | error
  emc_io_status_estop on | off
  emc_io_status_mist on | off
  emc_io_status_flood on | off
  emc_io_status_lube on | off
  emc_io_status_lube_level ok | low
  emc_io_status_tool_prepped <number>
  emc_io_status_tool_in_spindle <number>

//FIXME: The following have been commented out, if there is need they will be made to work again,
    or all the io will go through the motion controller, in which case all the NML needs to get rerouted
  emc_io_status_spindle_speed <speed>
  emc_io_status_spindle_enabled on | off
  emc_io_status_spindle_direction <pos> <neg> 0
  emc_io_status_spindle_increasing <pos> <neg> 0
  emc_io_status_spindle_brake on | off

  IO commands:

  inb <address>
  Reads and returns the byte at <address>. If address begins with 0x,
  it's interpreted as a hex number, otherwise it's decimal.

  outb <address> <value>
  Writes the byte <value> to <address>. If address or value begins with 0x,
  it's interpreted as a hex number, otherwise it's decimal. Returns nothing.

  inw <address>
  Reads and returns the short at <address>. If address begins with 0x,
  it's interpreted as a hex number, otherwise it's decimal.

  outw <address> <value>
  Writes the short <value> to <address>. If address or value begins with 0x,
  it's interpreted as a hex number, otherwise it's decimal. Returns nothing.

  inl <address>
  Reads and returns the long at <address>. If address begins with 0x,
  it's interpreted as a hex number, otherwise it's decimal.

  outl <address> <value>
  Writes the long <value> to <address>. If address or value begins with 0x,
  it's interpreted as a hex number, otherwise it's decimal. Returns nothing.

  emc_mot_move <axis> <position> <velocity>
    
  emc_mot_rawinput <axis>
    
  emc_mot_shmem: need no args
*/

// the NML channels to the EMC task
static RCS_CMD_CHANNEL *emcioCommandBuffer = 0;
// NML command channel data pointer
static RCS_CMD_MSG *emcioCommand = 0;
static RCS_STAT_CHANNEL *emcioStatusBuffer = 0;
static EMC_IO_STAT emcioStatus;

// the NML channel for errors
static NML *emcErrorBuffer = 0;

// Shared memory to communicate with emcmot
static emcmot_command_t emcmotCommand;
extern emcmot_struct_t *emcmotshmem;
static long shmem = 0;		// Shared memory flag

// "defined but not used"...
//static int motionId = 0;

static int emcIoNmlGet()
{
    int retval = 0;

    // try to connect to EMC IO cmd
    if (emcioCommandBuffer == 0) {
	emcioCommandBuffer =
	    new RCS_CMD_CHANNEL(emcFormat, "toolCmd", "tool", EMC_NMLFILE);
	if (!emcioCommandBuffer->valid()) {
	    rcs_print_error("emcToolCmd buffer not available\n");
	    delete emcioCommandBuffer;
	    emcioCommandBuffer = 0;
	    retval = -1;
	} else {
	    // get our command data structure
	    emcioCommand = emcioCommandBuffer->get_address();
	}
    }
    // try to connect to EMC IO status
    if (emcioStatusBuffer == 0) {
	emcioStatusBuffer =
	    new RCS_STAT_CHANNEL(emcFormat, "toolSts", "tool",
				 EMC_NMLFILE);
	if (!emcioStatusBuffer->valid()) {
	    rcs_print_error("toolSts buffer not available\n");
	    delete emcioStatusBuffer;
	    emcioStatusBuffer = 0;
	    retval = -1;
	} else {
	    // initialize and write status
	    emcioStatus.heartbeat = 0;
	    emcioStatus.command_type = 0;
	    emcioStatus.echo_serial_number = 0;
	    emcioStatus.status = RCS_DONE;
	    emcioStatusBuffer->write(&emcioStatus);
	}
    }

    return retval;
}

static int emcErrorNmlGet()
{
    int retval = 0;

    if (emcErrorBuffer == 0) {
	emcErrorBuffer =
	    new NML(nmlErrorFormat, "emcError", "tool", EMC_NMLFILE);
	if (!emcErrorBuffer->valid()) {
	    rcs_print_error("emcError buffer not available\n");
	    delete emcErrorBuffer;
	    emcErrorBuffer = 0;
	    retval = -1;
	}
    }

    return retval;
}

// EMC IO commands

static int emc_ini(ClientData clientdata,
		   Tcl_Interp * interp, int objc, Tcl_Obj * CONST objv[])
{
    IniFile inifile;
    const char *inistring;
    const char *varstr, *secstr;

    if (objc != 3) {
	Tcl_SetResult(interp, "emc_ini: need 'var' and 'section'",
		      TCL_VOLATILE);
	return TCL_ERROR;
    }
    // open it
    if (inifile.Open(EMC_INIFILE) == false) {
	return -1;
    }

    varstr = Tcl_GetStringFromObj(objv[1], 0);
    secstr = Tcl_GetStringFromObj(objv[2], 0);

    if (NULL == (inistring = inifile.Find(varstr, secstr))) {
	return TCL_OK;
    }

    Tcl_SetResult(interp, (char *) inistring, TCL_VOLATILE);

    // close it
    inifile.Close();

    return TCL_OK;
}

static int emc_io_connect(ClientData clientdata,
			  Tcl_Interp * interp, int objc,
			  Tcl_Obj * CONST objv[])
{
    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 (objc != 1) {
	Tcl_SetResult(interp, "emc_io_connect: need no args\n",
		      TCL_VOLATILE);
	return TCL_ERROR;
    }
    if (!(EMC_DEBUG & EMC_DEBUG_NML)) {
	set_rcs_print_destination(RCS_PRINT_TO_NULL);	// inhibit diag
	// messages
    }
    end = RETRY_TIME;
    good = 0;
    do {
	if (0 == emcIoNmlGet()) {
	    good = 1;
	    break;
	}
	esleep(RETRY_INTERVAL);
	end -= RETRY_INTERVAL;
    } while (end > 0.0);

    if (!(EMC_DEBUG & EMC_DEBUG_NML)) {
	set_rcs_print_destination(RCS_PRINT_TO_STDOUT);	// restore diag
	// messages
    }

    if (!good) {
	Tcl_SetObjResult(interp, Tcl_NewIntObj(-1));
	return TCL_OK;
    }

    if (!(EMC_DEBUG & EMC_DEBUG_NML)) {
	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)) {
	set_rcs_print_destination(RCS_PRINT_TO_STDOUT);	// restore diag
	// messages
    }
    if (!good) {
	Tcl_SetObjResult(interp, Tcl_NewIntObj(-1));
	return TCL_OK;
    }

    Tcl_SetObjResult(interp, Tcl_NewIntObj(0));
    return TCL_OK;
}

static int emc_io_disconnect(ClientData clientdata,
			     Tcl_Interp * interp, int objc,
			     Tcl_Obj * CONST objv[])
{
    if (objc != 1) {
	Tcl_SetResult(interp, "emc_io_disconnect: need no args\n",
		      TCL_VOLATILE);
	return TCL_ERROR;
    }

    if (emcErrorBuffer != 0) {
	delete emcErrorBuffer;
	emcErrorBuffer = 0;
    }

    if (emcioStatusBuffer != 0) {
	delete emcioStatusBuffer;
	emcioStatusBuffer = 0;
    }

    if (emcioCommandBuffer != 0) {
	delete emcioCommandBuffer;
	emcioCommandBuffer = 0;
    }

    Tcl_SetObjResult(interp, Tcl_NewIntObj(0));
    return TCL_OK;
}

/*
  emc_io_read_command peeks the NML command buffer. The side effect is
  that emcioCommand now latches the NML contents. Other functions, like
  emc_io_get_command,serial_number, get the contents from emcioCommand.

  Returns 0 if OK, -1 if error reading NML.
*/
static int emc_io_read_command(ClientData clientdata,
			       Tcl_Interp * interp, int objc,
			       Tcl_Obj * CONST objv[])
{
    if (objc != 1) {
	Tcl_SetResult(interp, "emc_io_read_command: need no args\n",
		      TCL_VOLATILE);
	return TCL_ERROR;
    }
    // read NML buffer
    if (0 == emcioCommandBuffer || 0 == emcioCommand) {
	Tcl_SetObjResult(interp, Tcl_NewIntObj(-1));
	return TCL_OK;
    }
    // latch new command into emcioCommand
    if (-1 == emcioCommandBuffer->peek()) {
	Tcl_SetObjResult(interp, Tcl_NewIntObj(-1));
	return TCL_OK;
    }

    Tcl_SetObjResult(interp, Tcl_NewIntObj(0));
    return TCL_OK;
}



/*
  load the tool table from file filename into toolTable[] array.
  Array is CANON_TOOL_MAX + 1 entries, since 0 is included.

  If filename is "", use global established from ini file
  */
static int loadToolTable(const char *filename,
			 CANON_TOOL_TABLE toolTable[])
{
    int t;
    FILE *fp;
    char buffer[CANON_TOOL_ENTRY_LEN];
    const char *name;

    // check filename
    if (filename[0] == 0) {
	name = TOOL_TABLE_FILE;
    } else {
	// point to name provided
	name = filename;
    }

    // open tool table file
    if (NULL == (fp = fopen(name, "r"))) {
	// can't open file
	return -1;
    }
    // clear out tool table
    for (t = 0; t <= CANON_TOOL_MAX; t++) {
	// unused tools are 0, 0.0, 0.0
	toolTable[t].id = 0;
	toolTable[t].xoffset = 0.0;
	toolTable[t].zoffset = 0.0;
	toolTable[t].diameter = 0.0;
        toolTable[t].frontangle = 0.0;
        toolTable[t].backangle = 0.0;
        toolTable[t].orientation = 0;
    }

    /*
       Override 0's with codes from tool file

       File format is:

       <header>
       <pocket # 0..CANON_TOOL_MAX> <FMS id> <length> <diameter>
       ...

     */

    // read and discard header
    if (NULL == fgets(buffer, 256, fp)) {
	// nothing in file at all
	fclose(fp);
	return -1;
    }

    while (!feof(fp)) {
	int pocket;
	int id;
	double zoffset;
        double xoffset;
	double diameter;
        double frontangle, backangle;
        int orientation;

	// just read pocket, ID, and length offset
	if (NULL == fgets(buffer, CANON_TOOL_ENTRY_LEN, fp)) {
	    break;
	}

        if (sscanf(buffer, "%d %d %lf %lf %lf %lf %lf %d", 
                   &pocket, &id, &zoffset, &xoffset, &diameter, 
                   &frontangle, &backangle, &orientation) == 8) {
	    if (pocket < 0 || pocket > CANON_TOOL_MAX) {
                printf("skipping tool: bad pocket number %d\n", pocket);
		continue;
	    } else {
		/* lathe tool */
		toolTable[pocket].id = id;
                toolTable[pocket].xoffset = xoffset;
		toolTable[pocket].zoffset = zoffset;
		toolTable[pocket].diameter = diameter;

                toolTable[pocket].frontangle = frontangle;
                toolTable[pocket].backangle = backangle;
                toolTable[pocket].orientation = orientation;
	    }
	} else if (sscanf(buffer, "%d %d %lf %lf", 
                   &pocket, &id, &zoffset, &diameter) == 4) {
	    if (pocket < 0 || pocket > CANON_TOOL_MAX) {
                printf("skipping tool: bad pocket number %d\n", pocket);
		continue;
	    } else {
                /* mill tool */
		toolTable[pocket].id = id;
		toolTable[pocket].zoffset = zoffset;
		toolTable[pocket].diameter = diameter;

                toolTable[pocket].frontangle = toolTable[pocket].backangle = 0.0;
                toolTable[pocket].xoffset = 0.0;
                toolTable[pocket].orientation = 0;
	    }
	} else {
            /* invalid line. skip it silently */
	    continue;
	}
    }

    // close the file
    fclose(fp);

    return 0;
}

/*
  save the tool table to file filename from toolTable[] array.
  Array is CANON_TOOL_MAX + 1 entries, since 0 is included.

  If filename is "", use global established from ini file
  */

/* FIXME - commented out, seems it's not used
static int saveToolTable(const char *filename,
			 CANON_TOOL_TABLE toolTable[])
{
    int pocket;
    FILE *fp;
    const char *name;

    // check filename
    if (filename[0] == 0) {
	name = TOOL_TABLE_FILE;
    } else {
	// point to name provided
	name = filename;
    }

    // open tool table file
    if (NULL == (fp = fopen(name, "w"))) {
	// can't open file
	return -1;
    }
    // write header
    fprintf(fp, "POC\tFMS\tLEN\t\tDIAM\n");

    for (pocket = 1; pocket <= CANON_TOOL_MAX; pocket++) {
	fprintf(fp, "%d\t%d\t%f\t%f\n",
		pocket,
		toolTable[pocket].id,
		toolTable[pocket].length, toolTable[pocket].diameter);
    }

    // close the file
    fclose(fp);

    return 0;
}
FIXME - commented out, seems it's not used*/

/*
  emc_io_get_command returns the string and any args associated with
  the command, or "none" if nothing is there.