From 8baf7dfabe2c168e27f94b67848ef304cedad23f Mon Sep 17 00:00:00 2001 From: "alden.hart" Date: Sun, 2 Mar 2014 12:36:24 -0500 Subject: [PATCH] 033.04 tracking tinyg 418.04 - ready to test --- TinyG2/canonical_machine.cpp | 205 ++++++++++++++++------------------- TinyG2/canonical_machine.h | 51 ++++++--- TinyG2/cycle_homing.cpp | 58 +++++++++- TinyG2/cycle_probing.cpp | 2 +- TinyG2/gcode_parser.cpp | 29 ++--- TinyG2/plan_arc.cpp | 11 +- TinyG2/plan_exec.cpp | 12 +- TinyG2/plan_line.cpp | 4 +- TinyG2/planner.cpp | 10 ++ TinyG2/tinyg2.h | 2 +- 10 files changed, 223 insertions(+), 161 deletions(-) diff --git a/TinyG2/canonical_machine.cpp b/TinyG2/canonical_machine.cpp index de3de8a1..9b77cdda 100755 --- a/TinyG2/canonical_machine.cpp +++ b/TinyG2/canonical_machine.cpp @@ -121,7 +121,6 @@ static void _exec_change_tool(float *value, float *flag); static void _exec_select_tool(float *value, float *flag); static void _exec_mist_coolant_control(float *value, float *flag); static void _exec_flood_coolant_control(float *value, float *flag); -static void _exec_absolute_origin(float *value, float *flag); static void _exec_program_finalize(float *value, float *flag); static int8_t _get_axis(const index_t index); @@ -188,12 +187,14 @@ uint8_t cm_get_units_mode(GCodeState_t *gcode_state) { return gcode_state->units uint8_t cm_get_select_plane(GCodeState_t *gcode_state) { return gcode_state->select_plane;} uint8_t cm_get_path_control(GCodeState_t *gcode_state) { return gcode_state->path_control;} uint8_t cm_get_distance_mode(GCodeState_t *gcode_state) { return gcode_state->distance_mode;} -uint8_t cm_get_inverse_feed_rate_mode(GCodeState_t *gcode_state) { return gcode_state->inverse_feed_rate_mode;} +uint8_t cm_get_feed_rate_mode(GCodeState_t *gcode_state) { return gcode_state->feed_rate_mode;} uint8_t cm_get_tool(GCodeState_t *gcode_state) { return gcode_state->tool;} uint8_t cm_get_spindle_mode(GCodeState_t *gcode_state) { return gcode_state->spindle_mode;} uint8_t cm_get_block_delete_switch() { return cm.gmx.block_delete_switch;} uint8_t cm_get_runtime_busy() { return (mp_get_runtime_busy());} +float cm_get_feed_rate(GCodeState_t *gcode_state) { return gcode_state->feed_rate;} + void cm_set_motion_mode(GCodeState_t *gcode_state, uint8_t motion_mode) { gcode_state->motion_mode = motion_mode;} void cm_set_spindle_mode(GCodeState_t *gcode_state, uint8_t spindle_mode) { gcode_state->spindle_mode = spindle_mode;} void cm_set_spindle_speed_parameter(GCodeState_t *gcode_state, float speed) { gcode_state->spindle_speed = speed;} @@ -316,7 +317,7 @@ float cm_get_work_position(GCodeState_t *gcode_state, uint8_t axis) /*********************************************************************************** * CRITICAL HELPERS - * Core functions supporting the canonical machining fucntions + * Core functions supporting the canonical machining functions * These functions are not part of the NIST defined functions ***********************************************************************************/ /* @@ -396,20 +397,20 @@ void cm_set_model_target(float target[], float flag[]) * steppers will still be processing the action and the real tool position is still close * to the starting point. */ -void cm_set_model_position(stat_t status) +void cm_set_model_position(stat_t status) { - // Even if we are coalescing the move, we need to keep the gcode model correct + // Even if we are coalescing the move need to keep the gcode model correct if (status == STAT_OK) { - copy_vector(cm.gmx.position, cm.gm.target); - } + copy_vector(cm.gmx.position, cm.gm.target); + } } void cm_set_model_position_from_runtime(stat_t status) { - // Even if we are coalescing the move, we need to keep the gcode model correct + // Even if we are coalescing the move need to keep the gcode model correct if (status == STAT_OK) { - copy_vector(cm.gmx.position, mr.gm.target); - } + copy_vector(cm.gmx.position, mr.gm.target); + } } /* @@ -472,8 +473,6 @@ void cm_set_model_position_from_runtime(stat_t status) * any time required for acceleration or deceleration. */ -#define JENNIFER 8675309 - void cm_set_move_times(GCodeState_t *gcode_state) { float inv_time=0; // inverse time if doing a feed in G93 mode @@ -481,16 +480,14 @@ void cm_set_move_times(GCodeState_t *gcode_state) float abc_time=0; // coordinated move rotary part at req feed rate float max_time=0; // time required for the rate-limiting axis float tmp_time=0; // used in computation - gcode_state->minimum_time = JENNIFER;// arbitrarily large number - - // NOTE: In the below code all references to 'cm.gm.' read from the canonical machine gm, - // not the target gcode model, which is referenced as target_gm-> In most cases - // the canonical machine will be the target, but this is not required. + gcode_state->minimum_time = 8675309;// arbitrarily large number // compute times for feed motion if (cm.gm.motion_mode == MOTION_MODE_STRAIGHT_FEED) { - if (cm.gm.inverse_feed_rate_mode == true) { - inv_time = cm.gmx.inverse_feed_rate; + if (cm.gm.feed_rate_mode == INVERSE_TIME_MODE) { + inv_time = cm.gm.feed_rate; // feed rate has been normalized to minutes + cm.gm.feed_rate = 0; // reset feed rate so next block requires an explicit feed rate setting + cm.gm.feed_rate_mode = UNITS_PER_MINUTE_MODE; } else { xyz_time = sqrt(square(cm.gm.target[AXIS_X] - cm.gmx.position[AXIS_X]) + // in mm square(cm.gm.target[AXIS_Y] - cm.gmx.position[AXIS_Y]) + @@ -509,7 +506,10 @@ void cm_set_move_times(GCodeState_t *gcode_state) tmp_time = fabs(cm.gm.target[axis] - cm.gmx.position[axis]) / cm.a[axis].velocity_max; } max_time = max(max_time, tmp_time); - gcode_state->minimum_time = min(gcode_state->minimum_time, tmp_time); + // collect minimum time if not zero + if (tmp_time > 0) { + gcode_state->minimum_time = min(gcode_state->minimum_time, tmp_time); + } } gcode_state->move_time = max4(inv_time, max_time, xyz_time, abc_time); } @@ -572,6 +572,7 @@ void canonical_machine_init() cm_select_plane(cm.select_plane); cm_set_path_control(cm.path_control); cm_set_distance_mode(cm.distance_mode); + cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); // always the default cm.gmx.block_delete_switch = true; @@ -662,45 +663,54 @@ stat_t cm_hard_alarm(stat_t status) * Representation (4.3.3) * **************************/ /* - * Functions that affect the Gcode model only: + * Helper functions + */ + /* + * cm_set_axis_position() - set the position of a single axis in the model, planner and runtime + * + * This command sets an axis to a position provided as an argument. + * This is useful for setting origins for homing, probing, G28.3 and other operations. + * + * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + * !!!!! DO NOT CALL THIS FUNCTION WHILE IN A MACHINING CYCLE !!!!! + * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + * + * More specifically, do not call this function if there are any moves in the planner. + * The system must be quiescent or you will introduce positional errors. This is true + * because the planned moves had a different reference frame than the one you are now + * going to set. This should only be called during initialization sequences and during + * cycles (such as homing cycles) when you know there are no more moves in the planner. + */ + +void cm_set_axis_position(uint8_t axis, const float position) +{ + cm.gmx.position[axis] = position; + cm.gm.target[axis] = position; + mp_set_planner_position(axis, position); + mp_set_runtime_position(axis, position); +} + +/************************************************************* + * Representation functions that affect the Gcode model only * * cm_select_plane() - G17,G18,G19 select axis plane * cm_set_units_mode() - G20, G21 * cm_set_distance_mode() - G90, G91 * cm_set_coord_offsets() - G10 (delayed persistence) - * - * Functions that affect gcode model and are queued to planner - * - * cm_set_coord_system() - G54-G59 - * cm_set_absolute_origin() - G28.3 - model, planner and queue to runtime - * cm_set_axis_origin() - set the origin of a single axis - model and planner - * cm_set_origin_offsets() - G92 - * cm_reset_origin_offsets() - G92.1 - * cm_suspend_origin_offsets() - G92.2 - * cm_resume_origin_offsets() - G92.3 */ -/* - * cm_select_plane() - G17,G18,G19 select axis plane (AFFECTS MODEL ONLY) - */ stat_t cm_select_plane(uint8_t plane) { cm.gm.select_plane = plane; return (STAT_OK); } -/* - * cm_set_units_mode() - G20, G21 (affects MODEL only) - */ stat_t cm_set_units_mode(uint8_t mode) { cm.gm.units_mode = mode; // 0 = inches, 1 = mm. return(STAT_OK); } -/* - * cm_set_distance_mode() - G90, G91 (affects MODEL only) - */ stat_t cm_set_distance_mode(uint8_t mode) { cm.gm.distance_mode = mode; // 0 = absolute mode, 1 = incremental @@ -717,6 +727,7 @@ stat_t cm_set_distance_mode(uint8_t mode) * It also does not reset the work_offsets which may be accomplished by calling * cm_set_work_offsets() immediately afterwards. */ + stat_t cm_set_coord_offsets(uint8_t coord_system, float offset[], float flag[]) { if ((coord_system < G54) || (coord_system > COORD_SYSTEM_MAX)) { // you can't set G53 @@ -731,6 +742,10 @@ stat_t cm_set_coord_offsets(uint8_t coord_system, float offset[], float flag[]) return (STAT_OK); } +/**************************************************************************** + * Representation functions that affect gcode model and are queued to planner + * + */ /* * cm_set_coord_system() - G54-G59 * _exec_offset() - callback from planner @@ -755,57 +770,6 @@ static void _exec_offset(float *value, float *flag) // cm_set_work_offsets(RUNTIME); } -/* - * cm_set_absolute_origin() - G28.3 - model, planner and queue to runtime - * _exec_absolute_origin() - callback from planner - * - * cm_set_absolute_origin() takes a vector of origins (presumably 0's, but not - * necessarily) and applies them to all axes where the corresponding position - * in the flag vector is true (1). - * - * This is a 2 step process. The model and planner contexts are set immediately, - * the runtime command is queued and synchronized woth the planner queue. - */ -stat_t cm_set_absolute_origin(float origin[], float flag[]) -{ - float value[AXES]; - - for (uint8_t axis = AXIS_X; axis < AXES; axis++) { - if (fp_TRUE(flag[axis])) { - value[axis] = cm.offset[cm.gm.coord_system][axis] + _to_millimeters(origin[axis]); - cm_set_axis_origin(axis, value[axis]); - } - } - mp_queue_command(_exec_absolute_origin, value, flag); - return (STAT_OK); -} - -static void _exec_absolute_origin(float *value, float *flag) -{ - for (uint8_t axis = AXIS_X; axis < AXES; axis++) { - if (fp_TRUE(flag[axis])) { - mp_set_runtime_position(axis, value[axis]); - cm.homed[axis] = true; // it's not considered homed until you get to the runtime - } - } -} - -/* - * cm_set_axis_origin() - set the origin of a single axis - model and planner - * - * This is an "unofficial gcode" command to allow arbitrarily setting an axis - * to an absolute position. This is needed to support the Otherlab infinite - * Y axis. USE: With the axis(or axes) where you want it, issue g92.4 y0 - * (for example). The Y axis will now be set to 0 (or whatever value provided) - */ -void cm_set_axis_origin(uint8_t axis, const float position) -{ - cm.gmx.position[axis] = position; - cm.gm.target[axis] = position; - mp_set_planner_position(axis, position); - mp_set_runtime_position(axis, position); -} - /* * cm_set_origin_offsets() - G92 * cm_reset_origin_offsets() - G92.1 @@ -924,16 +888,13 @@ stat_t cm_goto_g30_position(float target[], float flags[]) /* * cm_set_feed_rate() - F parameter (affects MODEL only) * - * Sets feed rate; or sets inverse feed rate if it's active. - * Converts all values to internal format (mm's) - * Errs out of feed rate exceeds maximum, but doesn't compute maximum for - * inverse feed rate as this would require knowing the move length in advance. + * Normalize feed rate to mm/min or to minutes if in inverse time mode */ stat_t cm_set_feed_rate(float feed_rate) { - if (cm.gm.inverse_feed_rate_mode == true) { - cm.gmx.inverse_feed_rate = feed_rate; // minutes per motion for this block only + if (cm.gm.feed_rate_mode == INVERSE_TIME_MODE) { + cm.gm.feed_rate = 1 / feed_rate; // normalize to minutes (NB: active for this gcode block only) } else { cm.gm.feed_rate = _to_millimeters(feed_rate); } @@ -941,15 +902,16 @@ stat_t cm_set_feed_rate(float feed_rate) } /* - * cm_set_inverse_feed_rate() - G93, G94 (affects MODEL only) + * cm_set_feed_rate_mode() - G93, G94 (affects MODEL only) * - * TRUE = inverse time feed rate in effect - for this block only - * FALSE = units per minute feed rate in effect + * INVERSE_TIME_MODE = 0, // G93 + * UNITS_PER_MINUTE_MODE, // G94 + * UNITS_PER_REVOLUTION_MODE // G95 (unimplemented) */ -stat_t cm_set_inverse_feed_rate_mode(uint8_t mode) +stat_t cm_set_feed_rate_mode(uint8_t mode) { - cm.gm.inverse_feed_rate_mode = mode; + cm.gm.feed_rate_mode = mode; return (STAT_OK); } @@ -989,7 +951,7 @@ stat_t cm_straight_feed(float target[], float flags[]) cm.gm.motion_mode = MOTION_MODE_STRAIGHT_FEED; // trap zero feed rate condition - if ((cm.gm.inverse_feed_rate_mode == false) && (fp_ZERO(cm.gm.feed_rate))) { + if ((cm.gm.feed_rate_mode != INVERSE_TIME_MODE) && (fp_ZERO(cm.gm.feed_rate))) { return (STAT_GCODE_FEEDRATE_ERROR); } cm_set_model_target(target, flags); @@ -1355,7 +1317,7 @@ static void _exec_program_finalize(float *value, float *flag) cm_set_units_mode(cm.units_mode); // reset to default units mode cm_spindle_control(SPINDLE_OFF); // M5 cm_flood_coolant_control(false); // M9 - cm_set_inverse_feed_rate_mode(false); + cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE);// G94 // cm_set_motion_mode(MOTION_MODE_STRAIGHT_FEED);// NIST specifies G1, but we cancel motion mode. Safer. cm_set_motion_mode(MODEL, MOTION_MODE_CANCEL_MOTION_MODE); } @@ -1503,9 +1465,9 @@ static const char msg_g90[] PROGMEM = "G90 - absolute distance mode"; static const char msg_g91[] PROGMEM = "G91 - incremental distance mode"; static const char *const msg_dist[] PROGMEM = { msg_g90, msg_g91 }; -static const char msg_g94[] PROGMEM = "G94 - units-per-minute mode (i.e. feedrate mode)"; static const char msg_g93[] PROGMEM = "G93 - inverse time mode"; -static const char *const msg_frmo[] PROGMEM = { msg_g94, msg_g93 }; +static const char msg_g94[] PROGMEM = "G94 - units-per-minute mode (i.e. feedrate mode)"; +static const char *const msg_frmo[] PROGMEM = { msg_g93, msg_g94 }; #else @@ -1617,7 +1579,7 @@ stat_t cm_get_momo(cmdObj_t *cmd) { return(_get_msg_helper(cmd, msg_momo, cm_get stat_t cm_get_plan(cmdObj_t *cmd) { return(_get_msg_helper(cmd, msg_plan, cm_get_select_plane(ACTIVE_MODEL)));} stat_t cm_get_path(cmdObj_t *cmd) { return(_get_msg_helper(cmd, msg_path, cm_get_path_control(ACTIVE_MODEL)));} stat_t cm_get_dist(cmdObj_t *cmd) { return(_get_msg_helper(cmd, msg_dist, cm_get_distance_mode(ACTIVE_MODEL)));} -stat_t cm_get_frmo(cmdObj_t *cmd) { return(_get_msg_helper(cmd, msg_frmo, cm_get_inverse_feed_rate_mode(ACTIVE_MODEL)));} +stat_t cm_get_frmo(cmdObj_t *cmd) { return(_get_msg_helper(cmd, msg_frmo, cm_get_feed_rate_mode(ACTIVE_MODEL)));} stat_t cm_get_toolv(cmdObj_t *cmd) { @@ -1744,9 +1706,34 @@ stat_t cm_run_home(cmdObj_t *cmd) return (STAT_OK); } -stat_t cm_dd1(cmdObj_t *cmd) +/* + * Debugging Commands + * + * cm_dam() - dump active model + * cm_drm() - dump runtime model + */ + +stat_t cm_dam(cmdObj_t *cmd) { -// printf(); + printf("Active model:\n"); + cm_print_vel(cmd); + cm_print_feed(cmd); + cm_print_line(cmd); + cm_print_stat(cmd); + cm_print_macs(cmd); + cm_print_cycs(cmd); + cm_print_mots(cmd); + cm_print_hold(cmd); + cm_print_home(cmd); + cm_print_unit(cmd); + cm_print_coor(cmd); + cm_print_momo(cmd); + cm_print_plan(cmd); + cm_print_path(cmd); + cm_print_dist(cmd); + cm_print_frmo(cmd); + cm_print_tool(cmd); + return (STAT_OK); } diff --git a/TinyG2/canonical_machine.h b/TinyG2/canonical_machine.h index aad91f7c..91613193 100755 --- a/TinyG2/canonical_machine.h +++ b/TinyG2/canonical_machine.h @@ -79,20 +79,21 @@ extern "C"{ * G92 offsets. cfg has the power-on / reset gcode default values, but gm has * the operating state for the values (which may have changed). */ - typedef struct GCodeState { // Gcode model state - used by model, planning and runtime - uint32_t linenum; // Gcode block line number +typedef struct GCodeState { // Gcode model state - used by model, planning and runtime + uint32_t linenum; // Gcode block line number uint8_t motion_mode; // Group1: G0, G1, G2, G3, G38.2, G80, G81, - // G82, G83 G84, G85, G86, G87, G88, G89 + // G82, G83 G84, G85, G86, G87, G88, G89 float target[AXES]; // XYZABC where the move should go float work_offset[AXES]; // offset from the work coordinate system (for reporting only) float move_time; // optimal time for move given axis constraints float minimum_time; // minimum time possible for move given axis constraints - float feed_rate; // F - normalized to millimeters/minute + float feed_rate; // F - normalized to millimeters/minute or in inverse time mode + uint8_t feed_rate_mode; // See cmFeedRateMode for settings + float spindle_speed; // in RPM float parameter; // P - parameter used for dwell time in seconds, G10 coord select... - uint8_t inverse_feed_rate_mode; // G93 TRUE = inverse, FALSE = normal (G94) uint8_t select_plane; // G17,G18,G19 - values to set plane to uint8_t units_mode; // G20,G21 - 0=inches (G20), 1 = mm (G21) uint8_t coord_system; // G54-G59 - select coordinate system 1-9 @@ -117,7 +118,6 @@ typedef struct GCodeStateExtended { // Gcode dynamic state extensions - used by float g28_position[AXES]; // XYZABC stored machine position for G28 float g30_position[AXES]; // XYZABC stored machine position for G30 - float inverse_feed_rate; // ignored if inverse_feed_rate not active float feed_rate_override_factor; // 1.0000 x F feed rate. Go up or down from there float traverse_override_factor; // 1.0000 x traverse rate. Go down from there uint8_t feed_rate_override_enable; // TRUE = overrides enabled (M48), F=(M49) @@ -141,17 +141,16 @@ typedef struct GCodeStateExtended { // Gcode dynamic state extensions - used by typedef struct GCodeInput { // Gcode model inputs - meaning depends on context uint8_t next_action; // handles G modal group 1 moves & non-modals uint8_t motion_mode; // Group1: G0, G1, G2, G3, G38.2, G80, G81, - // G82, G83 G84, G85, G86, G87, G88, G89 + // G82, G83 G84, G85, G86, G87, G88, G89 uint8_t program_flow; // used only by the gcode_parser uint32_t linenum; // N word or autoincrement in the model float target[AXES]; // XYZABC where the move should go float feed_rate; // F - normalized to millimeters/minute - float inverse_feed_rate; // ignored if inverse_feed_rate not active float feed_rate_override_factor; // 1.0000 x F feed rate. Go up or down from there float traverse_override_factor; // 1.0000 x traverse rate. Go down from there - uint8_t inverse_feed_rate_mode; // G93 TRUE = inverse, FALSE = normal (G94) + uint8_t feed_rate_mode; // See cmFeedRateMode for settings uint8_t feed_rate_override_enable; // TRUE = overrides enabled (M48), F=(M49) uint8_t traverse_override_enable; // TRUE = traverse override enabled uint8_t override_enables; // enables for feed and spoindle (GN/GF only) @@ -247,9 +246,11 @@ typedef struct cmSingleton { // struct to manage cm globals and cycles uint8_t probe_state; // 1==success, 0==failed float probe_results[AXES]; // probing results + uint8_t set_origin_state; // used to control set_origin cycles + uint8_t g28_flag; // true = complete a G28 move uint8_t g30_flag; // true = complete a G30 move - uint8_t g10_persist_flag; //.G10 changed offsets - persist them + uint8_t g10_persist_flag; // G10 changed offsets - persist them uint8_t feedhold_requested; // feedhold character has been received uint8_t queue_flush_requested; // queue flush character has been received uint8_t cycle_start_requested; // cycle start character has been received (flag to end feedhold) @@ -332,7 +333,8 @@ enum cmCycleState { CYCLE_MACHINING, // in normal machining cycle CYCLE_PROBE, // in probe cycle CYCLE_HOMING, // homing is treated as a specialized cycle - CYCLE_JOG // jogging is treated as a specialized cycle + CYCLE_JOG, // jogging is treated as a specialized cycle + CYCLE_SET_ORIGIN // set origin to new coordinates }; enum cmMotionState { @@ -362,6 +364,12 @@ enum cmProbeState { // applies to cm.probe_state PROBE_WAITING // probe is waiting to be started }; +enum cmSetOriginState { // applies to cm.set_origin_state + SET_ORIGIN_OFF = 0, + SET_ORIGIN_SUCCEDED = 1, // end state + SET_ORIGIN_WAITING // waiting for planner to drain +}; + /* The difference between NextAction and MotionMode is that NextAction is * used by the current block, and may carry non-modal commands, whereas * MotionMode persists across blocks (as G modal group 1) @@ -370,7 +378,7 @@ enum cmProbeState { // applies to cm.probe_state enum cmNextAction { // these are in order to optimized CASE statement NEXT_ACTION_DEFAULT = 0, // Must be zero (invokes motion modes) NEXT_ACTION_SEARCH_HOME, // G28.2 homing cycle - NEXT_ACTION_SET_ABSOLUTE_ORIGIN, // G28.3 origin set + NEXT_ACTION_SET_ORIGIN, // G28.3 origin set NEXT_ACTION_HOMING_NO_SET, // G28.4 homing cycle with no coordinate setting NEXT_ACTION_SET_G28_POSITION, // G28.1 set position in abs coordingates NEXT_ACTION_GOTO_G28_POSITION, // G28 go to machine position @@ -459,6 +467,12 @@ enum cmDistanceMode { INCREMENTAL_MODE // G91 }; +enum cmFeedRateMode { + INVERSE_TIME_MODE = 0, // G93 + UNITS_PER_MINUTE_MODE, // G94 + UNITS_PER_REVOLUTION_MODE // G95 (unimplemented) +}; + enum cmOriginOffset { ORIGIN_OFFSET_SET=0, // G92 - set origin offsets ORIGIN_OFFSET_CANCEL, // G92.1 - zero out origin offsets @@ -519,12 +533,14 @@ uint8_t cm_get_units_mode(GCodeState_t *gcode_state); uint8_t cm_get_select_plane(GCodeState_t *gcode_state); uint8_t cm_get_path_control(GCodeState_t *gcode_state); uint8_t cm_get_distance_mode(GCodeState_t *gcode_state); -uint8_t cm_get_inverse_feed_rate_mode(GCodeState_t *gcode_state); +uint8_t cm_get_feed_rate_mode(GCodeState_t *gcode_state); uint8_t cm_get_tool(GCodeState_t *gcode_state); uint8_t cm_get_spindle_mode(GCodeState_t *gcode_state); uint8_t cm_get_block_delete_switch(void); uint8_t cm_get_runtime_busy(void); +float cm_get_feed_rate(GCodeState_t *gcode_state); + void cm_set_motion_mode(GCodeState_t *gcode_state, uint8_t motion_mode); void cm_set_spindle_mode(GCodeState_t *gcode_state, uint8_t spindle_mode); void cm_set_spindle_speed_parameter(GCodeState_t *gcode_state, float speed); @@ -561,10 +577,10 @@ stat_t cm_set_units_mode(uint8_t mode); // G20, G21 stat_t cm_homing_cycle_start(void); // G28.2 stat_t cm_homing_cycle_start_no_set(void); // G28.4 -stat_t cm_homing_callback(void); // G28.2 main loop callback +stat_t cm_homing_callback(void); // G28.2/.4 main loop callback -stat_t cm_set_absolute_origin(float origin[], float flags[]); // G28.3 (special function) -void cm_set_axis_origin(uint8_t axis, const float position); // set absolute position (used by G28's) +stat_t cm_set_origin_cycle_start(void); // G28.3 (special function) +stat_t cm_set_origin_callback(void); // G28.3 main loop callback stat_t cm_jogging_callback(void); // jogging cycle main loop stat_t cm_jogging_cycle_start(uint8_t axis); // {"jogx":-100.3} @@ -578,6 +594,7 @@ stat_t cm_goto_g30_position(float target[], float flags[]); // G30 stat_t cm_straight_probe(float target[], float flags[]); // G38.2 stat_t cm_probe_callback(void); // G38.2 main loop callback +void cm_set_axis_position(uint8_t axis, const float position); // set absolute position stat_t cm_set_coord_system(uint8_t coord_system); // G54 - G59 stat_t cm_set_coord_offsets(uint8_t coord_system, float offset[], float flag[]); // G10 L2 stat_t cm_set_distance_mode(uint8_t mode); // G90, G91 @@ -588,7 +605,7 @@ stat_t cm_resume_origin_offsets(void); // G92.3 stat_t cm_straight_traverse(float target[], float flags[]); stat_t cm_set_feed_rate(float feed_rate); // F parameter -stat_t cm_set_inverse_feed_rate_mode(uint8_t mode); // True= inv mode +stat_t cm_set_feed_rate_mode(uint8_t mode); // G93, G94, (G95 unimplemented) stat_t cm_set_path_control(uint8_t mode); // G61, G61.1, G64 stat_t cm_straight_feed(float target[], float flags[]); // G1 stat_t cm_dwell(float seconds); // G4, P parameter diff --git a/TinyG2/cycle_homing.cpp b/TinyG2/cycle_homing.cpp index 88621470..d911f17d 100755 --- a/TinyG2/cycle_homing.cpp +++ b/TinyG2/cycle_homing.cpp @@ -69,13 +69,13 @@ struct hmHomingSingleton { // persistent homing runtime variables float latch_velocity; // latch speed as positive number float latch_backoff; // max distance to back off switch during latch phase float zero_backoff; // distance to back off switch before setting zero - float max_clear_backoff; // maximum distance of switch clearing backoffs before erring out +// float max_clear_backoff; // maximum distance of switch clearing backoffs before erring out // state saved from gcode model - float saved_feed_rate; // F setting uint8_t saved_units_mode; // G20,G21 global setting uint8_t saved_coord_system; // G54 - G59 setting uint8_t saved_distance_mode; // G90,G91 global setting + float saved_feed_rate; // F setting float saved_jerk; // saved and restored for each axis homed }; static struct hmHomingSingleton hm; @@ -98,6 +98,10 @@ static stat_t _homing_finalize_exit(int8_t axis); static int8_t _get_next_axis(int8_t axis); //static int8_t _get_next_axes(int8_t axis); +/*********************************************************************************** + **** G28.2 Homing Cycle *********************************************************** + ***********************************************************************************/ + /***************************************************************************** * cm_homing_cycle_start() - G28.2 homing cycle using limit switches * @@ -156,7 +160,7 @@ stat_t cm_homing_cycle_start(void) hm.saved_units_mode = cm_get_units_mode(ACTIVE_MODEL); //cm.gm.units_mode; hm.saved_coord_system = cm_get_coord_system(ACTIVE_MODEL); //cm.gm.coord_system; hm.saved_distance_mode = cm_get_distance_mode(ACTIVE_MODEL); //cm.gm.distance_mode; - hm.saved_feed_rate = cm_get_distance_mode(ACTIVE_MODEL); //cm.gm.feed_rate; + hm.saved_feed_rate = cm_get_feed_rate(ACTIVE_MODEL); //cm.gm.feed_rate; // set working values cm_set_units_mode(MILLIMETERS); @@ -348,11 +352,10 @@ static stat_t _homing_axis_zero_backoff(int8_t axis) // backoff to zero positio static stat_t _homing_axis_set_zero(int8_t axis) // set zero and finish up { if (hm.set_coordinates != false) { // do not set axis if in G28.4 cycle - cm_set_axis_origin(axis, 0); - mp_set_runtime_position(axis, 0); + cm_set_axis_position(axis, 0); cm.homed[axis] = true; } else { - cm_set_axis_origin(axis, cm_get_work_position(RUNTIME, axis)); + cm_set_axis_position(axis, cm_get_work_position(RUNTIME, axis)); } cm.a[axis].jerk_max = hm.saved_jerk; // restore the max jerk value @@ -594,6 +597,49 @@ int8_t _get_next_axes(int8_t axis) } */ +/*********************************************************************************** + **** G28.3 Set Origin Cycle ******************************************************* + ***********************************************************************************/ + +/***************************************************************************** + * cm_set_origin_cycle_start() - G28.3 - model, planner and queue to runtime + * cm_set_origin_callback() - callback from controller + * + * This function is called by the gcode interpreter to execute a G28.3 command. + * + * It enters a cycle to allow the planner queue to empty, then once that's happened + * it sets the axis or axes to the values in the G28.3 command. + */ + +stat_t cm_set_origin_cycle_start() +{ + for (uint8_t axis = AXIS_X; axis < AXES; axis++) { + if (fp_TRUE(cm.gf.target[axis])) { + cm.gm.target[axis] = cm.offset[cm.gm.coord_system][axis] + _to_millimeters(cm.gn.target[axis]); + } + } + cm.cycle_state = CYCLE_SET_ORIGIN; + cm.set_origin_state = SET_ORIGIN_WAITING; + return (STAT_OK); +} + +stat_t cm_set_origin_callback(void) +{ + if (cm.cycle_state != CYCLE_SET_ORIGIN) { return (STAT_NOOP);} // exit if not in an origin cycle + if (cm_get_runtime_busy() == true) { return (STAT_EAGAIN);} // wait until planner empties + + for (uint8_t axis = AXIS_X; axis < AXES; axis++) { + if (fp_TRUE(cm.gf.target[axis])) { + cm_set_axis_position(axis, cm.gm.target[axis]); + } + } + cm.set_origin_state = SET_ORIGIN_SUCCEDED; + cm_set_motion_mode(MODEL, MOTION_MODE_CANCEL_MOTION_MODE); + cm.cycle_state = CYCLE_OFF; // required + cm_cycle_end(); + return (STAT_OK); +} + #ifdef __cplusplus } #endif diff --git a/TinyG2/cycle_probing.cpp b/TinyG2/cycle_probing.cpp index 2cc30c98..27852221 100755 --- a/TinyG2/cycle_probing.cpp +++ b/TinyG2/cycle_probing.cpp @@ -99,7 +99,7 @@ static stat_t _set_pb_func(uint8_t (*func)()); uint8_t cm_straight_probe(float target[], float flags[]) { // trap zero feed rate condition - if ((cm.gm.inverse_feed_rate_mode == false) && (fp_ZERO(cm.gm.feed_rate))) { + if ((cm.gm.feed_rate_mode != INVERSE_TIME_MODE) && (fp_ZERO(cm.gm.feed_rate))) { return (STAT_GCODE_FEEDRATE_ERROR); } diff --git a/TinyG2/gcode_parser.cpp b/TinyG2/gcode_parser.cpp index 04d58460..a31cb951 100755 --- a/TinyG2/gcode_parser.cpp +++ b/TinyG2/gcode_parser.cpp @@ -270,7 +270,7 @@ static stat_t _parse_gcode_block(char_t *buf) case 0: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_GOTO_G28_POSITION); case 1: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_G28_POSITION); case 2: SET_NON_MODAL (next_action, NEXT_ACTION_SEARCH_HOME); - case 3: SET_NON_MODAL (next_action, NEXT_ACTION_SET_ABSOLUTE_ORIGIN); + case 3: SET_NON_MODAL (next_action, NEXT_ACTION_SET_ORIGIN); case 4: SET_NON_MODAL (next_action, NEXT_ACTION_HOMING_NO_SET); default: status = STAT_UNRECOGNIZED_COMMAND; } @@ -322,8 +322,9 @@ static stat_t _parse_gcode_block(char_t *buf) } break; } - case 93: SET_MODAL (MODAL_GROUP_G5, inverse_feed_rate_mode, true); - case 94: SET_MODAL (MODAL_GROUP_G5, inverse_feed_rate_mode, false); + case 93: SET_MODAL (MODAL_GROUP_G5, feed_rate_mode, INVERSE_TIME_MODE); + case 94: SET_MODAL (MODAL_GROUP_G5, feed_rate_mode, UNITS_PER_MINUTE_MODE); +// case 95: SET_MODAL (MODAL_GROUP_G5, feed_rate_mode, UNITS_PER_REVOLUTION_MODE); default: status = STAT_UNRECOGNIZED_COMMAND; } break; @@ -421,7 +422,7 @@ static stat_t _execute_gcode_block() stat_t status = STAT_OK; cm_set_model_linenum(cm.gn.linenum); - EXEC_FUNC(cm_set_inverse_feed_rate_mode, inverse_feed_rate_mode); + EXEC_FUNC(cm_set_feed_rate_mode, feed_rate_mode); EXEC_FUNC(cm_set_feed_rate, feed_rate); EXEC_FUNC(cm_feed_rate_override_factor, feed_rate_override_factor); EXEC_FUNC(cm_traverse_override_factor, traverse_override_factor); @@ -450,14 +451,14 @@ static stat_t _execute_gcode_block() //--> set retract mode goes here switch (cm.gn.next_action) { - case NEXT_ACTION_SET_G28_POSITION: { status = cm_set_g28_position(); break;} // G28.1 - case NEXT_ACTION_GOTO_G28_POSITION: { status = cm_goto_g28_position(cm.gn.target, cm.gf.target); break;} // G28 - case NEXT_ACTION_SET_G30_POSITION: { status = cm_set_g30_position(); break;} // G30.1 - case NEXT_ACTION_GOTO_G30_POSITION: { status = cm_goto_g30_position(cm.gn.target, cm.gf.target); break;} // G30 + case NEXT_ACTION_SET_G28_POSITION: { status = cm_set_g28_position(); break;} // G28.1 + case NEXT_ACTION_GOTO_G28_POSITION: { status = cm_goto_g28_position(cm.gn.target, cm.gf.target); break;} // G28 + case NEXT_ACTION_SET_G30_POSITION: { status = cm_set_g30_position(); break;} // G30.1 + case NEXT_ACTION_GOTO_G30_POSITION: { status = cm_goto_g30_position(cm.gn.target, cm.gf.target); break;} // G30 - case NEXT_ACTION_SEARCH_HOME: { status = cm_homing_cycle_start(); break;} // G28.2 - case NEXT_ACTION_SET_ABSOLUTE_ORIGIN: { status = cm_set_absolute_origin(cm.gn.target, cm.gf.target); break;}// G28.3 - case NEXT_ACTION_HOMING_NO_SET: { status = cm_homing_cycle_start_no_set(); break;} // G28.4 + case NEXT_ACTION_SEARCH_HOME: { status = cm_homing_cycle_start(); break;} // G28.2 + case NEXT_ACTION_SET_ORIGIN: { status = cm_set_origin_cycle_start(); break;} // G28.3 + case NEXT_ACTION_HOMING_NO_SET: { status = cm_homing_cycle_start_no_set(); break;} // G28.4 case NEXT_ACTION_STRAIGHT_PROBE: { status = cm_straight_probe(cm.gn.target, cm.gf.target); break;} // G38.2 @@ -474,9 +475,9 @@ static stat_t _execute_gcode_block() case MOTION_MODE_STRAIGHT_TRAVERSE: { status = cm_straight_traverse(cm.gn.target, cm.gf.target); break;} case MOTION_MODE_STRAIGHT_FEED: { status = cm_straight_feed(cm.gn.target, cm.gf.target); break;} case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: - // gf.radius sets radius mode if radius was collected in gn - { status = cm_arc_feed(cm.gn.target, cm.gf.target, cm.gn.arc_offset[0], cm.gn.arc_offset[1], - cm.gn.arc_offset[2], cm.gn.arc_radius, cm.gn.motion_mode); break;} + // gf.radius sets radius mode if radius was collected in gn + { status = cm_arc_feed(cm.gn.target, cm.gf.target, cm.gn.arc_offset[0], cm.gn.arc_offset[1], + cm.gn.arc_offset[2], cm.gn.arc_radius, cm.gn.motion_mode); break;} } } } diff --git a/TinyG2/plan_arc.cpp b/TinyG2/plan_arc.cpp index 118af6ef..d3567cbc 100755 --- a/TinyG2/plan_arc.cpp +++ b/TinyG2/plan_arc.cpp @@ -76,9 +76,10 @@ stat_t cm_arc_feed(float target[], float flags[],// arc endpoints uint8_t motion_mode) // defined motion mode { // trap zero feed rate condition - if ((cm.gm.inverse_feed_rate_mode == false) && (fp_ZERO(cm.gm.feed_rate))) { + if ((cm.gm.feed_rate_mode != INVERSE_TIME_MODE) && (fp_ZERO(cm.gm.feed_rate))) { return (STAT_GCODE_FEEDRATE_ERROR); } + // 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) @@ -223,7 +224,7 @@ static stat_t _compute_arc() // length is the total mm of travel of the helix (or just a planar arc) arc.length = hypot(arc.angular_travel * arc.radius, fabs(arc.linear_travel)); - if (arc.length < cm.arc_segment_len) return (STAT_MINIMUM_LENGTH_MOVE); // too short to draw + if (arc.length < cm.arc_segment_len) return (STAT_MINIMUM_LENGTH_MOVE); // arc is too short to draw arc.time = _get_arc_time(arc.linear_travel, arc.angular_travel, arc.radius); @@ -373,8 +374,10 @@ static float _get_arc_time (const float linear_travel, // in mm float move_time=0; // picks through the times and retains the slowest float planar_travel = fabs(angular_travel * radius);// travel in arc plane - if (cm.gm.inverse_feed_rate_mode == true) { - move_time = cm.gmx.inverse_feed_rate; + if (cm.gm.feed_rate_mode == INVERSE_TIME_MODE) { + move_time = cm.gm.feed_rate; // feed rate has been normalized to minutes + cm.gm.feed_rate = 0; // reset feed rate so next block requires an explicit feed rate setting + cm.gm.feed_rate_mode = UNITS_PER_MINUTE_MODE; } else { move_time = sqrt(square(planar_travel) + square(linear_travel)) / cm.gm.feed_rate; } diff --git a/TinyG2/plan_exec.cpp b/TinyG2/plan_exec.cpp index 2dcedb98..f8c76c86 100755 --- a/TinyG2/plan_exec.cpp +++ b/TinyG2/plan_exec.cpp @@ -210,15 +210,13 @@ stat_t mp_exec_aline(mpBuf_t *bf) #endif // generate the waypoints for position correction at section ends - for (uint8_t i=0; ilength; // tail alternate form } - - } - // NB: from this point on the contents of the bf buffer do not affect execution //**** main dispatcher to process segments *** diff --git a/TinyG2/plan_line.cpp b/TinyG2/plan_line.cpp index 9059f9b5..1a7977fb 100755 --- a/TinyG2/plan_line.cpp +++ b/TinyG2/plan_line.cpp @@ -493,7 +493,7 @@ static void _calculate_trapezoid(mpBuf_t *bf) // Rate-limited HT' case (asymmetric) - this is relatively expensive but it's not called very often float computed_velocity = bf->cruise_vmax; - uint8_t i=0; +// unneeded uint8_t i=0; do { bf->cruise_velocity = computed_velocity; // initialize from previous iteration bf->head_length = _get_target_length(bf->entry_velocity, bf->cruise_velocity, bf); @@ -505,7 +505,7 @@ static void _calculate_trapezoid(mpBuf_t *bf) bf->tail_length = (bf->tail_length / (bf->head_length + bf->tail_length)) * bf->length; computed_velocity = _get_target_velocity(bf->exit_velocity, bf->tail_length, bf); } - if (++i > TRAPEZOID_ITERATION_MAX) { fprintf_P(stderr,PSTR("_calculate_trapezoid() failed to converge"));} +// unneeded if (++i > TRAPEZOID_ITERATION_MAX) { fprintf_P(stderr,PSTR("_calculate_trapezoid() failed to converge"));} } while ((fabs(bf->cruise_velocity - computed_velocity) / computed_velocity) > TRAPEZOID_ITERATION_ERROR_PERCENT); // set velocity and clean up any parts that are too short diff --git a/TinyG2/planner.cpp b/TinyG2/planner.cpp index 31fe9b02..fb7d99d4 100755 --- a/TinyG2/planner.cpp +++ b/TinyG2/planner.cpp @@ -57,6 +57,7 @@ #include "plan_arc.h" #include "planner.h" #include "stepper.h" +#include "encoder.h" #include "report.h" #include "util.h" @@ -143,6 +144,9 @@ void mp_flush_planner() * - mr.target - target position of runtime segment * - mr.endpoint - final target position of runtime segment * + * In addition to all that you have to make sure the encoder steps agree with the + * runtime position. + * * Note that position is set immediately when called and may not be not an accurate * representation of the tool position. The motors are still processing the * action and the real tool position is still close to the starting point. @@ -156,6 +160,12 @@ void mp_set_planner_position(uint8_t axis, const float position) void mp_set_runtime_position(uint8_t axis, const float position) { mr.position[axis] = position; + + // reset all step counters and encoders - these are in motor space + float zero[] = {0,0,0,0,0,0}; + en_set_encoders(zero); +// en_set_encoders(position); + mp_reset_step_counts(); } /************************************************************************************ diff --git a/TinyG2/tinyg2.h b/TinyG2/tinyg2.h index 63362e75..c402a919 100755 --- a/TinyG2/tinyg2.h +++ b/TinyG2/tinyg2.h @@ -36,7 +36,7 @@ #include "MotatePins.h" #ifndef TINYG_FIRMWARE_BUILD -#define TINYG_FIRMWARE_BUILD 033.03 // tracking tinyg 418.04 - updated stepper files +#define TINYG_FIRMWARE_BUILD 033.04 // tracking tinyg 418.04 - ready to test #endif #define TINYG_FIRMWARE_VERSION 0.8 // firmware major version #define TINYG_HARDWARE_PLATFORM 3 // hardware platform indicator (2 = Native Arduino Due)