NAN Bug with circular moves
- Subject: NAN Bug with circular moves
- From: Will Shackleford <shackle-at-cme.nist.gov>
- Date: Mon, 7 Feb 2000 16:20:34 -0500 (EST)
- Content-Type: MULTIPART/mixed; BOUNDARY=Charm_of_Finches_718_000
- Reply-To: Will Shackleford <shackle-at-cme.nist.gov>
The attached file contains one possible fix for the NAN bug that occurs
with circular moves that start and end at the same location.
To try it, copy it to emc/src/emctask/emccanon.cc, recompile and
run a program similiar to
g1x5y0
g2x5y0i-5j0
---------------------------------------------------------------
William Penn Shackleford III shackle-at-nist.gov
National Institute of Standards & Technology Tel: (301) 975-4286
100 Bureau Drive Stop 8230 FAX: (301) 990-9688
Gaithersburg MD 20899 USA http://www.isd.mel.nist.gov/personnell/shackleford/
/*
emccanon.cc
Canonical definitions for 3-axis NC application
Modification history:
2/7/00 WPS modified ARC_FEED to add an additional rotation when
the starting and ending positions are equal.
2-Jun-1999 FMP added setting of feed rate in INIT_CANON from
TRAJ_DEFAULT_VELOCITY
11-Jan-1999 FMP subtracted tool length offset from Z position in
GET_EXTERNAL_POSITION(), a fix for Jon Elson's email report of this bug:
MDI,G43H<tool with some length>,MANUAL,jog Z,MDI,G1X1F60, and the
Z moves.
23-Dec-1998 FMP put tool length offset and program origin on the
interp list to prevent display of the read-ahead values of these
15-Oct-1998 FMP implemented MESSAGE with queue of EMC_OPERATOR_DISPLAY,
CANON_ERROR with queue of EMC_OPERATOR_ERROR
11-Aug-1998 FMP fixed bug in CANON_PLANE_XZ offset and aligment of
args, thanks to Angelo Brousalis for this one
3-Aug-1998 FMP added GET_TOOL_LENGTH_OFFSET()
12-Jun-1998 FMP added canonMotionMode flag, anticipating mods to
motion controller for stop-at-end flag to inhibit blending
21-May-1998 FMP fixed GET_EXTERNAL_POSITION to return position with
offsets subtracted off-- this fixed the problem where axes that were
not specified in the interpreter moved (now that <interpreter>_synch()
is called in emctask.cc).
15-May-1998 FMP added TRAJ_MAX_VELOCITY global
14-Apr-1998 FMP set default traverse and feed rates to 10 mm/sec
1-Apr-1998 FMP added spindleOn flag for resending CW, CCW if speed change
24-Feb-1998 FMP added TO_EXT_LEN, FROM_PROG_LEN, etc.
20-Feb-1998 FMP added tool length offsets
13-Jan-1998 FMP set rot values in straight feed
2-Dec-1997 FMP changed NML to EMC vocabulary
17-Oct-1997 FMP created
*/
/*
Notes:
Units
-----
The canonical interface uses mm length units and degree angle units.
All lengths and units output by the interpreter are converted to these
units, and then EXT_LEN(),ANGLE() are called to convert these to user
units since user units are expressed in terms of mm and deg.
Values are stored internally as mm/degree units, e.g, program offsets,
end point, tool length offset.
Tool Length Offsets
-------------------
The interpreter does not subtract off tool length offsets. It calls
USE_TOOL_LENGTH_OFFSETS(length), which we record here and apply to
all subsequent Z values.
*/
// ident tag
static char ident[] = "$Id: emccanon.cc,v 1.24 2000/02/07 19:00:47 shackle Exp $";
#include <stdio.h>
#include <stdarg.h>
#include <math.h>
#include <string.h> // strncpy()
#include "emc.hh" // EMC NML
#include "canon.hh" // these decls
#include "interpl.hh" // interp_list
#include "emcglb.h" // TRAJ_MAX_VELOCITY
/* FIXME-- testing orientation control */
static PM_QUATERNION quat;
/*
Origin offsets, length units, and active plane are all maintained
here in this file. Controller runs in absolute mode, and does not
have plane select concept.
programOrigin is stored in mm always, and converted when set or read.
When it's applied to positions, convert positions to mm units first
and then add programOrigin.
Units are then converted from mm to external units, as reported by
the GET_EXTERNAL_LENGTH_UNITS() function.
*/
static CANON_VECTOR programOrigin(0.0, 0.0, 0.0);
static CANON_UNITS lengthUnits = CANON_UNITS_MM;
static CANON_PLANE activePlane = CANON_PLANE_XY;
/*
canonEndPoint is the last programmed end point, stored in case it's
needed for subsequent calculations. It's in absolute frame, mm units.
*/
static CANON_VECTOR canonEndPoint;
static void canonUpdateEndPoint(double x, double y, double z)
{
canonEndPoint.x = x;
canonEndPoint.y = y;
canonEndPoint.z = z;
}
/* motion control mode is used to signify blended v. stop-at-end moves.
Set to 0 (invalid) at start, so first call will send command out */
static CANON_MOTION_MODE canonMotionMode = 0;
/* macros for converting internal (mm/deg) units to external units */
#define TO_EXT_LEN(mm) ((mm) * GET_EXTERNAL_LENGTH_UNITS())
#define TO_EXT_ANG(deg) ((deg) * GET_EXTERNAL_ANGLE_UNITS())
/* macros for converting external units to internal (mm/deg) units */
#define FROM_EXT_LEN(ext) ((ext) / GET_EXTERNAL_LENGTH_UNITS())
#define FROM_EXT_ANG(ext) ((ext) / GET_EXTERNAL_ANGLE_UNITS())
/* macros for converting internal (mm/deg) units to program units */
#define TO_PROG_LEN(mm) ((mm) / (lengthUnits == CANON_UNITS_INCHES ? 25.4 : lengthUnits == CANON_UNITS_CM ? 10.0 : 1.0))
#define TO_PROG_ANG(deg) (deg)
/* macros for converting program units to internal (mm/deg) units */
#define FROM_PROG_LEN(prog) ((prog) * (lengthUnits == CANON_UNITS_INCHES ? 25.4 : lengthUnits == CANON_UNITS_CM ? 10.0 : 1.0))
#define FROM_PROG_ANG(prog) (prog)
/*
Spindle speed is saved here
*/
static double spindleSpeed = 0.0;
/*
Prepped tool is saved here
*/
static int preppedTool = 0;
/*
Feed and traverse rates are saved here; values are in mm/sec. They
will be initially set in INIT_CANON(
) below.
*/
static double currentFeedRate = 1.0;
static double currentTraverseRate = 1.0;
/*
Tool length offset is saved here
*/
static double currentToolLengthOffset = 0.0;
/*
EMC traj interface uses one speed. For storing and applying separate
feed and traverse rates, a flag for which was last applied is used.
Requests for motions check this flag and if the improper speed was
last set the proper one is set before the command goes out.
*/
// types of rates set
#define FEED_RATE 1
#define TRAVERSE_RATE 2
// sends a request to set the appropriate feed or traverse rate, if necessary
static int properVelocity(int which)
{
EMC_TRAJ_SET_VELOCITY velMsg;
if (which == FEED_RATE)
{
velMsg.velocity = TO_EXT_LEN(currentFeedRate);
interp_list.append(velMsg);
return 0;
}
if (which == TRAVERSE_RATE)
{
velMsg.velocity = TO_EXT_LEN(currentTraverseRate);
interp_list.append(velMsg);
return 0;
}
return -1; // invalid 'which'
}
/* Representation */
void SET_ORIGIN_OFFSETS(double x, double y, double z)
{
EMC_TRAJ_SET_ORIGIN set_origin_msg;
/* convert to mm units */
x = FROM_PROG_LEN(x);
y = FROM_PROG_LEN(y);
z = FROM_PROG_LEN(z);
programOrigin.x = x;
programOrigin.y = y;
programOrigin.z = z;
/* append it to interp list so it gets updated at the right time,
not at read-ahead time */
set_origin_msg.origin.tran.x = TO_EXT_LEN(programOrigin.x);
set_origin_msg.origin.tran.y = TO_EXT_LEN(programOrigin.y);
set_origin_msg.origin.tran.z = TO_EXT_LEN(programOrigin.z);
set_origin_msg.origin.rot.s = 1.0;
set_origin_msg.origin.rot.x = 0.0;
set_origin_msg.origin.rot.y = 0.0;
set_origin_msg.origin.rot.z = 0.0;
interp_list.append(set_origin_msg);
}
void USE_LENGTH_UNITS(CANON_UNITS in_unit)
{
lengthUnits = in_unit;
}
/* Free Space Motion */
void SET_TRAVERSE_RATE(double rate)
{
/* convert to traj units (mm) if needed */
rate = FROM_PROG_LEN(rate);
/* convert from /min to /sec */
rate /= 60.0;
/* record it for use by properVelocity() */
currentTraverseRate = rate;
}
void STRAIGHT_TRAVERSE (double x, double y, double z)
{
EMC_TRAJ_LINEAR_MOVE linearMoveMsg;
// convert to mm units
x = FROM_PROG_LEN(x);
y = FROM_PROG_LEN(y);
z = FROM_PROG_LEN(z);
x += programOrigin.x;
y += programOrigin.y;
z += programOrigin.z;
z += currentToolLengthOffset;
// now x, y, z, and b are in absolute mm or degree units
linearMoveMsg.end.tran.x = TO_EXT_LEN(x);
linearMoveMsg.end.tran.y = TO_EXT_LEN(y);
linearMoveMsg.end.tran.z = TO_EXT_LEN(z);
// ensure proper feed rate
properVelocity(TRAVERSE_RATE);
// fill in the orientation
linearMoveMsg.end.rot = quat;
interp_list.append(linearMoveMsg);
canonUpdateEndPoint(x, y, z);
}
/* Machining Attributes */
void SET_FEED_RATE(double rate)
{
/* convert to traj units (mm) if needed */
rate = FROM_PROG_LEN(rate);
/* convert from /min to /sec */
rate /= 60.0;
/* record it for use by properVelocity() */
currentFeedRate = rate;
}
void SET_FEED_REFERENCE(CANON_FEED_REFERENCE reference)
{
// nothing need be done here
}
void SET_MOTION_CONTROL_MODE(CANON_MOTION_MODE mode)
{
EMC_TRAJ_SET_TERM_COND setTermCondMsg;
if (mode != canonMotionMode)
{
canonMotionMode = mode;
switch (mode)
{
case CANON_CONTINUOUS:
setTermCondMsg.cond = EMC_TRAJ_TERM_COND_BLEND;
break;
default:
setTermCondMsg.cond = EMC_TRAJ_TERM_COND_STOP;
break;
}
interp_list.append(setTermCondMsg);
}
}
CANON_MOTION_MODE GET_MOTION_CONTROL_MODE()
{
return canonMotionMode;
}
void SELECT_PLANE(CANON_PLANE in_plane)
{
activePlane = in_plane;
}
void SET_CUTTER_RADIUS_COMPENSATION(double radius)
{
// nothing need be done here
}
void START_CUTTER_RADIUS_COMPENSATION(int side)
{
// nothing need be done here
}
void STOP_CUTTER_RADIUS_COMPENSATION()
{
// nothing need be done here
}
void START_SPEED_FEED_SYNCH()
{
// FIXME-- unimplemented
}
void STOP_SPEED_FEED_SYNCH()
{
// FIXME-- unimplemented
}
void SELECT_MOTION_MODE(CANON_MOTION_MODE mode)
{
// nothing need be done here
}
/* Machining Functions */
void ARC_FEED(double first_end, double second_end,
double first_axis, double second_axis, int rotation,
double axis_end_point)
{
PM_POSE end;
PM_CARTESIAN center, normal;
EMC_TRAJ_CIRCULAR_MOVE circularMoveMsg;
EMC_TRAJ_LINEAR_MOVE linearMoveMsg;
// convert to absolute mm units
first_axis = FROM_PROG_LEN(first_axis);
second_axis = FROM_PROG_LEN(second_axis);
first_end = FROM_PROG_LEN(first_end);
second_end = FROM_PROG_LEN(second_end);
axis_end_point = FROM_PROG_LEN(axis_end_point);
// associate x with x, etc., offset by program origin, and set normals
switch (activePlane)
{
case CANON_PLANE_XY:
// offset and align args properly
end.tran.x = first_end + programOrigin.x;
end.tran.y = second_end + programOrigin.y;
end.tran.z = axis_end_point + programOrigin.z;
end.tran.z += currentToolLengthOffset;
center.x = first_axis + programOrigin.x;
center.y = second_axis + programOrigin.y;
center.z = end.tran.z;
normal.x = 0.0;
normal.y = 0.0;
normal.z = 1.0;
break;
case CANON_PLANE_YZ:
// offset and align args properly
end.tran.y = first_end + programOrigin.y;
end.tran.z = second_end + programOrigin.z;
end.tran.x = axis_end_point + programOrigin.x;
end.tran.z += currentToolLengthOffset;
center.y = first_axis + programOrigin.y;
center.z = second_axis + programOrigin.z;
center.x = end.tran.x;
normal.y = 0.0;
normal.z = 0.0;
normal.x = 1.0;
break;
case CANON_PLANE_XZ:
// offset and align args properly
end.tran.z = first_end + programOrigin.z;
end.tran.x = second_end + programOrigin.x;
end.tran.y = axis_end_point + programOrigin.y;
end.tran.z += currentToolLengthOffset;
center.z = first_axis + programOrigin.z;
center.x = second_axis + programOrigin.x;
center.y = end.tran.y;
normal.z = 0.0;
normal.x = 0.0;
normal.y = 1.0;
break;
}
// set proper velocity
properVelocity(FEED_RATE);
/*
mapping of rotation to turns:
rotation turns
-------- -----
0 none (linear move)
1 0
2 1
-1 -1
-2 -2
*/
if (rotation == 0)
{
// linear move
linearMoveMsg.end.tran.x = TO_EXT_LEN(end.tran.x);
linearMoveMsg.end.tran.y = TO_EXT_LEN(end.tran.y);
linearMoveMsg.end.tran.z = TO_EXT_LEN(end.tran.z);
// fill in the orientation
linearMoveMsg.end.rot = quat;
interp_list.append(linearMoveMsg);
}
else if (rotation > 0)
{
// If starting and ending on same point move around the
// circle, don't just stay put.
if(canonEndPoint.x == end.tran.x &&
canonEndPoint.y == end.tran.y &&
canonEndPoint.z == end.tran.z)
{
rotation++;
}
circularMoveMsg.end.tran.x = TO_EXT_LEN(end.tran.x);
circularMoveMsg.end.tran.y = TO_EXT_LEN(end.tran.y);
circularMoveMsg.end.tran.z = TO_EXT_LEN(end.tran.z);
circularMoveMsg.center.x = TO_EXT_LEN(center.x);
circularMoveMsg.center.y = TO_EXT_LEN(center.y);
circularMoveMsg.center.z = TO_EXT_LEN(center.z);
circularMoveMsg.normal.x = TO_EXT_LEN(normal.x);
circularMoveMsg.normal.y = TO_EXT_LEN(normal.y);
circularMoveMsg.normal.z = TO_EXT_LEN(normal.z);
circularMoveMsg.turn = rotation - 1;
// fill in the orientation
circularMoveMsg.end.rot = quat;
interp_list.append(circularMoveMsg);
}
else
{
// reverse turn
// If starting and ending on same point move around the
// circle, don't just stay put.
if(canonEndPoint.x == end.tran.x &&
canonEndPoint.y == end.tran.y &&
canonEndPoint.z == end.tran.z)
{
rotation--;
}
circularMoveMsg.end.tran.x = TO_EXT_LEN(end.tran.x);
circularMoveMsg.end.tran.y = TO_EXT_LEN(end.tran.y);
circularMoveMsg.end.tran.z = TO_EXT_LEN(end.tran.z);
circularMoveMsg.center.x = TO_EXT_LEN(center.x);
circularMoveMsg.center.y = TO_EXT_LEN(center.y);
circularMoveMsg.center.z = TO_EXT_LEN(center.z);
circularMoveMsg.normal.x = TO_EXT_LEN(normal.x);
circularMoveMsg.normal.y = TO_EXT_LEN(normal.y);
circularMoveMsg.normal.z = TO_EXT_LEN(normal.z);
circularMoveMsg.turn = rotation;
// fill in the orientation
circularMoveMsg.end.rot = quat;
interp_list.append(circularMoveMsg);
}
// update the end point
canonUpdateEndPoint(end.tran.x, end.tran.y, end.tran.z);
}
void STRAIGHT_FEED (double x, double y, double z)
{
EMC_TRAJ_LINEAR_MOVE linearMoveMsg;
// convert to mm units
x = FROM_PROG_LEN(x);
y = FROM_PROG_LEN(y);
z = FROM_PROG_LEN(z);
x += programOrigin.x;
y += programOrigin.y;
z += programOrigin.z;
z += currentToolLengthOffset;
// now x, y, z, and b are in absolute mm or degree units
linearMoveMsg.end.tran.x = TO_EXT_LEN(x);
linearMoveMsg.end.tran.y = TO_EXT_LEN(y);
linearMoveMsg.end.tran.z = TO_EXT_LEN(z);
// ensure proper feed rate
properVelocity(FEED_RATE);
// fill in the orientation
linearMoveMsg.end.rot = quat;
interp_list.append(linearMoveMsg);
canonUpdateEndPoint(x, y, z);
}
void STRAIGHT_PROBE (double x, double y, double z)
{
// FIXME-- unimplemented
}
void DWELL(double seconds)
{
EMC_TRAJ_DELAY delayMsg;
delayMsg.delay = seconds;
interp_list.append(delayMsg);
}
/* Spindle Functions */
void SPINDLE_RETRACT_TRAVERSE()
{
// FIXME-- unimplemented
}
/* 0 is off, -1 is CCW, 1 is CW; used as flag if settting speed again */
static int spindleOn = 0;
void START_SPINDLE_CLOCKWISE()
{
EMC_SPINDLE_ON emc_spindle_on_msg;
emc_spindle_on_msg.speed = spindleSpeed;
interp_list.append(emc_spindle_on_msg);
spindleOn = 1;
}
void START_SPINDLE_COUNTERCLOCKWISE()
{
EMC_SPINDLE_ON emc_spindle_on_msg;
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()
{
// FIXME-- unimplemented
}
void ORIENT_SPINDLE(double orientation, CANON_DIRECTION direction)
{
// FIXME-- unimplemented
}
void USE_NO_SPINDLE_FORCE()
{
// 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.rot.s = 1.0;
set_offset_msg.offset.rot.x = 0.0;
set_offset_msg.offset.rot.y = 0.0;
set_offset_msg.offset.rot.z = 0.0;
interp_list.append(set_offset_msg);
}
void CHANGE_TOOL(int slot)
{
EMC_TOOL_LOAD load_tool_msg;
interp_list.append(load_tool_msg);
}
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)
{
// FIXME-- unimplemented
}
void COMMENT(char *comment)
{
// nothing need be done here, but you can play tricks with hot comments
// hot comment for setting 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)) {
return; // ignore
}
else {
// set quaternion global
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);
}
}
}
void DISABLE_FEED_OVERRIDE()
{
// FIXME-- unimplemented
}
void DISABLE_SPEED_OVERRIDE()
{
// FIXME-- unimplemented
}
void ENABLE_FEED_OVERRIDE()
{
// FIXME-- unimplemented
}
void ENABLE_SPEED_OVERRIDE()
{
// 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, EMC_OPERATOR_DISPLAY_LEN);
operator_display_msg.display[EMC_OPERATOR_DISPLAY_LEN - 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()
{
// FIXME-- unimplemented
}
void TURN_PROBE_OFF()
{
// FIXME-- unimplemented
}
void TURN_PROBE_ON()
{
// FIXME-- unimplemented
}
void UNCLAMP_AXIS(CANON_AXIS axis)
{
// FIXME-- unimplemented
}
/* Program Functions */
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()
{
// 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()
{
// initialize locals to original values
programOrigin.x = 0.0;
programOrigin.y = 0.0;
programOrigin.z = 0.0;
lengthUnits = CANON_UNITS_MM;
activePlane = CANON_PLANE_XY;
canonEndPoint.x = 0.0;
canonEndPoint.y = 0.0;
canonEndPoint.z = 0.0;
SET_MOTION_CONTROL_MODE(CANON_CONTINUOUS);
spindleSpeed = 0.0;
preppedTool = 0;
currentFeedRate = FROM_EXT_LEN(TRAJ_DEFAULT_VELOCITY); // from emcglb.h
currentTraverseRate = FROM_EXT_LEN(TRAJ_MAX_VELOCITY); // from emcglb.h
currentToolLengthOffset = 0.0;
}
/* 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()
{
CANON_POSITION position;
PM_POSE pos;
pos = emcStatus->motion.traj.position;
position.x = TO_PROG_LEN(FROM_EXT_LEN(pos.tran.x) - programOrigin.x);
position.y = TO_PROG_LEN(FROM_EXT_LEN(pos.tran.y) - programOrigin.y);
position.z = TO_PROG_LEN(FROM_EXT_LEN(pos.tran.z) - programOrigin.z);
// knock off tool length offset
position.z -= TO_PROG_LEN(currentToolLengthOffset);
return position;
}
CANON_POSITION GET_EXTERNAL_PROBE_POSITION()
{
// FIXME-- unimplemented
return CANON_POSITION(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
}
double GET_EXTERNAL_PROBE_VALUE()
{
// FIXME-- unimplemented
return 0.0;
}
int IS_EXTERNAL_QUEUE_EMPTY()
{
return emcStatus->motion.traj.queue == 0 ? 1 : 0;
}
// feed rate wanted is in program units per minute
double GET_EXTERNAL_FEED_RATE()
{
double feed;
// convert from external to program units
feed = TO_PROG_LEN(FROM_EXT_LEN(emcStatus->motion.traj.velocity));
// now convert from per-sec to per-minute
feed *= 60.0;
return feed;
}
// traverse rate wanted is in program units per minute
double GET_EXTERNAL_TRAVERSE_RATE()
{
double traverse;
// convert from external to program units
traverse = TO_PROG_LEN(FROM_EXT_LEN(emcStatus->motion.traj.maxVelocity));
// now convert from per-sec to per-minute
traverse *= 60.0;
return traverse;
}
double GET_EXTERNAL_LENGTH_UNITS()
{
double u;
u = emcStatus->motion.traj.linearUnits;
if (u == 0)
{
CANON_ERROR("external length units are zero");
return 1.0;
}
else
{
return u;
}
}
double GET_EXTERNAL_ANGLE_UNITS()
{
double u;
u = emcStatus->motion.traj.angularUnits;
if (u == 0)
{
CANON_ERROR("external angle units are zero");
return 1.0;
}
else
{
return u;
}
}
int GET_EXTERNAL_TOOL()
{
return emcStatus->io.tool.toolInSpindle;
}
int GET_EXTERNAL_MIST()
{
return emcStatus->io.coolant.mist;
}
int GET_EXTERNAL_FLOOD()
{
return emcStatus->io.coolant.flood;
}
int GET_EXTERNAL_POCKET()
{
return emcStatus->io.tool.toolPrepped;
}
double GET_EXTERNAL_SPEED()
{
// speed is in RPMs everywhere
return emcStatus->io.spindle.speed;
}
CANON_DIRECTION GET_EXTERNAL_SPINDLE()
{
if (emcStatus->io.spindle.speed == 0)
{
return CANON_STOPPED;
}
if (emcStatus->io.spindle.speed >= 0.0)
{
return CANON_CLOCKWISE;
}
return CANON_COUNTERCLOCKWISE;
}
int GET_EXTERNAL_TOOL_MAX()
{
return CANON_TOOL_MAX;
}
Date Index |
Thread Index |
Back to archive index |
Back to Mailing List Page
Problems or questions? Contact