/********************************************************************* Description: emccanon.cc*   Canonical definitions for 3-axis NC application**   Derived from a work by Fred Proctor & Will Shackleford** Author:* License: GPL Version 2* System: Linux*    * Copyright (c) 2004 All rights reserved.*********************************************************************//*  Notes:  Units  -----  Values are stored internally as mm and degree units, e.g, program  offsets, end point, tool length offset.  These are "internal  units". "External units" are the units used by the EMC motion planner.  All lengths and units output by the interpreter are converted to  internal units here, using FROM_PROG_LEN,ANG, and then  TO_EXT_LEN(),ANG are called to convert these to external units.  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.  */#include "config.h"#include <stdio.h>#include <stdarg.h>#include <math.h>#include <string.h>		// strncpy()#include <ctype.h>		// isspace()#include "emc.hh"		// EMC NML#include "canon.hh"		// these decls#include "interpl.hh"		// interp_list#include "emcglb.h"		// TRAJ_MAX_VELOCITY#include "emcpos.h"#ifndef MIN#define MIN(a,b) ((a)<(b)?(a):(b))#endif#ifndef MIN3#define MIN3(a,b,c) (MIN(MIN((a),(b)),(c)))#endif#ifndef MAX#define MAX(a,b) ((a)>(b)?(a):(b))#endif#ifndef MAX4#define MAX4(a,b,c,d) (MAX(MAX((a),(b)),MAX((c),(d))))#endifstatic PM_QUATERNION quat(1, 0, 0, 0);/*  These decls were from the old 3-axis canon.hh, and refer functions  defined here that are used for convenience but no longer have decls  in the 6-axis canon.hh. So, we declare them here now.*/extern void CANON_ERROR(const char *fmt, ...);/*  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_POSITION programOrigin(0.0, 0.0, 0.0, 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_POSITION canonEndPoint;static void canonUpdateEndPoint(double x, double y, double z, double a,				double b, double c){    canonEndPoint.x = x;    canonEndPoint.y = y;    canonEndPoint.z = z;    canonEndPoint.a = a;    canonEndPoint.b = b;    canonEndPoint.c = c;}/* 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;/* motion path-following tolerance is used to set the max path-following   deviation during CANON_CONTINUOUS.   If this param is 0, then it will behave as emc always did, allowing   almost any deviation trying to keep speed up. */static double canonMotionTolerance = 0.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;/* Tool length offset is saved here */static double currentToolLengthOffset = 0.0;/*  Feed rate is saved here; values are in mm/sec or deg/sec.  It will be initially set in INIT_CANON() below.*/static double currentLinearFeedRate = 0.0;static double currentAngularFeedRate = 0.0;/* Used to indicate whether the current move is linear, angular, or    a combination of both. */   //AJ says: linear means axes XYZ move (lines or even circles)   //         angular means axes ABC movestatic int linear_move = 0;static int angular_move = 0;/* sends a request to set the vel, which is in internal units/sec */static int sendVelMsg(double vel, double ini_maxvel){    EMC_TRAJ_SET_VELOCITY velMsg;    if (linear_move && !angular_move) {	velMsg.velocity = TO_EXT_LEN(vel);	velMsg.ini_maxvel = TO_EXT_LEN(ini_maxvel);    } else if (!linear_move && angular_move) {	velMsg.velocity = TO_EXT_ANG(vel);	velMsg.ini_maxvel = TO_EXT_ANG(ini_maxvel);    } else if (linear_move && angular_move) {	velMsg.velocity = TO_EXT_LEN(vel);	velMsg.ini_maxvel = TO_EXT_LEN(ini_maxvel);    } else { //seems this case was forgotten, neither linear, neither angular move (we are only sending vel)	velMsg.velocity = TO_EXT_LEN(vel);	velMsg.ini_maxvel = TO_EXT_LEN(ini_maxvel);    }	    interp_list.append(velMsg);    return 0;}static int sendAccMsg(double acc){    EMC_TRAJ_SET_ACCELERATION accMsg;    if (linear_move && !angular_move) {	accMsg.acceleration = TO_EXT_LEN(acc);    } else if (!linear_move && angular_move) {	accMsg.acceleration = TO_EXT_ANG(acc);    } else if (linear_move && angular_move) {	accMsg.acceleration = TO_EXT_LEN(acc);    }    interp_list.append(accMsg);    return 0;}/* Representation */void SET_ORIGIN_OFFSETS(double x, double y, double z,			double a, double b, double c){    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);    a = FROM_PROG_ANG(a);    b = FROM_PROG_ANG(b);    c = FROM_PROG_ANG(c);    programOrigin.x = x;    programOrigin.y = y;    programOrigin.z = z;    programOrigin.a = a;    programOrigin.b = b;    programOrigin.c = c;    /* 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.a = TO_EXT_ANG(programOrigin.a);    set_origin_msg.origin.b = TO_EXT_ANG(programOrigin.b);    set_origin_msg.origin.c = TO_EXT_ANG(programOrigin.c);    interp_list.append(set_origin_msg);}void USE_LENGTH_UNITS(CANON_UNITS in_unit){    lengthUnits = in_unit;    emcStatus->task.programUnits = in_unit;}/* Free Space Motion */void SET_TRAVERSE_RATE(double rate){    // nothing need be done here}void SET_FEED_RATE(double rate){    /* convert from /min to /sec */    rate /= 60.0;    /* convert to traj units (mm & deg) if needed */    currentLinearFeedRate = FROM_PROG_LEN(rate);    currentAngularFeedRate = FROM_PROG_ANG(rate);}void SET_FEED_REFERENCE(CANON_FEED_REFERENCE reference){    // nothing need be done here}double getStraightAcceleration(double x, double y, double z,			   double a, double b, double c){    double dx, dy, dz, da, db, dc;    double tx, ty, tz, ta, tb, tc, tmax;    double acc, dtot;    const double tiny = 1e-10;    acc = 0.0; // if a move to nowhere    // Compute absolute travel distance for each axis:    dx = fabs(x - canonEndPoint.x);    dy = fabs(y - canonEndPoint.y);    dz = fabs(z - canonEndPoint.z);    da = fabs(a - canonEndPoint.a);    db = fabs(b - canonEndPoint.b);    dc = fabs(c - canonEndPoint.c);    if(dx < tiny) dx = 0.0;    if(dy < tiny) dy = 0.0;    if(dz < tiny) dz = 0.0;    if(da < tiny) da = 0.0;    if(db < tiny) db = 0.0;    if(dc < tiny) dc = 0.0;    // Figure out what kind of move we're making:    if (dx <= 0.0 && dy <= 0.0 && dz <= 0.0) {	linear_move = 0;    } else {	linear_move = 1;    }    if (da <= 0.0 && db <= 0.0 && dc <= 0.0) {	angular_move = 0;    } else {	angular_move = 1;    }    // Pure linear move:    if (linear_move && !angular_move) {	tx = (dx / FROM_EXT_LEN(AXIS_MAX_ACCELERATION[0]));	ty = (dy / FROM_EXT_LEN(AXIS_MAX_ACCELERATION[1]));	tz = (dz / FROM_EXT_LEN(AXIS_MAX_ACCELERATION[2]));	tmax = tx > ty ? tx : ty;	tmax = tz > tmax ? tz : tmax;	dtot = sqrt(dx * dx + dy * dy + dz * dz);	if (tmax > 0.0) {	    acc = dtot / tmax;	}    }    // Pure angular move:    else if (!linear_move && angular_move) {	ta = da? (da / FROM_EXT_ANG(AXIS_MAX_ACCELERATION[3])): 0.0;	tb = db? (db / FROM_EXT_ANG(AXIS_MAX_ACCELERATION[4])): 0.0;	tc = dc? (dc / FROM_EXT_ANG(AXIS_MAX_ACCELERATION[5])): 0.0;	tmax = ta > tb ? ta : tb;	tmax = tc > tmax ? tc : tmax;	dtot = sqrt(da * da + db * db + dc * dc);	if (tmax > 0.0) {	    acc = dtot / tmax;	}    }    // Combination angular and linear move:    else if (linear_move && angular_move) {	tx = (dx / FROM_EXT_LEN(AXIS_MAX_ACCELERATION[0]));	ty = (dy / FROM_EXT_LEN(AXIS_MAX_ACCELERATION[1]));	tz = (dz / FROM_EXT_LEN(AXIS_MAX_ACCELERATION[2]));	ta = da? (da / FROM_EXT_ANG(AXIS_MAX_ACCELERATION[3])): 0.0;	tb = db? (db / FROM_EXT_ANG(AXIS_MAX_ACCELERATION[4])): 0.0;	tc = dc? (dc / FROM_EXT_ANG(AXIS_MAX_ACCELERATION[5])): 0.0;	tmax = tx > ty ? tx : ty;	tmax = tz > tmax ? tz : tmax;	tmax = ta > tmax ? ta : tmax;	tmax = tb > tmax ? tb : tmax;	tmax = tc > tmax ? tc : tmax;/*  According to NIST IR6556 Section 2.1.2.5 Paragraph A    a combnation move is handled like a linear move, except    that the angular axes are allowed sufficient time to    complete their motion coordinated with the motion of    the linear axes.*/	dtot = sqrt(dx * dx + dy * dy + dz * dz);	if (tmax > 0.0) {	    acc = dtot / tmax;	}    }    return acc;}double getStraightVelocity(double x, double y, double z,			   double a, double b, double c){    double dx, dy, dz, da, db, dc;    double tx, ty, tz, ta, tb, tc, tmax;    double vel, dtot;    const double tiny = 1e-10;/* If we get a move to nowhere (!linear_move && !angular_move)   we might as well go there at the currentLinearFeedRate...*/    vel = currentLinearFeedRate;    // Compute absolute travel distance for each axis:    dx = fabs(x - canonEndPoint.x);    dy = fabs(y - canonEndPoint.y);    dz = fabs(z - canonEndPoint.z);    da = fabs(a - canonEndPoint.a);    db = fabs(b - canonEndPoint.b);    dc = fabs(c - canonEndPoint.c);    if(dx < tiny) dx = 0.0;    if(dy < tiny) dy = 0.0;    if(dz < tiny) dz = 0.0;    if(da < tiny) da = 0.0;    if(db < tiny) db = 0.0;    if(dc < tiny) dc = 0.0;    // Figure out what kind of move we're making:    if (dx <= 0.0 && dy <= 0.0 && dz <= 0.0) {	linear_move = 0;    } else {	linear_move = 1;    }    if (da <= 0.0 && db <= 0.0 && dc <= 0.0) {	angular_move = 0;    } else {	angular_move = 1;    }    // Pure linear move:    if (linear_move && !angular_move) {	tx = fabs(dx / FROM_EXT_LEN(AXIS_MAX_VELOCITY[0]));	ty = fabs(dy / FROM_EXT_LEN(AXIS_MAX_VELOCITY[1]));	tz = fabs(dz / FROM_EXT_LEN(AXIS_MAX_VELOCITY[2]));	tmax = tx > ty ? tx : ty;	tmax = tz > tmax ? tz : tmax;	dtot = sqrt(dx * dx + dy * dy + dz * dz);	if (tmax <= 0.0) {	    vel = currentLinearFeedRate;	} else {	    vel = dtot / tmax;	}    }    // Pure angular move:    else if (!linear_move && angular_move) {	ta = da? fabs(da / FROM_EXT_ANG(AXIS_MAX_VELOCITY[3])):0.0;	tb = db? fabs(db / FROM_EXT_ANG(AXIS_MAX_VELOCITY[4])):0.0;	tc = dc? fabs(dc / FROM_EXT_ANG(AXIS_MAX_VELOCITY[5])):0.0;	tmax = ta > tb ? ta : tb;	tmax = tc > tmax ? tc : tmax;	dtot = sqrt(da * da + db * db + dc * dc);	if (tmax <= 0.0) {	    vel = currentAngularFeedRate;	} else {	    vel = dtot / tmax;	}    }    // Combination angular and linear move:    else if (linear_move && angular_move) {	tx = fabs(dx / FROM_EXT_LEN(AXIS_MAX_VELOCITY[0]));	ty = fabs(dy / FROM_EXT_LEN(AXIS_MAX_VELOCITY[1]));	tz = fabs(dz / FROM_EXT_LEN(AXIS_MAX_VELOCITY[2]));	ta = da? fabs(da / FROM_EXT_ANG(AXIS_MAX_VELOCITY[3])):0.0;	tb = db? fabs(db / FROM_EXT_ANG(AXIS_MAX_VELOCITY[4])):0.0;	tc = dc? fabs(dc / FROM_EXT_ANG(AXIS_MAX_VELOCITY[5])):0.0;	tmax = tx > ty ? tx : ty;	tmax = tz > tmax ? tz : tmax;	tmax = ta > tmax ? ta : tmax;	tmax = tb > tmax ? tb : tmax;	tmax = tc > tmax ? tc : tmax;/*  According to NIST IR6556 Section 2.1.2.5 Paragraph A    a combnation move is handled like a linear move, except    that the angular axes are allowed sufficient time to    complete their motion coordinated with the motion of    the linear axes.*/	dtot = sqrt(dx * dx + dy * dy + dz * dz);	if (tmax <= 0.0) {	    vel = currentLinearFeedRate;	} else {	    vel = dtot / tmax;	}    }    return vel;}void STRAIGHT_TRAVERSE(double x, double y, double z,		       double a, double b, double c){    double vel, acc;    EMC_TRAJ_LINEAR_MOVE linearMoveMsg;    // convert to mm units    x = FROM_PROG_LEN(x);    y = FROM_PROG_LEN(y);    z = FROM_PROG_LEN(z);    a = FROM_PROG_ANG(a);    b = FROM_PROG_ANG(b);    c = FROM_PROG_ANG(c);    x += programOrigin.x;    y += programOrigin.y;    z += programOrigin.z;    a += programOrigin.a;    b += programOrigin.b;    c += programOrigin.c;    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);    // fill in the orientation    linearMoveMsg.end.a = TO_EXT_ANG(a);    linearMoveMsg.end.b = TO_EXT_ANG(b);    linearMoveMsg.end.c = TO_EXT_ANG(c);    vel = getStraightVelocity(x, y, z, a, b, c);    sendVelMsg(vel, vel);    if((acc = getStraightAcceleration(x, y, z, a, b, c)))        sendAccMsg(acc);