emcmodule.cc

CNC 的开放码,EMC2 V2.2.8版

//    This is a component of AXIS, a front-end for emc
//    Copyright 2004, 2005, 2006 Jeff Epler <[e-mail]> and 
//    Chris Radek <[e-mail]>
//
//    This program is free software; you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation; either version 2 of the License, or
//    (at your option) any later version.
//
//    This program is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License
//    along with this program; if not, write to the Free Software
//    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

#include <Python.h>
#include <structseq.h>
#include <pthread.h>
#include <structmember.h>
#include "config.h"
#include "rcs.hh"
#include "emc.hh"
#include "emc_nml.hh"
#include "kinematics.h"
#include "config.h"
#include "inifile.hh"
#include "timer.hh"
#include "nml_oi.hh"
#include "rcs_print.hh"

#include <cmath>

// The C++ standard probably doesn't specify the amount of storage for a 'bool',
// and on some systems it might be more than one byte.  However, on x86 and
// x86-64, sizeof(bool) == 1.  When a counterexample is found, this must be
// replaced with the result of a configure test.
#define T_BOOL T_UBYTE

#define NUM_AXES (9)

#define LOCAL_SPINDLE_FORWARD (1)
#define LOCAL_SPINDLE_REVERSE (-1)
#define LOCAL_SPINDLE_OFF (0)
#define LOCAL_SPINDLE_INCREASE (10)
#define LOCAL_SPINDLE_DECREASE (11)
#define LOCAL_SPINDLE_CONSTANT (12)

#define LOCAL_MIST_ON (1)
#define LOCAL_MIST_OFF (0)

#define LOCAL_FLOOD_ON (1)
#define LOCAL_FLOOD_OFF (0)

#define LOCAL_BRAKE_ENGAGE (1)
#define LOCAL_BRAKE_RELEASE (0)

#define LOCAL_JOG_STOP (0)
#define LOCAL_JOG_CONTINUOUS (1)
#define LOCAL_JOG_INCREMENT (2)

#define LOCAL_AUTO_RUN (0)
#define LOCAL_AUTO_PAUSE (1)
#define LOCAL_AUTO_RESUME (2)
#define LOCAL_AUTO_STEP (3)

/* This definition of offsetof avoids the g++ warning
 * 'invalid offsetof from non-POD type'.
 */
#undef offsetof
#define offsetof(T,x) (size_t)(-1+(char*)&(((T*)1)->x))

struct pyIniFile {
    PyObject_HEAD
    IniFile *i;
};

struct pyStatChannel {
    PyObject_HEAD
    RCS_STAT_CHANNEL *c;
    EMC_STAT status;
};

struct pyCommandChannel {
    PyObject_HEAD
    RCS_CMD_CHANNEL *c;
    RCS_STAT_CHANNEL *s;
    int serial;
};

struct pyErrorChannel {
    PyObject_HEAD
    NML *c;
};

static PyObject *m = NULL, *error = NULL;

static int Ini_init(pyIniFile *self, PyObject *a, PyObject *k) {
    char *inifile;
    if(!PyArg_ParseTuple(a, "s", &inifile)) return -1;

    self->i = new IniFile();

    if (!self->i->Open(inifile)) {
        PyErr_Format( error, "inifile.open() failed");
        return -1;
    }
    return 0;
}

static PyObject *Ini_find(pyIniFile *self, PyObject *args) {
    const char *s1, *s2, *out;
    int num = 1; 
    if(!PyArg_ParseTuple(args, "ss|i:find", &s1, &s2, &num)) return NULL;
    
    out = self->i->Find(s2, s1, num);
    if(out == NULL) {
        Py_INCREF(Py_None);
        return Py_None;
    }
    return PyString_FromString(const_cast<char*>(out));
}

static PyObject *Ini_findall(pyIniFile *self, PyObject *args) {
    const char *s1, *s2, *out;
    int num = 1; 
    if(!PyArg_ParseTuple(args, "ss:findall", &s1, &s2)) return NULL;
    
    PyObject *result = PyList_New(0);
    while(1) {
        out = self->i->Find(s2, s1, num);
        if(out == NULL) {
            break;
        }
        PyList_Append(result, PyString_FromString(const_cast<char*>(out)));
        num++;
    }
    return result;
}

static void Ini_dealloc(pyIniFile *self) {
    self->i->Close();
    delete self->i;
    PyObject_Del(self);
}

static PyMethodDef Ini_methods[] = {
    {"find", (PyCFunction)Ini_find, METH_VARARGS,
        "Find value in inifile as string.  This uses the ConfigParser-style "
        "(section,option) order, not the emc order."},
    {"findall", (PyCFunction)Ini_findall, METH_VARARGS,
        "Find value in inifile as a list.  This uses the ConfigParser-style "
        "(section,option) order, not the emc order."},
    {NULL}
};

PyTypeObject Ini_Type = {
    PyObject_HEAD_INIT(NULL)
    0,                      /*ob_size*/
    "emc.ini",              /*tp_name*/
    sizeof(pyIniFile),      /*tp_basicsize*/
    0,                      /*tp_itemsize*/
    /* methods */
    (destructor)Ini_dealloc,/*tp_dealloc*/
    0,                      /*tp_print*/
    0,                      /*tp_getattr*/
    0,                      /*tp_setattr*/
    0,                      /*tp_compare*/
    0,                      /*tp_repr*/
    0,                      /*tp_as_number*/
    0,                      /*tp_as_sequence*/
    0,                      /*tp_as_mapping*/
    0,                      /*tp_hash*/
    0,                      /*tp_call*/
    0,                      /*tp_str*/
    0,                      /*tp_getattro*/
    0,                      /*tp_setattro*/
    0,                      /*tp_as_buffer*/
    Py_TPFLAGS_DEFAULT,     /*tp_flags*/
    0,                      /*tp_doc*/
    0,                      /*tp_traverse*/
    0,                      /*tp_clear*/
    0,                      /*tp_richcompare*/
    0,                      /*tp_weaklistoffset*/
    0,                      /*tp_iter*/
    0,                      /*tp_iternext*/
    Ini_methods,            /*tp_methods*/
    0,                      /*tp_members*/
    0,                      /*tp_getset*/
    0,                      /*tp_base*/
    0,                      /*tp_dict*/
    0,                      /*tp_descr_get*/
    0,                      /*tp_descr_set*/
    0,                      /*tp_dictoffset*/
    (initproc)Ini_init,     /*tp_init*/
    0,                      /*tp_alloc*/
    PyType_GenericNew,      /*tp_new*/
    0,                      /*tp_free*/
    0,                      /*tp_is_gc*/
};


#define EMC_COMMAND_TIMEOUT 2.0  // how long to wait until timeout
#define EMC_COMMAND_DELAY   0.01 // how long to sleep between checks

static int emcWaitCommandComplete(int serial_number, RCS_STAT_CHANNEL *s) {
    double start = etime();

    while (etime() - start < EMC_COMMAND_TIMEOUT) {
        if(s->peek() == EMC_STAT_TYPE) {
           EMC_STAT *stat = (EMC_STAT*)s->get_address();
//           printf("WaitComplete: %d %d %d\n", serial_number, stat->echo_serial_number, stat->status);
           if (stat->echo_serial_number == serial_number &&
               ( stat->status == RCS_DONE || stat->status == RCS_ERROR )) {
                return s->get_address()->status;
           }
        }
        esleep(EMC_COMMAND_DELAY);
    }
    return -1;
}


static void emcWaitCommandReceived(int serial_number, RCS_STAT_CHANNEL *s) {
    double start = etime();

    while (etime() - start < EMC_COMMAND_TIMEOUT) {
        if(s->peek() == EMC_STAT_TYPE &&
           s->get_address()->echo_serial_number == serial_number) {
                return;
           }
        esleep(EMC_COMMAND_DELAY);
    }
}

static int next_serial(pyCommandChannel *c) {
    return ++c->serial;
}

static char *get_nmlfile(void) {
    PyObject *fileobj = PyObject_GetAttrString(m, "nmlfile");
    if(fileobj == NULL) return NULL;
    return PyString_AsString(fileobj);
}

static int Stat_init(pyStatChannel *self, PyObject *a, PyObject *k) {
    char *file = get_nmlfile();
    if(file == NULL) return -1;

    RCS_STAT_CHANNEL *c =
        new RCS_STAT_CHANNEL(emcFormat, "emcStatus", "xemc", file);
    if(!c) {
        PyErr_Format( error, "new RCS_STAT_CHANNEL failed");
        return -1;
    }

    self->c = c;
    return 0;
}

static void Stat_dealloc(PyObject *self) {
    delete ((pyStatChannel*)self)->c;
    PyObject_Del(self);
}

static bool check_stat(RCS_STAT_CHANNEL *emcStatusBuffer) {
    if(!emcStatusBuffer->valid()) { 
        PyErr_Format( error, "emcStatusBuffer invalid" );
        return false;
    }
    return true;
}

static bool wait_stat(RCS_STAT_CHANNEL *emcStatusBuffer) {
    if(!emcStatusBuffer->valid()) { 
        PyErr_Format( error, "emcStatusBuffer invalid" );
        return false;
    }
    while(emcStatusBuffer->peek() != EMC_STAT_TYPE) {
        usleep(10000); 
    }
    return true;
}

static PyObject *gettaskfile(pyStatChannel *s, PyObject *o) {
    if(!wait_stat(s->c)) return NULL;
    EMC_STAT *emcStatus = static_cast<EMC_STAT*>(s->c->get_address());
    return PyString_FromString(emcStatus->task.file);
}

static PyObject *poll(pyStatChannel *s, PyObject *o) {
    if(!check_stat(s->c)) return NULL;
    if(s->c->peek() == EMC_STAT_TYPE) {
        EMC_STAT *emcStatus = static_cast<EMC_STAT*>(s->c->get_address());
        memcpy(&s->status, emcStatus, sizeof(EMC_STAT));
    }
    Py_INCREF(Py_None);
    return Py_None;
}

static PyMethodDef Stat_methods[] = {
    {"poll", (PyCFunction)poll, METH_NOARGS, "Update current machine state"},
    {"gettaskfile", (PyCFunction)gettaskfile, METH_NOARGS,
                                             "Get current task file"},
    {NULL}
};


#define O(x) offsetof(pyStatChannel,status.x)
#define POS(prefix, member) \
    {prefix "x", T_DOUBLE, O(member.tran.x), READONLY}, \
    {prefix "y", T_DOUBLE, O(member.tran.y), READONLY}, \
    {prefix "z", T_DOUBLE, O(member.tran.z), READONLY}, \
    {prefix "a", T_DOUBLE, O(member.a), READONLY}, \
    {prefix "b", T_DOUBLE, O(member.b), READONLY}, \
    {prefix "c", T_DOUBLE, O(member.c), READONLY}
static PyMemberDef Stat_members[] = {
// stat 
    {"echo_serial_number", T_INT, O(echo_serial_number), READONLY},
    {"state", T_INT, O(status), READONLY},
    {"line", T_INT, O(line), READONLY},
    {"source_line", T_INT, O(line), READONLY},
    {"source_file", T_STRING_INPLACE, O(source_file), READONLY},

// task
    {"task_mode", T_INT, O(task.mode), READONLY},
    {"task_state", T_INT, O(task.state), READONLY},
    {"exec_state", T_INT, O(task.execState), READONLY},
    {"interp_state", T_INT, O(task.interpState), READONLY},
    {"read_line", T_INT, O(task.readLine), READONLY},
    {"motion_line", T_INT, O(task.motionLine), READONLY},
    {"current_line", T_INT, O(task.currentLine), READONLY},
    {"file", T_STRING_INPLACE, O(task.file), READONLY},
    {"command", T_STRING_INPLACE, O(task.command), READONLY},
    {"program_units", T_INT, O(task.programUnits), READONLY},
    {"interpreter_errcode", T_INT, O(task.interpreter_errcode), READONLY},
    {"optional_stop", T_BOOL, O(task.optional_stop_state), READONLY},
    {"block_delete", T_BOOL, O(task.block_delete_state), READONLY},

// motion
//   EMC_TRAJ_STAT traj
    {"linear_units", T_DOUBLE, O(motion.traj.linearUnits), READONLY},
    {"angular_units", T_DOUBLE, O(motion.traj.angularUnits), READONLY},
    {"cycle_time", T_DOUBLE, O(motion.traj.cycleTime), READONLY},
    {"axes", T_INT, O(motion.traj.axes), READONLY},
    {"axis_mask", T_INT, O(motion.traj.axis_mask), READONLY},
    {"motion_mode", T_INT, O(motion.traj.mode), READONLY},
    {"enabled", T_INT, O(motion.traj.enabled), READONLY},
    {"inpos", T_INT, O(motion.traj.inpos), READONLY},
    {"queue", T_INT, O(motion.traj.queue), READONLY},
    {"id", T_INT, O(motion.traj.id), READONLY},
    {"paused", T_INT, O(motion.traj.paused), READONLY},
    {"feedrate", T_DOUBLE, O(motion.traj.scale), READONLY},
    {"spindlerate", T_DOUBLE, O(motion.traj.spindle_scale), READONLY},
    
    {"velocity", T_DOUBLE, O(motion.traj.velocity), READONLY},
    {"acceleration", T_DOUBLE, O(motion.traj.acceleration), READONLY},
    {"max_velocity", T_DOUBLE, O(motion.traj.maxVelocity), READONLY},
    {"max_acceleration", T_DOUBLE, O(motion.traj.maxAcceleration), READONLY},
    {"probe_index", T_INT, O(motion.traj.probe_index), READONLY},
    {"probe_polarity", T_INT, O(motion.traj.probe_polarity), READONLY},
    {"probe_tripped", T_INT, O(motion.traj.probe_tripped), READONLY},
    {"probing", T_INT, O(motion.traj.probing), READONLY},
    {"probe_val", T_INT, O(motion.traj.probeval), READONLY},
    {"kinematics_type", T_INT, O(motion.traj.kinematics_type), READONLY},
    {"motion_type", T_INT, O(motion.traj.motion_type), READONLY},
    {"distance_to_go", T_DOUBLE, O(motion.traj.distance_to_go), READONLY},
    {"current_vel", T_DOUBLE, O(motion.traj.current_vel), READONLY},

// io
// EMC_TOOL_STAT io.tool
    {"tool_prepped", T_INT, O(io.tool.toolPrepped), READONLY},
    {"tool_in_spindle", T_INT, O(io.tool.toolInSpindle), READONLY},
    

// EMC_SPINDLE_STAT motion.spindle
    {"spindle_speed", T_DOUBLE, O(motion.spindle.speed), READONLY},
    {"spindle_direction", T_INT, O(motion.spindle.direction), READONLY},
    {"spindle_brake", T_INT, O(motion.spindle.brake), READONLY},
    {"spindle_increasing", T_INT, O(motion.spindle.increasing), READONLY},
    {"spindle_enabled", T_INT, O(motion.spindle.enabled), READONLY},

// EMC_COOLANT_STAT io.cooland
    {"mist", T_INT, O(io.coolant.mist), READONLY},
    {"flood", T_INT, O(io.coolant.flood), READONLY},

// EMC_AUX_STAT     io.aux
    {"estop", T_INT, O(io.aux.estop), READONLY},

// EMC_LUBE_STAT    io.lube
    {"lube", T_INT, O(io.lube.on), READONLY},
    {"lube_level", T_INT, O(io.lube.level), READONLY},

    {"debug", T_INT, O(debug), READONLY},
    {NULL}
};

static PyObject *uchar_array(unsigned char *arr, int sz) {
    PyObject *res = PyTuple_New(sz);
    for(int i = 0; i < sz; i++) {
        PyTuple_SET_ITEM(res, i, PyInt_FromLong(arr[i]));
    }
    return res;
}

static PyObject *int_array(int *arr, int sz) {
    PyObject *res = PyTuple_New(sz);
    for(int i = 0; i < sz; i++) {
        PyTuple_SET_ITEM(res, i, PyInt_FromLong(arr[i]));
    }
    return res;
}

static PyObject *double_array(double *arr, int sz) {
    PyObject *res = PyTuple_New(sz);
    for(int i = 0; i < sz; i++) {
        PyTuple_SET_ITEM(res, i, PyFloat_FromDouble(arr[i]));
    }
    return res;
}

static PyObject *pose(const EmcPose &p) {
    PyObject *res = PyTuple_New(9);
    PyTuple_SET_ITEM(res, 0, PyFloat_FromDouble(p.tran.x));
    PyTuple_SET_ITEM(res, 1, PyFloat_FromDouble(p.tran.y));
    PyTuple_SET_ITEM(res, 2, PyFloat_FromDouble(p.tran.z));
    PyTuple_SET_ITEM(res, 3, PyFloat_FromDouble(p.a));
    PyTuple_SET_ITEM(res, 4, PyFloat_FromDouble(p.b));
    PyTuple_SET_ITEM(res, 5, PyFloat_FromDouble(p.c));
    PyTuple_SET_ITEM(res, 6, PyFloat_FromDouble(p.u));
    PyTuple_SET_ITEM(res, 7, PyFloat_FromDouble(p.v));
    PyTuple_SET_ITEM(res, 8, PyFloat_FromDouble(p.w));
    return res;
}

static PyObject *Stat_origin(pyStatChannel *s) {
    return pose(s->status.task.origin);
}

static PyObject *Stat_tool_offset(pyStatChannel *s) {
    return pose(s->status.task.toolOffset);
}

static PyObject *Stat_position(pyStatChannel *s) {
    return pose(s->status.motion.traj.position);
}

static PyObject *Stat_actual(pyStatChannel *s) {
    return pose(s->status.motion.traj.actualPosition);