emccanon.cc

CNC 的开放码,EMC2 V2.2.8版

void START_SPEED_FEED_SYNCH(double feed_per_revolution, bool velocity_mode)
{
    flush_segments();
    EMC_TRAJ_SET_SPINDLESYNC spindlesyncMsg;
    spindlesyncMsg.feed_per_revolution = TO_EXT_LEN(FROM_PROG_LEN(feed_per_revolution));
    spindlesyncMsg.velocity_mode = velocity_mode;
    interp_list.append(spindlesyncMsg);
}

void STOP_SPEED_FEED_SYNCH()
{
    flush_segments();
    EMC_TRAJ_SET_SPINDLESYNC spindlesyncMsg;
    spindlesyncMsg.feed_per_revolution = 0.0;
    spindlesyncMsg.velocity_mode = false;
    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,
              double u, double v, double w)
{
    EmcPose end;
    PM_CARTESIAN center, normal;
    EMC_TRAJ_CIRCULAR_MOVE circularMoveMsg;
    EMC_TRAJ_LINEAR_MOVE linearMoveMsg;
    double v1, v2,  a1, a2, vel, ini_maxvel, circ_maxvel, axial_maxvel=0.0, circ_acc, axial_acc, acc=0.0;
    double radius, angle, theta1, theta2, helical_length, axis_len;
    double tcircle, taxial, tmax, thelix, ta, tb, tc, da, db, dc;
    double tu, tv, tw, du, dv, dw;
    
    //ini_maxvel = max vel defined by various ini constraints
    //circ_maxvel = max vel defined by ini constraints in the circle plane (XY, YZ or XZ)
    //axial_maxvel = max vel defined by ini constraints in the axial direction (Z, X or Y)

    linearMoveMsg.feed_mode = feed_mode;
    circularMoveMsg.feed_mode = feed_mode;
    flush_segments();
    /* 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;

    u = FROM_PROG_LEN(u);
    v = FROM_PROG_LEN(v);
    w = FROM_PROG_LEN(w);
    u += programOrigin.u;
    v += programOrigin.v;
    w += programOrigin.w;

    da = fabs(canonEndPoint.a - a);
    db = fabs(canonEndPoint.b - b);
    dc = fabs(canonEndPoint.c - c);

    du = fabs(canonEndPoint.u - u);
    dv = fabs(canonEndPoint.v - v);
    dw = fabs(canonEndPoint.w - w);

    /* 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.x += currentXToolOffset;
	end.tran.z += currentZToolOffset;
	center.x = first_axis + programOrigin.x;
        center.x += currentXToolOffset;
	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]);
        circ_maxvel = ini_maxvel = MIN(v1, v2);
        circ_acc = acc = MIN(a1, a2);
        if(axis_len > 0.001) {
            axial_maxvel = v1 = FROM_EXT_LEN(AXIS_MAX_VELOCITY[2]);
            axial_acc = a1 = FROM_EXT_LEN(AXIS_MAX_ACCELERATION[2]);
            ini_maxvel = MIN(ini_maxvel, v1);
            acc = MIN(acc, a1);
        } else {
	    axis_len = 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.x += currentXToolOffset;
	end.tran.z += currentZToolOffset;

	center.y = first_axis + programOrigin.y;
	center.z = second_axis + programOrigin.z;
	center.z += currentZToolOffset;
	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]);
        circ_maxvel = ini_maxvel = MIN(v1, v2);
        circ_acc = acc = MIN(a1, a2);
        if(axis_len > 0.001) {
            axial_maxvel = v1 = FROM_EXT_LEN(AXIS_MAX_VELOCITY[0]);
            axial_acc = a1 = FROM_EXT_LEN(AXIS_MAX_ACCELERATION[0]);
            ini_maxvel = MIN(ini_maxvel, v1);
            acc = MIN(acc, a1);
        } else {
	    axis_len = 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.x += currentXToolOffset;
	end.tran.z += currentZToolOffset;

	center.z = first_axis + programOrigin.z;
	center.z += currentZToolOffset;
	center.x = second_axis + programOrigin.x;
	center.x += currentXToolOffset;
	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]);
	circ_maxvel = ini_maxvel = MIN(v1, v2);
        circ_acc = acc = MIN(a1, a2);
        if(axis_len > 0.001) {
            axial_maxvel = v1 = FROM_EXT_LEN(AXIS_MAX_VELOCITY[1]);
            axial_acc = a1 = FROM_EXT_LEN(AXIS_MAX_ACCELERATION[1]);
            ini_maxvel = MIN(ini_maxvel, v1);
            acc = MIN(acc, a1);
        } else {
	    axis_len = 0;
	}
	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);

// COMPUTE VELOCITIES
    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;

    tu = du? (du / FROM_EXT_LEN(AXIS_MAX_VELOCITY[6])): 0.0;
    tv = dv? (dv / FROM_EXT_LEN(AXIS_MAX_VELOCITY[7])): 0.0;
    tw = dw? (dw / FROM_EXT_LEN(AXIS_MAX_VELOCITY[8])): 0.0;

    //we have accel, check what the max_vel is that doesn't violate the centripetal accel=accel
    v1 = sqrt(circ_acc * radius);
    circ_maxvel = MIN(v1, circ_maxvel);

    // find out how long the arc takes at ini_maxvel
    tcircle = fabs(angle * radius / circ_maxvel);

    taxial = fabs(axis_len / axial_maxvel);
    tmax = MAX(taxial, tcircle);
    tmax = MAX4(tmax, ta, tb, tc);
    tmax = MAX4(tmax, tu, tv, tw);

    if (tmax <= 0.0) {
        vel = currentLinearFeedRate;
    } else {
        ini_maxvel = helical_length / tmax; //compute the new maxvel based on all previous constraints
        vel = MIN(vel, ini_maxvel); //the programmed vel is either feedrate or machine_maxvel if lower
    }

    // for arcs we always user linear move since there is no
    // arc possible with only ABC motion

    cartesian_move = 1;

// COMPUTE ACCELS

    // the next calcs are not really times.  the units are time^2, but
    // the division at the end gives the right units for accel.  if you
    // try to think of these in terms of any real-world value (time to
    // do what?), you're probably doomed.  think of them as a parametric
    // expression of the acceleration in the various directions.

    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;

    tu = du? (du / FROM_EXT_LEN(AXIS_MAX_ACCELERATION[6])): 0.0;
    tv = dv? (dv / FROM_EXT_LEN(AXIS_MAX_ACCELERATION[7])): 0.0;
    tw = dw? (dw / FROM_EXT_LEN(AXIS_MAX_ACCELERATION[8])): 0.0;

    tmax = MAX4(thelix, ta, tb, tc);
    tmax = MAX4(tmax, tu, tv, tw);

    if (tmax > 0.0) {
        acc = helical_length / tmax;
    }

    /* 
       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.end.u = TO_EXT_LEN(u);
	linearMoveMsg.end.v = TO_EXT_LEN(v);
	linearMoveMsg.end.w = TO_EXT_LEN(w);

        linearMoveMsg.type = EMC_MOTION_TYPE_ARC;
        linearMoveMsg.vel = toExtVel(vel);
        linearMoveMsg.ini_maxvel = toExtVel(ini_maxvel);
        linearMoveMsg.acc = toExtAcc(acc);
        if(acc)
            interp_list.append(linearMoveMsg);
    } else {
	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 = normal;

        if (rotation > 0)
            circularMoveMsg.turn = rotation - 1;
        else
            // reverse turn
            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.end.u = TO_EXT_LEN(u);
	circularMoveMsg.end.v = TO_EXT_LEN(v);
	circularMoveMsg.end.w = TO_EXT_LEN(w);

        circularMoveMsg.type = EMC_MOTION_TYPE_ARC;

        // These are suboptimal but safe values.  The actual maximums
        // are hard to calculate but may be somewhat larger than
        // these.  Imagine an arc with very large radius going from
        // 0,0,0 to 1,1,1 on a machine with maxvel=1 and maxaccel=1 on
        // all axes.  The actual maximums will be near sqrt(3) but
        // we'll be using 1 instead.
        circularMoveMsg.vel = toExtVel(vel);
        circularMoveMsg.ini_maxvel = toExtVel(ini_maxvel);
        circularMoveMsg.acc = toExtAcc(acc);
        if(acc)
            interp_list.append(circularMoveMsg);
    }
    // update the end point
    canonUpdateEndPoint(end.tran.x, end.tran.y, end.tran.z, a, b, c, u, v, w);
}


void DWELL(double seconds)
{
    EMC_TRAJ_DELAY delayMsg;

    flush_segments();

    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 SET_SPINDLE_MODE(double css_max) {
    css_maximum = css_max;
}

void START_SPINDLE_CLOCKWISE()
{
    EMC_SPINDLE_ON emc_spindle_on_msg;

    flush_segments();

    if(css_maximum) {
	if(lengthUnits == CANON_UNITS_INCHES) 
	    css_numerator = 12 / (2 * M_PI) * spindleSpeed * TO_EXT_LEN(25.4);
	else
	    css_numerator = 1000 / (2 * M_PI) * spindleSpeed * TO_EXT_LEN(1);
	emc_spindle_on_msg.speed = css_maximum;
	emc_spindle_on_msg.factor = css_numerator;
	emc_spindle_on_msg.xoffset = TO_EXT_LEN(currentXToolOffset);
    } else {
	emc_spindle_on_msg.speed = spindleSpeed;
	css_numerator = 0;
    }
    interp_list.append(emc_spindle_on_msg);

    spindleOn = 1;
}

void START_SPINDLE_COUNTERCLOCKWISE()
{
    EMC_SPINDLE_ON emc_spindle_on_msg;

    flush_segments();

    if(css_maximum) {
	if(lengthUnits == CANON_UNITS_INCHES) 
	    css_numerator = -12 / (2 * M_PI) * spindleSpeed;
	else
	    css_numerator = -1000 / (2 * M_PI) * spindleSpeed;
	emc_spindle_on_msg.speed = css_maximum;
	emc_spindle_on_msg.factor = css_numerator;
	emc_spindle_on_msg.xoffset = TO_EXT_LEN(currentXToolOffset);
    } else {
	emc_spindle_on_msg.speed = -spindleSpeed;
	css_numerator = 0;
    }


    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;

    flush_segments();

    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 xoffset, double zoffset)
{
    EMC_TRAJ_SET_OFFSET set_offset_msg;

    flush_segments();

    /* convert to mm units for internal canonical use */
    currentXToolOffset = FROM_PROG_LEN(xoffset);
    currentZToolOffset = FROM_PROG_LEN(zoffset);

    /* append it to interp list so it gets updated at the right time, not at