From f31acbb45bdd33fcf9029e40586e015ed7217b6a Mon Sep 17 00:00:00 2001 From: Alden Hart Date: Sun, 19 Feb 2017 06:34:28 -0500 Subject: [PATCH] handling cases with very short moves --- g2core/canonical_machine.cpp | 50 +++++++++++++++++++----------------- g2core/canonical_machine.h | 3 +-- g2core/controller.cpp | 5 ++-- g2core/plan_arc.cpp | 2 +- g2core/plan_zoid.cpp | 5 +++- 5 files changed, 36 insertions(+), 29 deletions(-) diff --git a/g2core/canonical_machine.cpp b/g2core/canonical_machine.cpp index 8c7e8a17..329837c8 100644 --- a/g2core/canonical_machine.cpp +++ b/g2core/canonical_machine.cpp @@ -510,11 +510,9 @@ float cm_get_absolute_position(const GCodeState_t *gcode_state, const uint8_t ax * Core functions supporting the canonical machining functions * These functions are not part of the NIST defined functions ***********************************************************************************/ + /* - * cm_finalize_move() - perform final operations for a traverse or feed - * cm_update_model_position_from_runtime() - set endpoint position from final runtime position - * - * These routines set the point position in the gcode model. + * cm_update_model_position() - set gmx endpoint position for a traverse, feed or arc * * Note: As far as the canonical machine is concerned the final position of a Gcode block (move) * is achieved as soon as the move is planned and the move target becomes the new model position. @@ -522,14 +520,9 @@ float cm_get_absolute_position(const GCodeState_t *gcode_state, const uint8_t ax * execution, and the real tool position is still close to the starting point. */ -void cm_finalize_move() +void cm_update_model_position() { - copy_vector(cm->gmx.position, cm->gm.target); // update model position -} - -void cm_update_model_position_from_runtime() -{ - copy_vector(cm->gmx.position, mr->gm.target); + copy_vector(cm->gmx.position, cm->gm.target); // would be mr->gm.target if from runtime } /* @@ -1128,12 +1121,18 @@ stat_t cm_straight_traverse(const float target[], const bool flags[]) cm_set_display_offsets(&cm->gm); // capture the fully resolved offsets to the state cm_cycle_start(); // required for homing & other cycles stat_t status = mp_aline(&cm->gm); // send the move to the planner + cm_update_model_position(); // update gmx.position to ready for next incoming move - cm_finalize_move(); - if (status == STAT_MINIMUM_LENGTH_MOVE && !mp_has_runnable_buffer(mp)) { //applies to currently active machine - cm_cycle_end(); - return (STAT_OK); +// if (status == STAT_MINIMUM_LENGTH_MOVE && !mp_has_runnable_buffer(mp)) { //mp applies to currently active planner +// cm_cycle_end(); +// return (STAT_OK); +// } + if (status == STAT_MINIMUM_LENGTH_MOVE) { + if (!mp_has_runnable_buffer(mp)) { // handle condition where zero-length move is last or only move + cm_cycle_end(); // ...otherwise cycle will not end properly + } + status = STAT_OK; } return (status); } @@ -1273,8 +1272,8 @@ stat_t cm_straight_feed(const float target[], const bool flags[]) } cm->gm.motion_mode = MOTION_MODE_STRAIGHT_FEED; - if (!(flags[AXIS_X] | flags[AXIS_Y] | flags[AXIS_Z] | - flags[AXIS_A] | flags[AXIS_B] | flags[AXIS_C])) { + // it's legal for a G1 to have no axis words but we don't want to process it + if (!(flags[AXIS_X] | flags[AXIS_Y] | flags[AXIS_Z] | flags[AXIS_A] | flags[AXIS_B] | flags[AXIS_C])) { return(STAT_OK); } @@ -1283,12 +1282,17 @@ stat_t cm_straight_feed(const float target[], const bool flags[]) cm_set_display_offsets(&cm->gm); // capture the fully resolved offsets to the state cm_cycle_start(); // required for homing & other cycles stat_t status = mp_aline(&cm->gm); // send the move to the planner + cm_update_model_position(); // <-- ONLY safe because we don't care about status... - cm_finalize_move(); // <-- ONLY safe because we don't care about status... - - if (status == STAT_MINIMUM_LENGTH_MOVE && !mp_has_runnable_buffer(mp)) { - cm_cycle_end(); - return (STAT_OK); +// if (status == STAT_MINIMUM_LENGTH_MOVE && !mp_has_runnable_buffer(mp)) { //mp applies to currently active planner +// cm_cycle_end(); +// return (STAT_OK); +// } + if (status == STAT_MINIMUM_LENGTH_MOVE) { + if (!mp_has_runnable_buffer(mp)) { // handle condition where zero-length move is last or only move + cm_cycle_end(); // ...otherwise cycle will not end properly + } + status = STAT_OK; } return (status); } @@ -1605,7 +1609,7 @@ void cm_cycle_start() void cm_cycle_end() { - if(cm->cycle_state == CYCLE_MACHINING) { + if (cm->cycle_state == CYCLE_MACHINING) { float value[] = { (float)MACHINE_PROGRAM_STOP }; _exec_program_finalize(value, nullptr); } diff --git a/g2core/canonical_machine.h b/g2core/canonical_machine.h index 3caf0d54..8522d83f 100644 --- a/g2core/canonical_machine.h +++ b/g2core/canonical_machine.h @@ -341,8 +341,7 @@ float cm_get_display_position(const GCodeState_t *gcode_state, const uint8_t axi float cm_get_absolute_position(const GCodeState_t *gcode_state, const uint8_t axis); // Critical helpers -void cm_update_model_position_from_runtime(void); -void cm_finalize_move(void); +void cm_update_model_position(void); stat_t cm_deferred_write_callback(void); void cm_set_model_target(const float target[], const bool flag[]); bool cm_get_soft_limits(void); diff --git a/g2core/controller.cpp b/g2core/controller.cpp index 3ff80073..923a7994 100644 --- a/g2core/controller.cpp +++ b/g2core/controller.cpp @@ -166,8 +166,9 @@ static void _controller_HSM() DISPATCH(cm_probing_cycle_callback()); // probing cycle operation (G38.2) DISPATCH(cm_jogging_cycle_callback()); // jog cycle operation DISPATCH(cm_deferred_write_callback()); // persist G10 changes when not in machining cycle -// DISPATCH(cm_feedhold_command_blocker()); // blocks new commands from arriving while in feedhold - +// DISPATCH(cm_feedhold_sequencing_callback());// feedhold state machine runner + DISPATCH(cm_feedhold_command_blocker()); // blocks new Gcode from arriving while in feedhold + //----- command readers and parsers --------------------------------------------------// DISPATCH(_sync_to_planner()); // ensure there is at least one free buffer in planning queue diff --git a/g2core/plan_arc.cpp b/g2core/plan_arc.cpp index 013b3a05..775bbe83 100644 --- a/g2core/plan_arc.cpp +++ b/g2core/plan_arc.cpp @@ -250,7 +250,7 @@ stat_t cm_arc_feed(const float target[], const bool target_f[], // target en cm_cycle_start(); // if not already started cm->arc.run_state = BLOCK_ACTIVE; // enable arc to be run from the callback - cm_finalize_move(); + cm_update_model_position(); return (STAT_OK); } diff --git a/g2core/plan_zoid.cpp b/g2core/plan_zoid.cpp index 7deecd09..9d37d039 100644 --- a/g2core/plan_zoid.cpp +++ b/g2core/plan_zoid.cpp @@ -325,7 +325,10 @@ void mp_calculate_ramps(mpBlockRuntimeBuf_t* block, mpBuf_t* bf, const float ent // Rate-limited asymmetric cases (3) // compute meet velocity to see if the cruise velocity rises above the entry and/or exit velocities block->cruise_velocity = _get_meet_velocity(entry_velocity, block->exit_velocity, bf->length, bf, block); - TRAP_ZERO(block->cruise_velocity, "zoid() Vc=0 asymmetric HT case"); +// TRAP_ZERO(block->cruise_velocity, "zoid() Vc=0 asymmetric HT case"); + if (fp_ZERO(block->cruise_velocity)) { + __asm__("BKPT"); + } // We now store the head/tail lengths we computed in _get_meet_velocity. // treat as a full up and down (head and tail)