linearMoveMsg.type = EMC_MOTION_TYPE_TRAVERSE;    interp_list.append(linearMoveMsg);    canonUpdateEndPoint(x, y, z, a, b, c);}void STRAIGHT_FEED(double x, double y, double z, double a, double b,		   double c){    double ini_maxvel, 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);    ini_maxvel = vel = getStraightVelocity(x, y, z, a, b, c);    if (linear_move && !angular_move) {	if (vel > currentLinearFeedRate) {	    vel = currentLinearFeedRate;	}    } else if (!linear_move && angular_move) {	if (vel > currentAngularFeedRate) {	    vel = currentAngularFeedRate;	}    } else if (linear_move && angular_move) {	if (vel > currentLinearFeedRate) {	    vel = currentLinearFeedRate;	}    }    sendVelMsg(vel, ini_maxvel);    if((acc = getStraightAcceleration(x, y, z, a, b, c)))        sendAccMsg(acc);    linearMoveMsg.type = EMC_MOTION_TYPE_FEED;    interp_list.append(linearMoveMsg);    canonUpdateEndPoint(x, y, z, a, b, c);}void STRAIGHT_PROBE(double x, double y, double z, double a, double b,		    double c){    double ini_maxvel, vel, acc;    EMC_TRAJ_PROBE probeMsg;    // 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    probeMsg.pos.tran.x = TO_EXT_LEN(x);    probeMsg.pos.tran.y = TO_EXT_LEN(y);    probeMsg.pos.tran.z = TO_EXT_LEN(z);    // fill in the orientation    probeMsg.pos.a = TO_EXT_ANG(a);    probeMsg.pos.b = TO_EXT_ANG(b);    probeMsg.pos.c = TO_EXT_ANG(c);    ini_maxvel = vel = getStraightVelocity(x, y, z, a, b, c);    if (linear_move && !angular_move) {	if (vel > currentLinearFeedRate) {	    vel = currentLinearFeedRate;	}    } else if (!linear_move && angular_move) {	if (vel > currentAngularFeedRate) {	    vel = currentAngularFeedRate;	}    } else if (linear_move && angular_move) {	if (vel > currentLinearFeedRate) {	    vel = currentLinearFeedRate;	}    }    sendVelMsg(vel, ini_maxvel);    if((acc = getStraightAcceleration(x, y, z, a, b, c)))        sendAccMsg(acc);    interp_list.append(probeMsg);    canonUpdateEndPoint(x, y, z, a, b, c);}/* Machining Attributes */void SET_MOTION_CONTROL_MODE(CANON_MOTION_MODE mode, double tolerance){    EMC_TRAJ_SET_TERM_COND setTermCondMsg;    if ((mode != canonMotionMode) || (TO_EXT_LEN(FROM_PROG_LEN(tolerance)) != canonMotionTolerance)) {	canonMotionMode = mode;	canonMotionTolerance =  TO_EXT_LEN(FROM_PROG_LEN(tolerance));	switch (mode) {	case CANON_CONTINUOUS:	    setTermCondMsg.cond = EMC_TRAJ_TERM_COND_BLEND;	    setTermCondMsg.tolerance = canonMotionTolerance;	    break;	default:	    setTermCondMsg.cond = EMC_TRAJ_TERM_COND_STOP;	    break;	}	interp_list.append(setTermCondMsg);    }}CANON_MOTION_MODE GET_MOTION_CONTROL_MODE(){    return canonMotionMode;}double GET_MOTION_CONTROL_TOLERANCE(){    return canonMotionTolerance;}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(double sync){    EMC_TRAJ_SET_SPINDLESYNC spindlesyncMsg;    spindlesyncMsg.spindlesync = TO_EXT_LEN(FROM_PROG_LEN(sync));    interp_list.append(spindlesyncMsg);}void STOP_SPEED_FEED_SYNCH(){    EMC_TRAJ_SET_SPINDLESYNC spindlesyncMsg;    spindlesyncMsg.spindlesync = 0.0;    interp_list.append(spindlesyncMsg);}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, double a, double b, double c){    EmcPose end;    PM_CARTESIAN center, normal;    EMC_TRAJ_CIRCULAR_MOVE circularMoveMsg;    EMC_TRAJ_LINEAR_MOVE linearMoveMsg;    double v1, v2, a1, a2, vel, ini_maxvel, acc=0.0;    double radius, angle, theta1, theta2, helical_length, axis_len;    double tmax, thelix, ta, tb, tc, da, db, dc;    /* In response to  Bugs item #1274108 - rotary axis moves when coordinate       offsets used with A. Original code failed to include programOrigin on       rotary moves. */    a = FROM_PROG_ANG(a);    b = FROM_PROG_ANG(b);    c = FROM_PROG_ANG(c);    a += programOrigin.a;    b += programOrigin.b;    c += programOrigin.c;    da = fabs(canonEndPoint.a - a);    db = fabs(canonEndPoint.b - b);    dc = fabs(canonEndPoint.c - c);    /* Since there's no default case here,       we need to initialise vel to something safe! */    vel = ini_maxvel = currentLinearFeedRate;    // 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) {    default: // to eliminate "uninitalized" warnings    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;        theta1 = atan2(canonEndPoint.y - center.y, canonEndPoint.x - center.x);        theta2 = atan2(end.tran.y - center.y, end.tran.x - center.x);        radius = hypot(canonEndPoint.x - center.x, canonEndPoint.y - center.y);        axis_len = fabs(end.tran.z - canonEndPoint.z);	v1 = FROM_EXT_LEN(AXIS_MAX_VELOCITY[0]);	v2 = FROM_EXT_LEN(AXIS_MAX_VELOCITY[1]);	a1 = FROM_EXT_LEN(AXIS_MAX_ACCELERATION[0]);	a2 = FROM_EXT_LEN(AXIS_MAX_ACCELERATION[1]);        ini_maxvel = MIN(v1, v2);        acc = MIN(a1, a2);        if(axis_len > 0.001) {            v1 = FROM_EXT_LEN(AXIS_MAX_VELOCITY[2]);            a1 = FROM_EXT_LEN(AXIS_MAX_ACCELERATION[2]);            ini_maxvel = MIN(ini_maxvel, v1);            acc = MIN(acc, a1);        }	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.z += currentToolLengthOffset;	center.x = end.tran.x;	normal.y = 0.0;	normal.z = 0.0;	normal.x = 1.0;        theta1 = atan2(canonEndPoint.z - center.z, canonEndPoint.y - center.y);        theta2 = atan2(end.tran.z - center.z, end.tran.y - center.y);        radius = hypot(canonEndPoint.y - center.y, canonEndPoint.z - center.z);        axis_len = fabs(end.tran.x - canonEndPoint.x);	v1 = FROM_EXT_LEN(AXIS_MAX_VELOCITY[1]);	v2 = FROM_EXT_LEN(AXIS_MAX_VELOCITY[2]);	a1 = FROM_EXT_LEN(AXIS_MAX_ACCELERATION[1]);	a2 = FROM_EXT_LEN(AXIS_MAX_ACCELERATION[2]);        ini_maxvel = MIN(v1, v2);        acc = MIN(a1, a2);        if(axis_len > 0.001) {            v1 = FROM_EXT_LEN(AXIS_MAX_VELOCITY[0]);            a1 = FROM_EXT_LEN(AXIS_MAX_ACCELERATION[0]);            ini_maxvel = MIN(ini_maxvel, v1);            acc = MIN(acc, a1);        }	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.z += currentToolLengthOffset;	center.x = second_axis + programOrigin.x;	center.y = end.tran.y;	normal.z = 0.0;	normal.x = 0.0;	normal.y = 1.0;        theta1 = atan2(canonEndPoint.x - center.x, canonEndPoint.z - center.z);        theta2 = atan2(end.tran.x - center.x, end.tran.z - center.z);        radius = hypot(canonEndPoint.x - center.x, canonEndPoint.z - center.z);        axis_len = fabs(end.tran.y - canonEndPoint.y);	v1 = FROM_EXT_LEN(AXIS_MAX_VELOCITY[0]);	v2 = FROM_EXT_LEN(AXIS_MAX_VELOCITY[2]);	a1 = FROM_EXT_LEN(AXIS_MAX_ACCELERATION[0]);	a2 = FROM_EXT_LEN(AXIS_MAX_ACCELERATION[2]);	ini_maxvel = MIN(v1, v2);        acc = MIN(a1, a2);        if(axis_len > 0.001) {            v1 = FROM_EXT_LEN(AXIS_MAX_VELOCITY[1]);            a1 = FROM_EXT_LEN(AXIS_MAX_ACCELERATION[1]);            ini_maxvel = MIN(ini_maxvel, v1);            acc = MIN(acc, a1);        }	break;    }    if(rotation < 0) {        if(theta2 >= theta1) theta2 -= M_PI * 2.0;    } else {        if(theta2 <= theta1) theta2 += M_PI * 2.0;    }    angle = theta2 - theta1;    helical_length = hypot(angle * radius, axis_len);    thelix = fabs(helical_length / ini_maxvel);    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 = MAX4(thelix, ta, tb, tc);    if (tmax <= 0.0) {        vel = currentLinearFeedRate;    } else {        ini_maxvel = helical_length / tmax;        vel = MIN(vel, ini_maxvel);    }    // for arcs we always user linear move, as the ABC axes can't move (currently)    // linear move is actually a move involving axes X Y Z, not the type of the movement    linear_move = 1;    // set proper velocity    sendVelMsg(vel, ini_maxvel);    thelix = (helical_length / acc);    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 = MAX4(thelix, ta, tb, tc);    if (tmax > 0.0) {        acc = helical_length / tmax;    }    if(acc) sendAccMsg(acc);    /*        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.a = TO_EXT_ANG(a);	linearMoveMsg.end.b = TO_EXT_ANG(b);	linearMoveMsg.end.c = TO_EXT_ANG(c);        linearMoveMsg.type = EMC_MOTION_TYPE_ARC;	interp_list.append(linearMoveMsg);    } else if (rotation > 0) {	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.a = TO_EXT_ANG(a);	circularMoveMsg.end.b = TO_EXT_ANG(b);	circularMoveMsg.end.c = TO_EXT_ANG(c);        circularMoveMsg.type = EMC_MOTION_TYPE_ARC;	interp_list.append(circularMoveMsg);    } else {	// reverse turn	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.a = TO_EXT_ANG(a);	circularMoveMsg.end.b = TO_EXT_ANG(b);	circularMoveMsg.end.c = TO_EXT_ANG(c);        circularMoveMsg.type = EMC_MOTION_TYPE_ARC;	interp_list.append(circularMoveMsg);    }    // update the end point    canonUpdateEndPoint(end.tran.x, end.tran.y, end.tran.z, a, b, c);}/*  STRAIGHT_PROBE is exactly the same as STRAIGHT_FEED, except that it  uses a probe message instead of a linear move message.*/void DWELL(double seconds){    EMC_TRAJ_DELAY delayMsg;    delayMsg.delay = seconds;    interp_list.append(delayMsg);}/* Spindle Functions */void SPINDLE_RETRACT_TRAVERSE(){    /*! \todo 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;    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 clockwise.");    emc_spindle_on_msg.speed = spindleSpeed;    interp_list.append(emc_spindle_on_msg);