From 08e20dbbc6b89cbb886d707676b345778bf71dbc Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Thu, 3 May 2018 20:23:35 -0500 Subject: [PATCH] Improve sync of stepper positions --- Marlin/Marlin_main.cpp | 9 +++---- Marlin/planner.cpp | 58 ++++++++++++++++++++++++++---------------- Marlin/planner.h | 32 ++++++++++++++++++++--- Marlin/stepper.cpp | 44 ++++++++++++-------------------- Marlin/stepper.h | 29 ++++++++++++++++++--- 5 files changed, 109 insertions(+), 63 deletions(-) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 0e737f2520..3791aae687 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -6258,8 +6258,6 @@ void home_all_axes() { gcode_G28(true); } */ inline void gcode_G92() { - stepper.synchronize(); - #if ENABLED(CNC_COORDINATE_SYSTEMS) switch (parser.subcode) { case 1: @@ -6319,10 +6317,9 @@ inline void gcode_G92() { COPY(coordinate_system[active_coordinate_system], position_shift); #endif - if (didXYZ) - SYNC_PLAN_POSITION_KINEMATIC(); - else if (didE) - sync_plan_position_e(); + // Update planner/steppers only if the native coordinates changed + if (didXYZ) SYNC_PLAN_POSITION_KINEMATIC(); + else if (didE) sync_plan_position_e(); report_current_position(); } diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index 6e2ecb04e5..8e5733a6d3 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -819,15 +819,9 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE] const float esteps_float = de * e_factor[extruder]; const int32_t esteps = abs(esteps_float) + 0.5; - // Calculate the buffer head after we push this byte - const uint8_t next_buffer_head = next_block_index(block_buffer_head); - - // If the buffer is full: good! That means we are well ahead of the robot. - // Rest here until there is room in the buffer. - while (block_buffer_tail == next_buffer_head) idle(); - - // Prepare to set up new block - block_t* block = &block_buffer[block_buffer_head]; + // Wait for the next available block + uint8_t next_buffer_head; + block_t * const block = get_next_free_block(next_buffer_head); // Clear all flags, including the "busy" bit block->flag = 0x00; @@ -1467,6 +1461,26 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE] } // _buffer_steps() +/** + * Planner::buffer_sync_block + * Add a block to the buffer that just updates the position + */ +void Planner::buffer_sync_block() { + // Wait for the next available block + uint8_t next_buffer_head; + block_t * const block = get_next_free_block(next_buffer_head); + + block->steps[A_AXIS] = position[A_AXIS]; + block->steps[B_AXIS] = position[B_AXIS]; + block->steps[C_AXIS] = position[C_AXIS]; + block->steps[E_AXIS] = position[E_AXIS]; + + block->flag = BLOCK_FLAG_SYNC_POSITION; + + block_buffer_head = next_buffer_head; + stepper.wake_up(); +} // buffer_sync_block() + /** * Planner::buffer_segment * @@ -1595,19 +1609,19 @@ void Planner::_set_position_mm(const float &a, const float &b, const float &c, c #else #define _EINDEX E_AXIS #endif - const int32_t na = position[A_AXIS] = LROUND(a * axis_steps_per_mm[A_AXIS]), - nb = position[B_AXIS] = LROUND(b * axis_steps_per_mm[B_AXIS]), - nc = position[C_AXIS] = LROUND(c * axis_steps_per_mm[C_AXIS]), - ne = position[E_AXIS] = LROUND(e * axis_steps_per_mm[_EINDEX]); + position[A_AXIS] = LROUND(a * axis_steps_per_mm[A_AXIS]), + position[B_AXIS] = LROUND(b * axis_steps_per_mm[B_AXIS]), + position[C_AXIS] = LROUND(c * axis_steps_per_mm[C_AXIS]), + position[E_AXIS] = LROUND(e * axis_steps_per_mm[_EINDEX]); #if HAS_POSITION_FLOAT - position_float[X_AXIS] = a; - position_float[Y_AXIS] = b; - position_float[Z_AXIS] = c; + position_float[A_AXIS] = a; + position_float[B_AXIS] = b; + position_float[C_AXIS] = c; position_float[E_AXIS] = e; #endif - stepper.set_position(na, nb, nc, ne); previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest. ZERO(previous_speed); + buffer_sync_block(); } void Planner::set_position_mm_kinematic(const float (&cart)[XYZE]) { @@ -1655,23 +1669,23 @@ void Planner::set_position_mm(const AxisEnum axis, const float &v) { #if HAS_POSITION_FLOAT position_float[axis] = v; #endif - stepper.set_position(axis, position[axis]); previous_speed[axis] = 0.0; + buffer_sync_block(); } // Recalculate the steps/s^2 acceleration rates, based on the mm/s^2 void Planner::reset_acceleration_rates() { #if ENABLED(DISTINCT_E_FACTORS) - #define HIGHEST_CONDITION (i < E_AXIS || i == E_AXIS + active_extruder) + #define AXIS_CONDITION (i < E_AXIS || i == E_AXIS + active_extruder) #else - #define HIGHEST_CONDITION true + #define AXIS_CONDITION true #endif uint32_t highest_rate = 1; LOOP_XYZE_N(i) { max_acceleration_steps_per_s2[i] = max_acceleration_mm_per_s2[i] * axis_steps_per_mm[i]; - if (HIGHEST_CONDITION) NOLESS(highest_rate, max_acceleration_steps_per_s2[i]); + if (AXIS_CONDITION) NOLESS(highest_rate, max_acceleration_steps_per_s2[i]); } - cutoff_long = 4294967295UL / highest_rate; + cutoff_long = 4294967295UL / highest_rate; // 0xFFFFFFFFUL } // Recalculate position, steps_to_mm if axis_steps_per_mm changes! diff --git a/Marlin/planner.h b/Marlin/planner.h index af845600a5..e051c4a0dc 100644 --- a/Marlin/planner.h +++ b/Marlin/planner.h @@ -53,14 +53,18 @@ enum BlockFlagBit : char { BLOCK_BIT_BUSY, // The block is segment 2+ of a longer move - BLOCK_BIT_CONTINUED + BLOCK_BIT_CONTINUED, + + // Sync the stepper counts from the block + BLOCK_BIT_SYNC_POSITION }; enum BlockFlag : char { BLOCK_FLAG_RECALCULATE = _BV(BLOCK_BIT_RECALCULATE), BLOCK_FLAG_NOMINAL_LENGTH = _BV(BLOCK_BIT_NOMINAL_LENGTH), BLOCK_FLAG_BUSY = _BV(BLOCK_BIT_BUSY), - BLOCK_FLAG_CONTINUED = _BV(BLOCK_BIT_CONTINUED) + BLOCK_FLAG_CONTINUED = _BV(BLOCK_BIT_CONTINUED), + BLOCK_FLAG_SYNC_POSITION = _BV(BLOCK_BIT_SYNC_POSITION) }; /** @@ -409,6 +413,20 @@ class Planner { #endif + + /** + * Planner::get_next_free_block + * + * - Get the next head index (passed by reference) + * - Wait for a space to open up in the planner + * - Return the head block + */ + FORCE_INLINE static block_t* get_next_free_block(uint8_t &next_buffer_head) { + next_buffer_head = next_block_index(block_buffer_head); + while (block_buffer_tail == next_buffer_head) idle(); // while (is_full) + return &block_buffer[block_buffer_head]; + } + /** * Planner::_buffer_steps * @@ -426,6 +444,12 @@ class Planner { , float fr_mm_s, const uint8_t extruder, const float &millimeters=0.0 ); + /** + * Planner::buffer_sync_block + * Add a block to the buffer that just updates the position + */ + static void buffer_sync_block(); + /** * Planner::buffer_segment * @@ -505,7 +529,7 @@ class Planner { static void set_position_mm_kinematic(const float (&cart)[XYZE]); static void set_position_mm(const AxisEnum axis, const float &v); FORCE_INLINE static void set_z_position_mm(const float &z) { set_position_mm(Z_AXIS, z); } - FORCE_INLINE static void set_e_position_mm(const float &e) { set_position_mm(AxisEnum(E_AXIS), e); } + FORCE_INLINE static void set_e_position_mm(const float &e) { set_position_mm(E_AXIS, e); } /** * Sync from the stepper positions. (e.g., after an interrupted move) @@ -515,7 +539,7 @@ class Planner { /** * Does the buffer have any blocks queued? */ - static inline bool has_blocks_queued() { return (block_buffer_head != block_buffer_tail); } + FORCE_INLINE static bool has_blocks_queued() { return (block_buffer_head != block_buffer_tail); } /** * "Discard" the block and "release" the memory. diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp index 46707b5737..06dc29dc18 100644 --- a/Marlin/stepper.cpp +++ b/Marlin/stepper.cpp @@ -449,18 +449,30 @@ void Stepper::isr() { // If there is no current block, attempt to pop one from the buffer if (!current_block) { + // Anything in the buffer? if ((current_block = planner.get_current_block())) { + + // Sync block? Sync the stepper counts and return + while (TEST(current_block->flag, BLOCK_BIT_SYNC_POSITION)) { + _set_position( + current_block->steps[A_AXIS], current_block->steps[B_AXIS], + current_block->steps[C_AXIS], current_block->steps[E_AXIS] + ); + planner.discard_current_block(); + if (!(current_block = planner.get_current_block())) return; + } + trapezoid_generator_reset(); // Initialize Bresenham counters to 1/2 the ceiling counter_X = counter_Y = counter_Z = counter_E = -(current_block->step_event_count >> 1); - #if ENABLED(MIXING_EXTRUDER) MIXING_STEPPERS_LOOP(i) counter_m[i] = -(current_block->mix_event_count[i] >> 1); #endif + // No step events completed so far step_events_completed = 0; #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE) @@ -469,6 +481,7 @@ void Stepper::isr() { #endif #if ENABLED(Z_LATE_ENABLE) + // If delayed Z enable, postpone move for 1mS if (current_block->steps[Z_AXIS] > 0) { enable_Z(); _NEXT_ISR(2000); // Run at slow speed - 1 KHz @@ -595,7 +608,6 @@ void Stepper::isr() { #endif #if ENABLED(LIN_ADVANCE) - counter_E += current_block->steps[E_AXIS]; if (counter_E > 0) { #if DISABLED(MIXING_EXTRUDER) @@ -708,7 +720,6 @@ void Stepper::isr() { acceleration_time += interval; #if ENABLED(LIN_ADVANCE) - if (current_block->use_advance_lead) { if (step_events_completed == step_loops || (e_steps && eISR_Rate != current_block->advance_speed)) { nextAdvanceISR = 0; // Wake up eISR on first acceleration loop and fire ISR if final adv_rate is reached @@ -719,7 +730,6 @@ void Stepper::isr() { eISR_Rate = ADV_NEVER; if (e_steps) nextAdvanceISR = 0; } - #endif // LIN_ADVANCE } else if (step_events_completed > (uint32_t)current_block->decelerate_after) { @@ -742,7 +752,6 @@ void Stepper::isr() { deceleration_time += interval; #if ENABLED(LIN_ADVANCE) - if (current_block->use_advance_lead) { if (step_events_completed <= (uint32_t)current_block->decelerate_after + step_loops || (e_steps && eISR_Rate != current_block->advance_speed)) { nextAdvanceISR = 0; // Wake up eISR on first deceleration loop @@ -753,16 +762,13 @@ void Stepper::isr() { eISR_Rate = ADV_NEVER; if (e_steps) nextAdvanceISR = 0; } - #endif // LIN_ADVANCE } else { #if ENABLED(LIN_ADVANCE) - // If we have esteps to execute, fire the next advance_isr "now" if (e_steps && eISR_Rate != current_block->advance_speed) nextAdvanceISR = 0; - #endif SPLIT(OCR1A_nominal); // split step into multiple ISRs if larger than ENDSTOP_NOMINAL_OCR_VAL @@ -902,6 +908,7 @@ void Stepper::isr() { } void Stepper::advance_isr_scheduler() { + // Run main stepping ISR if flagged if (!nextMainISR) isr(); @@ -1120,12 +1127,7 @@ void Stepper::synchronize() { while (planner.has_blocks_queued() || cleaning_buf * This allows get_axis_position_mm to correctly * derive the current XYZ position later on. */ -void Stepper::set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e) { - - synchronize(); // Bad to set stepper counts in the middle of a move - - CRITICAL_SECTION_START; - +void Stepper::_set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e) { #if CORE_IS_XY // corexy positioning // these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html @@ -1148,21 +1150,7 @@ void Stepper::set_position(const int32_t &a, const int32_t &b, const int32_t &c, count_position[Y_AXIS] = b; count_position[Z_AXIS] = c; #endif - count_position[E_AXIS] = e; - CRITICAL_SECTION_END; -} - -void Stepper::set_position(const AxisEnum &axis, const int32_t &v) { - CRITICAL_SECTION_START; - count_position[axis] = v; - CRITICAL_SECTION_END; -} - -void Stepper::set_e_position(const int32_t &e) { - CRITICAL_SECTION_START; - count_position[E_AXIS] = e; - CRITICAL_SECTION_END; } /** diff --git a/Marlin/stepper.h b/Marlin/stepper.h index cdd8a3b161..597600e805 100644 --- a/Marlin/stepper.h +++ b/Marlin/stepper.h @@ -202,9 +202,32 @@ class Stepper { // // Set the current position in steps // - static void set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e); - static void set_position(const AxisEnum &a, const int32_t &v); - static void set_e_position(const int32_t &e); + static void _set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e); + + FORCE_INLINE static void _set_position(const AxisEnum a, const int32_t &v) { count_position[a] = v; } + + FORCE_INLINE static void set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e) { + synchronize(); + CRITICAL_SECTION_START; + _set_position(a, b, c, e); + CRITICAL_SECTION_END; + } + + static void set_position(const AxisEnum a, const int32_t &v) { + synchronize(); + CRITICAL_SECTION_START; + count_position[a] = v; + CRITICAL_SECTION_END; + } + + FORCE_INLINE static void _set_e_position(const int32_t &e) { count_position[E_AXIS] = e; } + + static void set_e_position(const int32_t &e) { + synchronize(); + CRITICAL_SECTION_START; + count_position[E_AXIS] = e; + CRITICAL_SECTION_END; + } // // Set direction bits for all steppers