emccanon.cc

Source code for an Numeric Cmputer

    spindleOn = 1;
}

void START_SPINDLE_COUNTERCLOCKWISE()
{
    EMC_SPINDLE_ON emc_spindle_on_msg;

    if (spindleSpeed == 0)
	CANON_ERROR("Spindle speed needs to be non-zero in order to enable.\nIf speed is 0 we have no way of telling that you really wanted counterclockwise.");

    emc_spindle_on_msg.speed = -spindleSpeed;

    interp_list.append(emc_spindle_on_msg);

    spindleOn = -1;
}

void SET_SPINDLE_SPEED(double r)
{
    // speed is in RPMs everywhere
    spindleSpeed = r;

    // check if we need to resend command
    if (spindleOn == 1) {
	START_SPINDLE_CLOCKWISE();
    } else if (spindleOn == -1) {
	START_SPINDLE_COUNTERCLOCKWISE();
    }
}

void STOP_SPINDLE_TURNING()
{
    EMC_SPINDLE_OFF emc_spindle_off_msg;

    interp_list.append(emc_spindle_off_msg);

    spindleOn = 0;
}

void SPINDLE_RETRACT()
{
    /*! \todo FIXME-- unimplemented */
}

void ORIENT_SPINDLE(double orientation, CANON_DIRECTION direction)
{
    /*! \todo FIXME-- unimplemented */
}

void USE_SPINDLE_FORCE(void)
{
    /*! \todo FIXME-- unimplemented */
}

void LOCK_SPINDLE_Z(void)
{
    /*! \todo FIXME-- unimplemented */
}

void USE_NO_SPINDLE_FORCE(void)
{
    /*! \todo FIXME-- unimplemented */
}

/* Tool Functions */

/*
  EMC has no tool length offset. To implement it, we save it here,
  and apply it when necessary
  */
void USE_TOOL_LENGTH_OFFSET(double length)
{
    EMC_TRAJ_SET_OFFSET set_offset_msg;

    /* convert to mm units for internal canonical use */
    currentToolLengthOffset = FROM_PROG_LEN(length);

    /* append it to interp list so it gets updated at the right time, not at
       read-ahead time */
    set_offset_msg.offset.tran.x = 0.0;
    set_offset_msg.offset.tran.y = 0.0;
    set_offset_msg.offset.tran.z = TO_EXT_LEN(currentToolLengthOffset);
    set_offset_msg.offset.a = 0.0;
    set_offset_msg.offset.b = 0.0;
    set_offset_msg.offset.c = 0.0;

    interp_list.append(set_offset_msg);
}

/* CHANGE_TOOL results from M6, for example */
void CHANGE_TOOL(int slot)
{
    EMC_TRAJ_LINEAR_MOVE linear_move_msg;
    EMC_TOOL_LOAD load_tool_msg;

    /* optional first move to tool change position */
    if (HAVE_TOOL_CHANGE_POSITION) {
	linear_move_msg.end.tran = TOOL_CHANGE_POSITION.tran;	// struct
	// copy
	linear_move_msg.end.a = 0.0;
	linear_move_msg.end.b = 0.0;
	linear_move_msg.end.c = 0.0;
        linear_move_msg.type = EMC_MOTION_TYPE_TOOLCHANGE;
	interp_list.append(linear_move_msg);
	/*! \todo FIXME-- orient spindle command goes here. We don't yet have an NML 
	   message for this. */
	/* first EMC_TOOL_LOAD message tells emcio to take tool out */
	interp_list.append(load_tool_msg);
    }

    /* optional move to clear Z */
    if (HAVE_TOOL_HOLDER_CLEAR) {
	linear_move_msg.end.tran = TOOL_HOLDER_CLEAR.tran;	// struct
	// copy
	linear_move_msg.end.a = 0.0;
	linear_move_msg.end.b = 0.0;
	linear_move_msg.end.c = 0.0;
        linear_move_msg.type = EMC_MOTION_TYPE_TOOLCHANGE;
	interp_list.append(linear_move_msg);
	/* second EMC_TOOL_LOAD message tells emcio rotate carousel */
	interp_list.append(load_tool_msg);
    }

    /* optional move back to tool change position */
    if (HAVE_TOOL_CHANGE_POSITION) {
	linear_move_msg.end.tran = TOOL_CHANGE_POSITION.tran;	// struct
	// copy
	linear_move_msg.end.a = 0.0;
	linear_move_msg.end.b = 0.0;
	linear_move_msg.end.c = 0.0;
        linear_move_msg.type = EMC_MOTION_TYPE_TOOLCHANGE;
	interp_list.append(linear_move_msg);
    }

    /* regardless of optional moves above, we'll always send a load tool
       message */
    interp_list.append(load_tool_msg);
}

/* SELECT_TOOL results from T1, for example */
void SELECT_TOOL(int slot)
{
    EMC_TOOL_PREPARE prep_for_tool_msg;

    prep_for_tool_msg.tool = slot;

    interp_list.append(prep_for_tool_msg);
}

/* Misc Functions */

void CLAMP_AXIS(CANON_AXIS axis)
{
    /*! \todo FIXME-- unimplemented */
}

/*
  setString and addString initializes or adds src to dst, never exceeding
  dst's maxlen chars.
*/

static char *setString(char *dst, const char *src, int maxlen)
{
    dst[0] = 0;
    strncat(dst, src, maxlen - 1);
    dst[maxlen - 1] = 0;
    return dst;
}

static char *addString(char *dst, const char *src, int maxlen)
{
    int dstlen = strlen(dst);
    int srclen = strlen(src);
    int actlen;

    if (srclen >= maxlen - dstlen) {
	actlen = maxlen - dstlen - 1;
	dst[maxlen - 1] = 0;
    } else {
	actlen = srclen;
    }

    strncat(dst, src, actlen);

    return dst;
}

/*
  The probe file is opened with a hot-comment (PROBEOPEN <filename>),
  and the results of each probed point are written to that file.
  The file is closed with a (PROBECLOSE) comment.
*/

static FILE *probefile = NULL;

void COMMENT(char *comment)
{
    // nothing need be done here, but you can play tricks with hot comments

    char msg[LINELEN];
    char probefilename[LINELEN];
    char *ptr;

    // set RPY orientation for subsequent moves
    if (!strncmp(comment, "RPY", strlen("RPY"))) {
	PM_RPY rpy;
	// it's RPY <R> <P> <Y>
	if (3 !=
	    sscanf(comment, "%*s %lf %lf %lf", &rpy.r, &rpy.p, &rpy.y)) {
	    // print current orientation
	    printf("rpy = %f %f %f, quat = %f %f %f %f\n",
		   rpy.r, rpy.p, rpy.y, quat.s, quat.x, quat.y, quat.z);
	} else {
	    // set and print orientation
	    quat = rpy;
	    printf("rpy = %f %f %f, quat = %f %f %f %f\n",
		   rpy.r, rpy.p, rpy.y, quat.s, quat.x, quat.y, quat.z);
	}
	return;
    }
    // open probe output file
    if (!strncmp(comment, "PROBEOPEN", strlen("PROBEOPEN"))) {
	// position ptr to first char after PROBEOPEN
	ptr = &comment[strlen("PROBEOPEN")];
	// and step over white space to name, or NULL
	while (isspace(*ptr)) {
	    ptr++;
	}
	setString(probefilename, ptr, LINELEN);
	if (NULL == (probefile = fopen(probefilename, "w"))) {
	    // pop up a warning message
	    setString(msg, "can't open probe file ", LINELEN);
	    addString(msg, probefilename, LINELEN);
	    MESSAGE(msg);
	    probefile = NULL;
	}
	return;
    }
    // close probe output file
    if (!strncmp(comment, "PROBECLOSE", strlen("PROBECLOSE"))) {
	if (probefile != NULL) {
	    fclose(probefile);
	    probefile = NULL;
	}
	return;
    }

    return;
}

void DISABLE_FEED_OVERRIDE()
{
    /*! \todo FIXME-- unimplemented */
}

void DISABLE_SPEED_OVERRIDE()
{
    /*! \todo FIXME-- unimplemented */
}

void ENABLE_FEED_OVERRIDE()
{
    /*! \todo FIXME-- unimplemented */
}

void ENABLE_SPEED_OVERRIDE()
{
    /*! \todo FIXME-- unimplemented */
}

void FLOOD_OFF()
{
    EMC_COOLANT_FLOOD_OFF flood_off_msg;

    interp_list.append(flood_off_msg);
}

void FLOOD_ON()
{
    EMC_COOLANT_FLOOD_ON flood_on_msg;

    interp_list.append(flood_on_msg);
}

void MESSAGE(char *s)
{
    EMC_OPERATOR_DISPLAY operator_display_msg;

    operator_display_msg.id = 0;
    strncpy(operator_display_msg.display, s, LINELEN);
    operator_display_msg.display[LINELEN - 1] = 0;

    interp_list.append(operator_display_msg);
}

void MIST_OFF()
{
    EMC_COOLANT_MIST_OFF mist_off_msg;

    interp_list.append(mist_off_msg);
}

void MIST_ON()
{
    EMC_COOLANT_MIST_ON mist_on_msg;

    interp_list.append(mist_on_msg);
}

void PALLET_SHUTTLE()
{
    /*! \todo FIXME-- unimplemented */
}

void TURN_PROBE_OFF()
{
    // don't do anything-- this is called when the probing is done
}

void TURN_PROBE_ON()
{
    EMC_TRAJ_CLEAR_PROBE_TRIPPED_FLAG clearMsg;

    interp_list.append(clearMsg);
}

void UNCLAMP_AXIS(CANON_AXIS axis)
{
    /*! \todo FIXME-- unimplemented */
}

/* Program Functions */

void STOP(void)
{
}

void PROGRAM_STOP()
{
    /* 
       implement this as a pause. A resume will cause motion to proceed. */
    EMC_TASK_PLAN_PAUSE pauseMsg;

    interp_list.append(pauseMsg);
}

void OPTIONAL_PROGRAM_STOP()
{
    /*! \todo FIXME-- implemented as PROGRAM_STOP, that is, no option */
    PROGRAM_STOP();
}

void PROGRAM_END()
{
    EMC_TASK_PLAN_END endMsg;

    interp_list.append(endMsg);
}

/* returns the current x, y, z origin offsets */
CANON_VECTOR GET_PROGRAM_ORIGIN()
{
    CANON_VECTOR origin;

    /* and convert from mm units to interpreter units */
    origin.x = TO_PROG_LEN(programOrigin.x);
    origin.y = TO_PROG_LEN(programOrigin.y);
    origin.z = TO_PROG_LEN(programOrigin.z);

    return origin;		/* in program units */
}

/* returns the current active units */
CANON_UNITS GET_LENGTH_UNITS()
{
    return lengthUnits;
}

CANON_PLANE GET_PLANE()
{
    return activePlane;
}

double GET_TOOL_LENGTH_OFFSET()
{
    return TO_EXT_LEN(currentToolLengthOffset);
}

/*
  INIT_CANON()
  Initialize canonical local variables to defaults
  */
void INIT_CANON()
{
    double units;

    // initialize locals to original values
    programOrigin.x = 0.0;
    programOrigin.y = 0.0;
    programOrigin.z = 0.0;
    programOrigin.a = 0.0;
    programOrigin.b = 0.0;
    programOrigin.c = 0.0;
    activePlane = CANON_PLANE_XY;
    canonEndPoint.x = 0.0;
    canonEndPoint.y = 0.0;
    canonEndPoint.z = 0.0;
    canonEndPoint.a = 0.0;
    canonEndPoint.b = 0.0;
    canonEndPoint.c = 0.0;
    SET_MOTION_CONTROL_MODE(CANON_CONTINUOUS, 0);
    spindleSpeed = 0.0;
    preppedTool = 0;
    linear_move = 0;
    angular_move = 0;
    currentLinearFeedRate = 0.0;
    currentAngularFeedRate = 0.0;
    currentToolLengthOffset = 0.0;
    /* 
       to set the units, note that GET_EXTERNAL_LENGTH_UNITS() returns
       traj->linearUnits, which is already set from the .ini file in
       iniTraj(). This is a floating point number, in user units per mm. We
       can compare this against known values and set the symbolic values
       accordingly. If it doesn't match, we have an error. */
    units = GET_EXTERNAL_LENGTH_UNITS();
    if (fabs(units - 1.0 / 25.4) < 1.0e-3) {
	lengthUnits = CANON_UNITS_INCHES;
    } else if (fabs(units - 1.0) < 1.0e-3) {
	lengthUnits = CANON_UNITS_MM;
    } else {
	CANON_ERROR
	    ("non-standard length units, setting interpreter to mm");
	lengthUnits = CANON_UNITS_MM;
    }
}

/* Sends error message */
void CANON_ERROR(const char *fmt, ...)
{
    va_list ap;
    EMC_OPERATOR_ERROR operator_error_msg;

    operator_error_msg.id = 0;
    if (fmt != NULL) {
	va_start(ap, fmt);
	vsprintf(operator_error_msg.error, fmt, ap);
	va_end(ap);
    } else {
	operator_error_msg.error[0] = 0;
    }

    interp_list.append(operator_error_msg);
}

/*
  GET_EXTERNAL_TOOL_TABLE(int pocket)

  Returns the tool table structure associated with pocket. Note that
  pocket can run from 0 (by definition, no tool), to pocket CANON_TOOL_MAX - 1.

  The value from emc status is in user units. We need to convert these
  to interpreter units, by calling FROM_EXT_LEN() to get them to mm, and
  then switching on lengthUnits.
  */
CANON_TOOL_TABLE GET_EXTERNAL_TOOL_TABLE(int pocket)
{
    CANON_TOOL_TABLE retval;

    if (pocket < 0 || pocket >= CANON_TOOL_MAX) {
	retval.id = 0;
	retval.length = 0.0;
	retval.diameter = 0.0;
    } else {
	retval = emcStatus->io.tool.toolTable[pocket];

	// convert from user to program units
	retval.length = TO_PROG_LEN(FROM_EXT_LEN(retval.length));
	retval.diameter = TO_PROG_LEN(FROM_EXT_LEN(retval.diameter));
    }

    return retval;
}

CANON_POSITION GET_EXTERNAL_POSITION()
{