emccanon.cc

Source code for an Numeric Cmputer

    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);