From: Joseph Coffland Date: Sun, 11 Sep 2016 10:49:27 +0000 (-0700) Subject: Implemented runtime tool change, added more arc error checking X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=3d63c11485c417277a03ec9cb76b1a609251effc;p=bbctrl-firmware Implemented runtime tool change, added more arc error checking --- diff --git a/src/config.h b/src/config.h index 82b895f..59f3472 100644 --- a/src/config.h +++ b/src/config.h @@ -266,6 +266,7 @@ typedef enum { #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 diff --git a/src/gcode_parser.c b/src/gcode_parser.c index c6982e9..cc602c9 100644 --- a/src/gcode_parser.c +++ b/src/gcode_parser.c @@ -48,7 +48,7 @@ parser_t parser = {{0}}; {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 @@ -170,8 +170,13 @@ static stat_t _get_next_gcode_word(char **pstr, char *letter, float *value) { /// 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; } @@ -186,12 +191,12 @@ static stat_t _validate_gcode_block() { // 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; } @@ -222,7 +227,7 @@ static stat_t _validate_gcode_block() { * 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) @@ -261,6 +266,7 @@ static stat_t _execute_gcode_block() { 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) { @@ -323,9 +329,10 @@ static stat_t _execute_gcode_block() { 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 } diff --git a/src/machine.c b/src/machine.c index 0a758f5..a7dfda0 100644 --- a/src/machine.c +++ b/src/machine.c @@ -237,9 +237,6 @@ void mach_set_spindle_mode(spindle_mode_t mode) { } -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; } @@ -555,6 +552,7 @@ void machine_init() { 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 @@ -582,6 +580,12 @@ void mach_set_distance_mode(distance_mode_t mode) { } +/// 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. @@ -744,9 +748,9 @@ stat_t mach_rapid(float target[], float flags[]) { 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; } @@ -817,7 +821,7 @@ void mach_set_path_mode(path_mode_t mode) { /// G4, P parameter (seconds) stat_t mach_dwell(float seconds) { - return mp_dwell(seconds, mach.gm.line); + return mp_dwell(seconds, mach_get_line()); } @@ -836,9 +840,9 @@ stat_t mach_feed(float target[], float flags[]) { 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; } @@ -852,8 +856,20 @@ stat_t mach_feed(float target[], float flags[]) { 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) @@ -959,7 +975,7 @@ static stat_t _exec_program_stop(mp_buffer_t *bf) { /// 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()); } @@ -1014,6 +1030,7 @@ void mach_program_end() { 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 diff --git a/src/machine.h b/src/machine.h index 2a51b3e..10c3bc0 100644 --- a/src/machine.h +++ b/src/machine.h @@ -84,7 +84,6 @@ PGM_P mach_get_distance_mode_pgmstr(distance_mode_t mode); 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); @@ -107,6 +106,7 @@ void machine_init(); 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[]); @@ -136,15 +136,16 @@ void mach_set_path_mode(path_mode_t mode); // 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); diff --git a/src/messages.def b/src/messages.def index b731d73..72134e4 100644 --- a/src/messages.def +++ b/src/messages.def @@ -58,6 +58,7 @@ STAT_MSG(INPUT_EXCEEDS_MAX_VALUE, "Input exceeds maximum value") 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") @@ -65,7 +66,9 @@ STAT_MSG(GCODE_FEEDRATE_NOT_SPECIFIED, "Feedrate not specified") 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") diff --git a/src/plan/arc.c b/src/plan/arc.c index 6f0809b..6fff741 100644 --- a/src/plan/arc.c +++ b/src/plan/arc.c @@ -225,10 +225,10 @@ static stat_t _compute_arc_offsets_from_radius(float offset[]) { * * 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]); @@ -340,77 +340,93 @@ static stat_t _compute_arc(const float position[], float offset[], * 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]) && @@ -425,15 +441,20 @@ stat_t mach_arc_feed(float target[], float flags[], // arc endpoints 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 diff --git a/src/plan/runtime.c b/src/plan/runtime.c index e7fbe1c..bc95b3e 100644 --- a/src/plan/runtime.c +++ b/src/plan/runtime.c @@ -52,9 +52,14 @@ typedef struct { 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; @@ -68,17 +73,6 @@ typedef struct { 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; @@ -86,13 +80,10 @@ 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 diff --git a/src/plan/runtime.h b/src/plan/runtime.h index 802bbb0..ba51b1e 100644 --- a/src/plan/runtime.h +++ b/src/plan/runtime.h @@ -38,6 +38,9 @@ void mp_runtime_set_busy(bool busy); 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); diff --git a/src/varcb.c b/src/varcb.c index 4a339da..3ccb3a2 100644 --- a/src/varcb.c +++ b/src/varcb.c @@ -40,8 +40,8 @@ float get_position(int index) {return mp_runtime_get_axis_position(index);} 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() {