示例#1
0
文件: plan_arc.cpp 项目: kitling/g2
/*
 * cm_arc_feed() - canonical machine entry point for arc
 *
 * Generates an arc by queueing line segments to the move buffer. The arc is
 * approximated by generating a large number of tiny, linear segments.
 */
stat_t cm_arc_feed(float target[], float flags[],// arc endpoints
				   float i, float j, float k, 	 // raw arc offsets
				   float radius, 				 // non-zero radius implies radius mode
				   uint8_t motion_mode)			 // defined motion mode
{
	// trap zero feed rate condition
	if ((cm.gm.feed_rate_mode != INVERSE_TIME_MODE) && (fp_ZERO(cm.gm.feed_rate))) {
		return (STAT_GCODE_FEEDRATE_NOT_SPECIFIED);
	}

	// Trap conditions where no arc movement will occur, but the system is still in
	// arc motion mode - this is not an error. This can happen when a F word or M
	// word is by itself.(The tests below are organized for execution efficiency)
	if ( fp_ZERO(i) && fp_ZERO(j) && fp_ZERO(k) && fp_ZERO(radius) ) {
		if ( fp_ZERO((flags[AXIS_X] + flags[AXIS_Y] + flags[AXIS_Z] +
					  flags[AXIS_A] + flags[AXIS_B] + flags[AXIS_C]))) {
			return (STAT_OK);
		}
	}

	// set values in the Gcode model state & copy it (linenum was already captured)
	cm_set_model_target(target, flags);
	cm.gm.motion_mode = motion_mode;
	cm_set_work_offsets(&cm.gm);					// capture the fully resolved offsets to gm
	memcpy(&arc.gm, &cm.gm, sizeof(GCodeState_t));	// copy GCode context to arc singleton - some will be overwritten to run segments

	// populate the arc control singleton
	copy_vector(arc.position, cm.gmx.position);		// set initial arc position from gcode model
	arc.radius = _to_millimeters(radius);			// set arc radius or zero
	arc.offset[0] = _to_millimeters(i);				// copy offsets with conversion to canonical form (mm)
	arc.offset[1] = _to_millimeters(j);
	arc.offset[2] = _to_millimeters(k);

	// Set the arc plane for the current G17/G18/G19 setting
	// Plane axis 0 and 1 are the arc plane, 2 is the linear axis normal to the arc plane
	if (cm.gm.select_plane == CANON_PLANE_XY) {	// G17 - the vast majority of arcs are in the G17 (XY) plane
		arc.plane_axis_0 = AXIS_X;
		arc.plane_axis_1 = AXIS_Y;
		arc.linear_axis  = AXIS_Z;
	} else if (cm.gm.select_plane == CANON_PLANE_XZ) {	// G18
		arc.plane_axis_0 = AXIS_X;
		arc.plane_axis_1 = AXIS_Z;
		arc.linear_axis  = AXIS_Y;
	} else if (cm.gm.select_plane == CANON_PLANE_YZ) {	// G19
		arc.plane_axis_0 = AXIS_Y;
		arc.plane_axis_1 = AXIS_Z;
		arc.linear_axis  = AXIS_X;
	}

	// compute arc runtime values and prep for execution by the callback
	ritorno(_compute_arc());

	// test arc soft limits
	stat_t status = _test_arc_soft_limits();
	if (status != STAT_OK) {
		cm.gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE;
		copy_vector(cm.gm.target, cm.gmx.position);		// reset model position
		return (cm_soft_alarm(status));
	}

	cm_cycle_start();						// if not already started
	arc.run_state = MOVE_RUN;				// enable arc to be run from the callback
	cm_finalize_move();
	return (STAT_OK);
}
示例#2
0
文件: plan_arc.c 项目: jtronics/TinyG
/*
 * cm_arc_feed() - canonical machine entry point for arc
 *
 * Generates an arc by queuing line segments to the move buffer. The arc is
 * approximated by generating a large number of tiny, linear arc_segments.
 */
stat_t cm_arc_feed(float target[], float flags[],       // arc endpoints
				   float i, float j, float k,           // raw arc offsets
				   float radius,                        // non-zero radius implies radius mode
				   uint8_t motion_mode)                 // defined motion mode
{
	////////////////////////////////////////////////////
	// Set axis plane and trap arc specification errors

	// trap missing feed rate
	if ((cm.gm.feed_rate_mode != INVERSE_TIME_MODE) && (fp_ZERO(cm.gm.feed_rate))) {
    	return (STAT_GCODE_FEEDRATE_NOT_SPECIFIED);
	}

    // set radius mode flag and do simple test(s)
	bool radius_f = fp_NOT_ZERO(cm.gf.arc_radius);			    // set true if radius arc
    if ((radius_f) && (cm.gn.arc_radius < MIN_ARC_RADIUS)) {    // radius value must be + and > minimum radius
        return (STAT_ARC_RADIUS_OUT_OF_TOLERANCE);
    }

    // setup some flags
	bool target_x = fp_NOT_ZERO(flags[AXIS_X]);	                // set true if X axis has been specified
	bool target_y = fp_NOT_ZERO(flags[AXIS_Y]);
	bool target_z = fp_NOT_ZERO(flags[AXIS_Z]);

    bool offset_i = fp_NOT_ZERO(cm.gf.arc_offset[0]);	        // set true if offset I has been specified
    bool offset_j = fp_NOT_ZERO(cm.gf.arc_offset[1]);           // J
    bool offset_k = fp_NOT_ZERO(cm.gf.arc_offset[2]);           // K

	// Set the arc plane for the current G17/G18/G19 setting and test arc specification
	// Plane axis 0 and 1 are the arc plane, the linear axis is normal to the arc plane.
	if (cm.gm.select_plane == CANON_PLANE_XY) {	// G17 - the vast majority of arcs are in the G17 (XY) plane
    	arc.plane_axis_0 = AXIS_X;
    	arc.plane_axis_1 = AXIS_Y;
    	arc.linear_axis  = AXIS_Z;
        if (radius_f) {
            if (!(target_x || target_y)) {                      // must have at least one endpoint specified
        	    return (STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE);
            }
        } else { // center format arc tests
            if (offset_k) { // it's OK to be missing either or both i and j, but error if k is present
        	    return (STAT_ARC_SPECIFICATION_ERROR);
            }
        }

    } else if (cm.gm.select_plane == CANON_PLANE_XZ) {	// G18
    	arc.plane_axis_0 = AXIS_X;
    	arc.plane_axis_1 = AXIS_Z;
    	arc.linear_axis  = AXIS_Y;
        if (radius_f) {
            if (!(target_x || target_z))
                return (STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE);
        } else {
            if (offset_j)
                return (STAT_ARC_SPECIFICATION_ERROR);
        }

    } else if (cm.gm.select_plane == CANON_PLANE_YZ) {	// G19
    	arc.plane_axis_0 = AXIS_Y;
    	arc.plane_axis_1 = AXIS_Z;
    	arc.linear_axis  = AXIS_X;
        if (radius_f) {
            if (!(target_y || target_z))
                return (STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE);
        } else {
            if (offset_i)
                return (STAT_ARC_SPECIFICATION_ERROR);
        }
	}

	// set values in the Gcode model state & copy it (linenum was already captured)
	cm_set_model_target(target, flags);

    // in radius mode it's an error for start == end
    if(radius_f) {
        if ((fp_EQ(cm.gmx.position[AXIS_X], cm.gm.target[AXIS_X])) &&
            (fp_EQ(cm.gmx.position[AXIS_Y], cm.gm.target[AXIS_Y])) &&
            (fp_EQ(cm.gmx.position[AXIS_Z], cm.gm.target[AXIS_Z]))) {
            return (STAT_ARC_ENDPOINT_IS_STARTING_POINT);
        }
    }

    // now get down to the rest of the work setting up the arc for execution
	cm.gm.motion_mode = motion_mode;
	cm_set_work_offsets(&cm.gm);					// capture the fully resolved offsets to gm
	memcpy(&arc.gm, &cm.gm, sizeof(GCodeState_t));	// copy GCode context to arc singleton - some will be overwritten to run segments
	copy_vector(arc.position, cm.gmx.position);		// set initial arc position from gcode model

	arc.radius = _to_millimeters(radius);			// set arc radius or zero

	arc.offset[0] = _to_millimeters(i);				// copy offsets with conversion to canonical form (mm)
	arc.offset[1] = _to_millimeters(j);
	arc.offset[2] = _to_millimeters(k);

	arc.rotations = floor(fabs(cm.gn.parameter));   // P must be a positive integer - force it if not

	// determine if this is a full circle arc. Evaluates true if no target is set
	arc.full_circle = (fp_ZERO(flags[arc.plane_axis_0]) & fp_ZERO(flags[arc.plane_axis_1]));

	// compute arc runtime values
	ritorno(_compute_arc());

	if (fp_ZERO(arc.length)) {
        return (STAT_MINIMUM_LENGTH_MOVE);          // trap zero length arcs that _compute_arc can throw
    }

/*	// test arc soft limits
	stat_t status = _test_arc_soft_limits();
	if (status != STAT_OK) {
    	cm.gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE;
    	copy_vector(cm.gm.target, cm.gmx.position);		// reset model position
    	return (cm_soft_alarm(status));
	}
*/
	cm_cycle_start();						// if not already started
	arc.run_state = MOVE_RUN;				// enable arc to be run from the callback
	cm_finalize_move();
	return (STAT_OK);
}