From 1b5b40e61d93b868c61947519fc82c73dc20ca4a Mon Sep 17 00:00:00 2001 From: Markus Hitter Date: Tue, 29 Oct 2013 14:10:32 +0100 Subject: [PATCH] dda_lookahead.c: move crossing speed calculation into a dedicated function. This is mostly a preparation for reverse walks through the movement queue, where crossing speed calculation is done only once, while actually used speeds can be raised successively with repeated walks. --- dda_lookahead.c | 224 ++++++++++++++++++++++++++++-------------------- dda_lookahead.h | 16 +--- 2 files changed, 134 insertions(+), 106 deletions(-) diff --git a/dda_lookahead.c b/dda_lookahead.c index 37c6d0c..aba97b0 100644 --- a/dda_lookahead.c +++ b/dda_lookahead.c @@ -151,16 +151,134 @@ void dda_emergency_shutdown(PGM_P msg) { } /** - * Join 2 moves by removing the full stop between them, where possible. - * To join the moves, the expected jerk - or force - of the change in direction is calculated. - * The jerk is used to scale the common feed rate between both moves to obtain an acceptable speed - * to transition between 'prev' and 'current'. + * \brief Find maximum corner speed between two moves. + * \details Find out how fast we can move around around a corner without + * exceeding the expected jerk. Worst case this speed is zero, which means a + * full stop between both moves. Best case it's the lower of the maximum speeds. * - * Premise: we currently join the last move in the queue and the one before it (if any). - * This means the feed rate at the end of the 'current' move is 0. + * This function is expected to be called from within dda_start(). * - * Premise: the 'current' move is not dispatched in the queue: it should remain constant while this - * function is running. + * \param [in] prev is the DDA structure of the move previous to the current one. + * \param [in] current is the DDA structure of the move currently created. + * + * \return dda->crossF + */ +void dda_find_crossing_speed(DDA *prev, DDA *current) { + int32_t jerk, jerk_e; + uint32_t crossF; + + // Bail out if there's nothing to join (e.g. G1 F1500). + if ( ! prev || prev->nullmove) { + current->crossF = 0; + return; + } + + // Only prepare for look-ahead if we have 2 available moves to + // join and the Z axis is unused (for now, Z axis moves are NOT joined). + if (prev->live == 0 && prev->delta_um.Z == current->delta_um.Z) { + // Calculate the jerk if the previous move and this move would be joined + // together at full speed. + jerk = dda_jerk_size_2d(prev->delta_um.X, prev->delta_um.Y, prev->endpoint.F, + current->delta_um.X, current->delta_um.Y, current->endpoint.F); + jerk_e = dda_jerk_size_1d(prev->delta_um.E, prev->endpoint.F, + current->delta_um.E, current->endpoint.F); + + if (DEBUG_DDA && (debug_flags & DEBUG_DDA)) + sersendf_P(PSTR("Jerk %lu Jerk_e: %lu\n"), jerk, jerk_e); + } else { + // Move already executing or Z moved: abort the join + current->crossF = 0; + return; + } + + // The initial crossing speed is the minimum between both target speeds + // Note: this is a given: the start speed and end speed can NEVER be + // higher than the target speed in a move! + // Note 2: this provides an upper limit, if needed, the speed is lowered. + crossF = prev->endpoint.F; + if (crossF > current->endpoint.F) + crossF = current->endpoint.F; + + // If the XY jerk is too big, scale the proposed cross speed. + if (jerk > LOOKAHEAD_MAX_JERK_XY) { + + if (DEBUG_DDA && (debug_flags & DEBUG_DDA)) + sersendf_P(PSTR("XY Jerk too big.\n")); + + // Perform an exponential scaling. + // Use unsigned to double the range before overflowing. + crossF = (crossF * LOOKAHEAD_MAX_JERK_XY * LOOKAHEAD_MAX_JERK_XY) / + ((uint32_t)jerk * (uint32_t)jerk); + + // Optimize: if the crossing speed is zero, there is no join possible. + if (crossF == 0) { + current->crossF = 0; + return; + } + + // Safety: make sure we never exceed the maximum speed of a move + if (crossF > current->endpoint.F) + crossF = current->endpoint.F; + if (crossF > prev->endpoint.F) + crossF = prev->endpoint.F; + } + + // Same to the extruder jerk: make sure we do not yank it. + if (jerk_e > LOOKAHEAD_MAX_JERK_E) { + uint32_t crossF2; + + if (DEBUG_DDA && (debug_flags & DEBUG_DDA)) + sersendf_P(PSTR("E Jerk too big.\n")); + + crossF2 = MAX(current->endpoint.F, prev->endpoint.F); + + // Perform an exponential scaling. + // Use unsigned to double the range before overflowing. + crossF2 = (crossF2 * LOOKAHEAD_MAX_JERK_E * LOOKAHEAD_MAX_JERK_E) / + ((uint32_t)jerk * (uint32_t)jerk); + + // Only continue with joining if there is a feasible crossing speed. + if (crossF2 == 0) { + current->crossF = 0; + return; + } + + // Safety: make sure the proposed speed is not higher than the target speeds of each move + crossF2 = MIN(crossF2, current->endpoint.F); + crossF2 = MIN(crossF2, prev->endpoint.F); + + if (crossF2 > crossF) { + sersendf_P(PSTR("Jerk_e: %lu => crossF: %lu (original: %lu)\r\n"), + jerk_e, crossF2, crossF); + } + + // Pick the crossing speed for these 2 move to be within the jerk limits + crossF = MIN(crossF, crossF2); + } + + current->crossF = crossF; +} + +/** + * \brief Join 2 moves by removing the full stop between them, where possible. + * \details To join the moves, the deceleration ramp of the previous move and + * the acceleration ramp of the current move are shortened, resulting in a + * non-zero speed at that point. The target speed at the corner is already to + * be found in dda->crossF. See dda_find_corner_speed(). + * + * Ideally, both ramps can be reduced to actually have Fcorner at the corner, + * but the surrounding movements might no be long enough to achieve this speed. + * Analysing both moves to find the best result is done here. + * + * TODO: to achieve better results with short moves (move distance < both ramps), + * this function should be able to enhance the corner speed on repeated + * calls when reverse-stepping through the movement queue. + * + * \param [in] prev is the DDA structure of the move previous to the current one. + * \param [in] current is the DDA structure of the move currently created. + * + * Premise: the 'current' move is not dispatched in the queue: it should remain + * constant while this function is running. * * Note: the planner always makes sure the movement can be stopped within the * last move (= 'current'); as a result a lot of small moves will still limit the speed. @@ -172,48 +290,25 @@ void dda_join_moves(DDA *prev, DDA *current) { // when we are done (and the previous move is not already active). uint32_t prev_F, prev_F_start, prev_F_end, prev_end; uint32_t prev_rampup, prev_rampdown, prev_total_steps; + uint32_t crossF; uint8_t prev_id; // Similarly, we only want to modify the current move if we have the results of the calculations; // until then, we do not want to touch the current move settings. // Note: we assume 'current' will not be dispatched while this function runs, so we do not to // back up the move settings: they will remain constant. uint32_t this_F_start, this_start, this_rampup, this_rampdown; - int32_t jerk, jerk_e; // Expresses the forces if we would change directions at full speed static uint32_t la_cnt = 0; // Counter: how many moves did we join? #ifdef LOOKAHEAD_DEBUG static uint32_t moveno = 0; // Debug counter to number the moves - helps while debugging moveno++; #endif + dda_find_crossing_speed(prev, current); + // Bail out if there's nothing to join (e.g. G1 F1500). - if ( ! prev || prev->nullmove) + if ( ! prev || prev->nullmove || current->crossF == 0) return; - serprintf(PSTR("Current Delta: %ld,%ld,%ld E:%ld Live:%d\r\n"), - current->delta_um.X, current->delta_um.Y, current->delta_um.Z, - current->delta_um.E, current->live); - serprintf(PSTR("Prev Delta: %ld,%ld,%ld E:%ld Live:%d\r\n"), - prev->delta_um.X, prev->delta_um.Y, prev->delta_um.Z, - prev->delta_um.E, prev->live); - - // Look-ahead: attempt to join moves into smooth movements - // Note: moves are only modified after the calculations are complete. - // Only prepare for look-ahead if we have 2 available moves to - // join and the Z axis is unused (for now, Z axis moves are NOT joined). - if (prev->live == 0 && prev->delta_um.Z == current->delta_um.Z) { - // Calculate the jerk if the previous move and this move would be joined - // together at full speed. - jerk = dda_jerk_size_2d(prev->delta_um.X, prev->delta_um.Y, prev->endpoint.F, - current->delta_um.X, current->delta_um.Y, current->endpoint.F); - serprintf(PSTR("Jerk: %lu\r\n"), jerk); - jerk_e = dda_jerk_size_1d(prev->delta_um.E, prev->endpoint.F, - current->delta_um.E, current->endpoint.F); - serprintf(PSTR("Jerk_e: %lu\r\n"), jerk_e); - } else { - // Move already executing or Z moved: abort the join - return; - } - // Make sure we have 2 moves and the previous move is not already active if (prev->live == 0) { // Perform an atomic copy to preserve volatile parameters during the calculations @@ -225,65 +320,12 @@ void dda_join_moves(DDA *prev, DDA *current) { prev_rampup = prev->rampup_steps; prev_rampdown = prev->rampdown_steps; prev_total_steps = prev->total_steps; + crossF = current->crossF; ATOMIC_END - // The initial crossing speed is the minimum between both target speeds - // Note: this is a given: the start speed and end speed can NEVER be - // higher than the target speed in a move! - // Note 2: this provides an upper limit, if needed, the speed is lowered. - uint32_t crossF = prev_F; - if(crossF > current->endpoint.F) crossF = current->endpoint.F; - - //sersendf_P(PSTR("j:%lu - XF:%lu"), jerk, crossF); - - // If the XY jerk is too big, scale the proposed cross speed - if(jerk > LOOKAHEAD_MAX_JERK_XY) { - serprintf(PSTR("Jerk too big: scale cross speed between moves\r\n")); - // Get the highest speed between both moves - if(crossF < prev_F) - crossF = prev_F; - - // Perform an exponential scaling - uint32_t ujerk = (uint32_t)jerk; // Use unsigned to double the range before overflowing - crossF = (crossF*LOOKAHEAD_MAX_JERK_XY*LOOKAHEAD_MAX_JERK_XY)/(ujerk*ujerk); - - // Optimize: if the crossing speed is zero, there is no join possible between these - // two (fast) moves. Stop calculating and leave the full stop that is currently between - // them. - if(crossF == 0) - return; - - // Safety: make sure we never exceed the maximum speed of a move - if(crossF > current->endpoint.F) crossF = current->endpoint.F; - if(crossF > prev_F) crossF = prev_F; - sersendf_P(PSTR("=>F:%lu"), crossF); - } - // Same to the extruder jerk: make sure we do not yank it - if(jerk_e > LOOKAHEAD_MAX_JERK_E) { - sersendf_P(PSTR("Jerk_e too big: scale cross speed between moves\r\n")); - uint32_t crossF2 = MAX(current->endpoint.F, prev_F); - - // Perform an exponential scaling - uint32_t ujerk = (uint32_t)jerk_e; // Use unsigned to double the range before overflowing - crossF2 = (crossF2*LOOKAHEAD_MAX_JERK_E*LOOKAHEAD_MAX_JERK_E)/(ujerk*ujerk); - - // Only continue with joining if there is a feasible crossing speed - if(crossF2 == 0) return; - - // Safety: make sure the proposed speed is not higher than the target speeds of each move - crossF2 = MIN(crossF2, current->endpoint.F); - crossF2 = MIN(crossF2, prev_F); - - if(crossF2 > crossF) { - sersendf_P(PSTR("Jerk_e: %lu => crossF: %lu (original: %lu)\r\n"), jerk_e, crossF2, crossF); - } - - // Pick the crossing speed for these 2 move to be within the jerk limits - crossF = MIN(crossF, crossF2); - } - // Show the proposed crossing speed - this might get adjusted below - serprintf(PSTR("Initial crossing speed: %lu\r\n"), crossF); + if (DEBUG_DDA && (debug_flags & DEBUG_DDA)) + sersendf_P(PSTR("Initial crossing speed: %lu\n"), crossF); // Forward check: test if we can actually reach the target speed in the previous move // If not: we need to determine the obtainable speed and adjust crossF accordingly. diff --git a/dda_lookahead.h b/dda_lookahead.h index a87dd42..633a263 100644 --- a/dda_lookahead.h +++ b/dda_lookahead.h @@ -44,21 +44,7 @@ #define MAX(a,b) (((a)>(b))?(a):(b)) #define MIN(a,b) (((a)<(b))?(a):(b)) -/** - * Join 2 moves by removing the full stop between them, where possible. - * To join the moves, the expected jerk - or force - of the change in direction is calculated. - * The jerk is used to scale the common feed rate between both moves to obtain an acceptable speed - * to transition between 'prev' and 'current'. - * - * Premise: we currently join the last move in the queue and the one before it (if any). - * This means the feed rate at the end of the 'current' move is 0. - * - * Premise: the 'current' move is not dispatched in the queue: it should remain constant while this - * function is running. - * - * Note: the planner always makes sure the movement can be stopped within the - * last move (= 'current'); as a result a lot of small moves will still limit the speed. - */ +void dda_find_crossing_speed(DDA *prev, DDA *current); void dda_join_moves(DDA *prev, DDA *current); // Debug counters