#define GCODE_DEFAULT_COORD_SYSTEM G54 // G54, G55, G56, G57, G58 or G59
#define GCODE_DEFAULT_PATH_CONTROL PATH_CONTINUOUS
#define GCODE_DEFAULT_DISTANCE_MODE ABSOLUTE_MODE
+#define GCODE_DEFAULT_ARC_DISTANCE_MODE INCREMENTAL_MODE
// Motor fault ISRs
{parser.gn.parm = val; parser.gf.parm = 1; modals[m] += 1; break;}
#define SET_NON_MODAL(parm, val) \
{parser.gn.parm = val; parser.gf.parm = 1; break;}
-#define EXEC_FUNC(f, v) if ((uint8_t)parser.gf.v) f(parser.gn.v)
+#define EXEC_FUNC(f, parm) if ((uint8_t)parser.gf.parm) f(parser.gn.parm)
static uint8_t modals[MODAL_GROUP_COUNT]; // collects modal groups in a block
/// Isolate the decimal point value as an integer
-static uint8_t _point(float value) {
- return value * 10 - trunc(value) * 10;
+static uint8_t _point(float value) {return value * 10 - trunc(value) * 10;}
+
+
+static bool _axis_changed() {
+ for (int axis = 0; axis < AXES; axis++)
+ if (parser.gf.target[axis]) return true;
+ return false;
}
// activity of the group 1 G-code is suspended for that line. The
// axis word-using G-codes from group 0 are G10, G28, G30, and G92"
- // if (modals[MODAL_GROUP_G0] && modals[MODAL_GROUP_G1])
- // return STAT_MODAL_GROUP_VIOLATION;
+ if (modals[MODAL_GROUP_G0] && modals[MODAL_GROUP_G1])
+ return STAT_MODAL_GROUP_VIOLATION;
// look for commands that require an axis word to be present
- // if (modals[MODAL_GROUP_G0] || modals[MODAL_GROUP_G1])
- // if (!_axis_changed()) return STAT_GCODE_AXIS_IS_MISSING;
+ if (modals[MODAL_GROUP_G0] || modals[MODAL_GROUP_G1])
+ if (!_axis_changed()) return STAT_GCODE_AXIS_IS_MISSING;
return STAT_OK;
}
* 14. cutter length compensation on or off (G43, G49)
* 15. coordinate system selection (G54, G55, G56, G57, G58, G59)
* 16. set path control mode (G61, G61.1, G64)
- * 17. set distance mode (G90, G91)
+ * 17. set distance mode (G90, G91, G90.1, G91.1)
* 18. set retract mode (G98, G99)
* 19a. homing functions (G28.2, G28.3, G28.1, G28, G30)
* 19b. update system data (G10)
EXEC_FUNC(mach_set_coord_system, coord_system);
EXEC_FUNC(mach_set_path_mode, path_mode);
EXEC_FUNC(mach_set_distance_mode, distance_mode);
+ EXEC_FUNC(mach_set_arc_distance_mode, arc_distance_mode);
//--> set retract mode goes here
switch (parser.gn.next_action) {
case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC:
// gf.radius sets radius mode if radius was collected in gn
status = mach_arc_feed(parser.gn.target, parser.gf.target,
- parser.gn.arc_offset[0], parser.gn.arc_offset[1],
- parser.gn.arc_offset[2], parser.gn.arc_radius,
- parser.gn.motion_mode);
+ parser.gn.arc_offset, parser.gf.arc_offset,
+ parser.gn.arc_radius, parser.gf.arc_radius,
+ parser.gn.parameter, parser.gf.parameter,
+ modals[MODAL_GROUP_G1], parser.gn.motion_mode);
break;
default: break; // Should not get here
}
}
-void mach_set_tool_number(uint8_t tool) {mach.gm.tool = tool;}
-
-
void mach_set_absolute_mode(bool absolute_mode) {
mach.gm.absolute_mode = absolute_mode;
}
mach_set_plane(GCODE_DEFAULT_PLANE);
mach_set_path_mode(GCODE_DEFAULT_PATH_CONTROL);
mach_set_distance_mode(GCODE_DEFAULT_DISTANCE_MODE);
+ mach_set_arc_distance_mode(GCODE_DEFAULT_ARC_DISTANCE_MODE);
mach_set_feed_mode(UNITS_PER_MINUTE_MODE); // always the default
// Sub-system inits
}
+/// G90.1, G91.1
+void mach_set_arc_distance_mode(distance_mode_t mode) {
+ mach.gm.arc_distance_mode = mode;
+}
+
+
/* G10 L2 Pn, delayed persistence
*
* This function applies the offset to the GM model.
if (status != STAT_OK) return ALARM(status);
// prep and plan the move
- mach_update_work_offsets(); // update fully resolved offsets to state
- status = mp_aline(mach.gm.target, mach.gm.line); // send the move to planner
- copy_vector(mach.position, mach.gm.target); // update model position
+ mach_update_work_offsets(); // update resolved offsets
+ status = mp_aline(mach.gm.target, mach_get_line()); // send move to planner
+ copy_vector(mach.position, mach.gm.target); // update model position
return status;
}
/// G4, P parameter (seconds)
stat_t mach_dwell(float seconds) {
- return mp_dwell(seconds, mach.gm.line);
+ return mp_dwell(seconds, mach_get_line());
}
if (status != STAT_OK) return ALARM(status);
// prep and plan the move
- mach_update_work_offsets(); // update fully resolved offsets to state
- status = mp_aline(mach.gm.target, mach.gm.line); // send the move to planner
- copy_vector(mach.position, mach.gm.target); // update model position
+ mach_update_work_offsets(); // update resolved offsets
+ status = mp_aline(mach.gm.target, mach_get_line()); // send move to planner
+ copy_vector(mach.position, mach.gm.target); // update model position
return status;
}
void mach_select_tool(uint8_t tool_select) {mach.gm.tool_select = tool_select;}
+static stat_t _exec_change_tool(mp_buffer_t *bf) {
+ mp_runtime_set_tool(bf->value);
+ return STAT_NOOP; // No move queued
+}
+
+
/// M6 This might become a complete tool change cycle
-void mach_change_tool(uint8_t tool) {mach.gm.tool = tool;}
+void mach_change_tool(bool x) {
+ mach.gm.tool = mach.gm.tool_select;
+
+ mp_buffer_t *bf = mp_queue_get_tail();
+ bf->value = mach.gm.tool;
+ mp_queue_push(_exec_change_tool, mach_get_line());
+}
// Miscellaneous Functions (4.3.9)
/// M0 Queue a program stop
void mach_program_stop() {
- mp_queue_push(_exec_program_stop, mach.gm.line);
+ mp_queue_push(_exec_program_stop, mach_get_line());
}
mach_set_coord_system(GCODE_DEFAULT_COORD_SYSTEM);
mach_set_plane(GCODE_DEFAULT_PLANE);
mach_set_distance_mode(GCODE_DEFAULT_DISTANCE_MODE);
+ mach_set_arc_distance_mode(GCODE_DEFAULT_ARC_DISTANCE_MODE);
mach.gm.spindle_mode = SPINDLE_OFF;
spindle_set(SPINDLE_OFF, 0);
mach_flood_coolant_control(false); // M9
void mach_set_motion_mode(motion_mode_t motion_mode);
void mach_set_spindle_mode(spindle_mode_t spindle_mode);
void mach_set_spindle_speed(float speed);
-void mach_set_tool_number(uint8_t tool);
void mach_set_absolute_mode(bool absolute_mode);
void mach_set_model_line(uint32_t line);
void mach_set_plane(plane_t plane);
void mach_set_units(units_t mode);
void mach_set_distance_mode(distance_mode_t mode);
+void mach_set_arc_distance_mode(distance_mode_t mode);
void mach_set_coord_offsets(coord_system_t coord_system, float offset[],
float flag[]);
// Machining Functions (4.3.6)
stat_t mach_feed(float target[], float flags[]);
-stat_t mach_arc_feed(float target[], float flags[], float i, float j, float k,
- float radius, uint8_t motion_mode);
+stat_t mach_arc_feed(float target[], float flags[], float offsets[],
+ float offset_f[], float radius, bool radius_f,
+ float P, bool P_f, bool modal_g1_f, uint8_t motion_mode);
stat_t mach_dwell(float seconds);
// Spindle Functions (4.3.7) see spindle.h
// Tool Functions (4.3.8)
void mach_select_tool(uint8_t tool);
-void mach_change_tool(uint8_t tool);
+void mach_change_tool(bool x);
// Miscellaneous Functions (4.3.9)
void mach_mist_coolant_control(bool mist_coolant);
STAT_MSG(INPUT_VALUE_RANGE_ERROR, "Input value range error")
// Gcode errors & warnings (Most originate from NIST)
+STAT_MSG(MODAL_GROUP_VIOLATION, "Modal group violation")
STAT_MSG(GCODE_COMMAND_UNSUPPORTED, "Invalid or unsupported G-Code command")
STAT_MSG(MCODE_COMMAND_UNSUPPORTED, "M code unsupported")
STAT_MSG(GCODE_AXIS_IS_MISSING, "Axis word missing")
STAT_MSG(ARC_SPECIFICATION_ERROR, "Arc specification error")
STAT_MSG(ARC_AXIS_MISSING_FOR_SELECTED_PLANE, "Arc missing axis")
STAT_MSG(ARC_RADIUS_OUT_OF_TOLERANCE, "Arc radius arc out of tolerance")
-STAT_MSG(ARC_ENDPOINT_IS_STARTING_POINT, "Arc endpoint is starting point")
+STAT_MSG(ARC_ENDPOINT_IS_STARTING_POINT, "Arc end point is starting point")
+STAT_MSG(ARC_OFFSETS_MISSING_FOR_PLANE, "arc offsets missing for plane")
+STAT_MSG(P_WORD_IS_NOT_A_POSITIVE_INTEGER, "P word is not a positive integer")
// Errors and warnings
STAT_MSG(MINIMUM_LENGTH_MOVE, "Move less than minimum length")
*
* Parts of this routine were originally sourced from the grbl project.
*/
-static stat_t _compute_arc(const float position[], float offset[],
- float rotations, bool full_circle) {
+static stat_t _compute_arc(bool radius_f, const float position[],
+ float offset[], float rotations, bool full_circle) {
// Compute radius. A non-zero radius value indicates a radius arc
- if (fp_NOT_ZERO(arc.radius)) _compute_arc_offsets_from_radius(offset);
+ if (radius_f) _compute_arc_offsets_from_radius(offset);
else // compute start radius
arc.radius = hypotf(-offset[arc.plane_axis_0], -offset[arc.plane_axis_1]);
* Generates an arc by queuing line segments to the move buffer. The arc is
* approximated by generating a large number of tiny, linear segments.
*/
-stat_t mach_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
+stat_t mach_arc_feed(float target[], float target_f[], // arc endpoints
+ float offsets[], float offsets_f[], // arc offsets
+ float radius, bool radius_f, // radius
+ float P, bool P_f, // parameter
+ bool modal_g1_f,
uint8_t motion_mode) { // defined motion mode
+
+ // Trap some precursor cases. Since motion mode (MODAL_GROUP_G1) persists
+ // from the previous move it's possible for non-modal commands such as F or P
+ // to arrive here when no motion has actually been specified. It's also
+ // possible to run an arc as simple as "I25" if CW or CCW motion mode was
+ // already set by a previous block. Here are 2 cases to handle if CW or CCW
+ // motion mode was set by a previous block:
+ //
+ // Case 1: F, P or other non modal is specified but no movement is specified
+ // (no offsets or radius). This is OK: return STAT_OK
+ //
+ // Case 2: Movement is specified w/o a new G2 or G3 word in the (new) block.
+ // This is OK: continue the move
+ //
+ if (!modal_g1_f && !offsets_f[0] && !offsets_f[1] && !offsets_f[2] &&
+ !radius_f) return STAT_OK;
+
// trap missing feed rate
if (fp_ZERO(mach_get_feed_rate()) ||
(mach_get_feed_mode() == INVERSE_TIME_MODE && !parser.gf.feed_rate))
return STAT_GCODE_FEEDRATE_NOT_SPECIFIED;
- // set radius mode flag and do simple test(s)
- bool radius_f = fp_NOT_ZERO(parser.gf.arc_radius); // set true if radius arc
-
- // radius value must be + and > minimum radius
- if (radius_f && parser.gn.arc_radius < MIN_ARC_RADIUS)
+ // radius must be positive and > minimum
+ if (radius_f && radius < MIN_ARC_RADIUS)
return STAT_ARC_RADIUS_OUT_OF_TOLERANCE;
- // setup some flags
- bool target_x = fp_NOT_ZERO(flags[AXIS_X]); // is X axis 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(parser.gf.arc_offset[0]); // is offset I specified
- bool offset_j = fp_NOT_ZERO(parser.gf.arc_offset[1]); // J
- bool offset_k = fp_NOT_ZERO(parser.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.
- // G17 - the vast majority of arcs are in the G17 (XY) plane
switch (mach_get_plane()) {
- case PLANE_XY:
+ case PLANE_XY: // G17
arc.plane_axis_0 = AXIS_X;
arc.plane_axis_1 = AXIS_Y;
arc.linear_axis = AXIS_Z;
-
- if (radius_f) {
- // must have at least one endpoint specified
- if (!(target_x || target_y))
- return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE;
-
- } else if (offset_k)
- // center format arc tests, it's OK to be missing either or both i and j,
- // but error if k is present
- return STAT_ARC_SPECIFICATION_ERROR;
break;
case 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;
break;
case 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;
break;
}
+ // Test if endpoints are specified in the selected plane
+ bool full_circle = false;
+ if (!target_f[arc.plane_axis_0] && !target_f[arc.plane_axis_1]) {
+ if (radius_f) // in radius mode arcs missing both endpoints is an error
+ return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE;
+ else full_circle = true; // in center format arc specifies full circle
+ }
+
+ // Test radius arcs for radius tolerance
+ if (radius_f) {
+ arc.radius = TO_MILLIMETERS(radius); // set to internal format (mm)
+ if (fabs(arc.radius) < MIN_ARC_RADIUS) // radius value must be > minimum
+ return STAT_ARC_RADIUS_OUT_OF_TOLERANCE;
+
+ // Test that center format absolute distance mode arcs have both offsets
+ } else if (mach_get_arc_distance_mode() == ABSOLUTE_MODE &&
+ !(offsets_f[arc.plane_axis_0] && offsets_f[arc.plane_axis_1]))
+ return STAT_ARC_OFFSETS_MISSING_FOR_PLANE;
+
+ // Set arc rotations
+ float rotations = 0;
+
+ if (P_f) {
+ // If P is present it must be a positive integer
+ if (P < 0 || 0 < floor(P) - P) return STAT_P_WORD_IS_NOT_A_POSITIVE_INTEGER;
+
+ rotations = P;
+
+ } else if (full_circle) rotations = 1; // default to 1 for full circles
+
// set values in Gcode model state & copy it (line was already captured)
- mach_set_model_target(target, flags);
+ mach_set_model_target(target, target_f);
// in radius mode it's an error for start == end
if (radius_f && fp_EQ(mach.position[AXIS_X], mach.gm.target[AXIS_X]) &&
copy_vector(arc.target, mach.gm.target); // copy move target
arc.radius = TO_MILLIMETERS(radius); // set arc radius or zero
- float offset[3] = {TO_MILLIMETERS(i), TO_MILLIMETERS(j), TO_MILLIMETERS(k)};
- float rotations = floor(fabs(parser.gn.parameter)); // P must be positive int
+ float offset[3];
+ for (int i = 0; i < 3; i++) offset[i] = TO_MILLIMETERS(offsets[i]);
- // determine if this is a full circle arc. Evaluates true if no target is set
- bool full_circle =
- fp_ZERO(flags[arc.plane_axis_0]) & fp_ZERO(flags[arc.plane_axis_1]);
+ if (mach_get_arc_distance_mode() == ABSOLUTE_MODE) {
+ if (offsets_f[0]) offset[0] -= mach.position[0];
+ if (offsets_f[1]) offset[1] -= mach.position[1];
+ if (offsets_f[2]) offset[2] -= mach.position[2];
+ }
// compute arc runtime values
- RITORNO(_compute_arc(mach.position, offset, rotations, full_circle));
+ RITORNO(_compute_arc(radius_f, mach.position, offset, rotations,
+ full_circle));
+
+ // Note, arc soft limits are not tested here
arc.running = true; // Enable arc run in callback
mach_arc_callback(); // Queue initial arc moves
typedef struct {
- int32_t line; // Current move GCode line number
+ bool busy; // True if a move is running
+ float position[AXES]; // Current move position
+ float work_offset[AXES]; // Current move work offset
+ float velocity; // Current move velocity
+ mp_steps_t steps;
- uint8_t tool;
+ int32_t line; // Current move GCode line number
+ uint8_t tool; // Active tool
float feed;
feed_mode_t feed_mode;
path_mode_t path_mode;
distance_mode_t distance_mode;
distance_mode_t arc_distance_mode;
-
-} mach_state_t;
-
-
-typedef struct {
- bool busy; // True if a move is running
- float position[AXES]; // Current move position
- float work_offset[AXES]; // Current move work offset
- float velocity; // Current move velocity
- mp_steps_t steps;
- mach_state_t mach;
} mp_runtime_t;
static mp_runtime_t rt;
bool mp_runtime_is_busy() {return rt.busy;}
void mp_runtime_set_busy(bool busy) {rt.busy = busy;}
-int32_t mp_runtime_get_line() {return rt.mach.line;}
-
-
-void mp_runtime_set_line(int32_t line) {
- rt.mach.line = line;
- report_request();
-}
+int32_t mp_runtime_get_line() {return rt.line;}
+void mp_runtime_set_line(int32_t line) {rt.line = line; report_request();}
+uint8_t mp_runtime_get_tool() {return rt.tool;}
+void mp_runtime_set_tool(uint8_t tool) {rt.tool = tool; report_request();}
/// Returns current segment velocity
int32_t mp_runtime_get_line();
void mp_runtime_set_line(int32_t line);
+uint8_t mp_runtime_get_tool();
+void mp_runtime_set_tool(uint8_t tool);
+
float mp_runtime_get_velocity();
void mp_runtime_set_velocity(float velocity);
int32_t get_line() {return mp_runtime_get_line();}
PGM_P get_unit() {return mach_get_units_pgmstr(mach_get_units());}
float get_speed() {return spindle_get_speed();}
-float get_feed() {return mach_get_feed_rate();}
-uint8_t get_tool() {return mach_get_tool();}
+float get_feed() {return mach_get_feed_rate();} // TODO get runtime value
+uint8_t get_tool() {return mp_runtime_get_tool();}
PGM_P get_feed_mode() {
projects / bbctrl-firmware / commitdiff