From 297aa28dfd658b59adb16ec9dd6899f6d87d9ac1 Mon Sep 17 00:00:00 2001 From: Roland Brochard Date: Tue, 10 Sep 2013 23:03:47 +0200 Subject: [PATCH] Lookahead: refactored code to compute everything in dda steps. --- dda.c | 2 - dda.h | 2 - dda_lookahead.c | 263 +++++++++++++++--------------------------------- 3 files changed, 82 insertions(+), 185 deletions(-) diff --git a/dda.c b/dda.c index be226e1..8bc953e 100644 --- a/dda.c +++ b/dda.c @@ -132,9 +132,7 @@ void dda_create(DDA *dda, TARGET *target) { // Set the start and stop speeds to zero for now = full stops between // moves. Also fallback if lookahead calculations fail to finish in time. dda->crossF = 0; - dda->F_start = 0; dda->start_steps = 0; - dda->F_end = 0; dda->end_steps = 0; // Give this move an identifier. dda->id = idcnt++; diff --git a/dda.h b/dda.h index 28e8ff9..ad65146 100644 --- a/dda.h +++ b/dda.h @@ -150,9 +150,7 @@ typedef struct { uint32_t distance; uint32_t crossF; // These two are based on the "fast" axis, the axis with the most steps. - uint32_t F_start; uint32_t start_steps; ///< would be required to reach start feedrate - uint32_t F_end; uint32_t end_steps; ///< would be required to stop from end feedrate // Displacement vector, in um, based between the difference of the starting // point and the target. Required to obtain the jerk between 2 moves. diff --git a/dda_lookahead.c b/dda_lookahead.c index 5efd2ab..f3d45b8 100644 --- a/dda_lookahead.c +++ b/dda_lookahead.c @@ -41,7 +41,7 @@ uint32_t lookahead_timeout = 0; // Moves that did not compute in time to be #endif // We also need the inverse: given a ramp length, determine the expected speed -// Note: the calculation is scaled by a factor 10000 to obtain an answer with a smaller +// Note: the calculation is scaled by a factor 16384 to obtain an answer with a smaller // rounding error. // Warning: this is an expensive function as it requires a square root to get the result. // @@ -53,9 +53,9 @@ uint32_t dda_steps_to_velocity(uint32_t steps) { // F_steps = sqrt((2000*a)/STEPS_PER_M_X) * 60 * sqrt(steps) static uint32_t part = 0; if(part == 0) - part = int_sqrt((uint32_t)((2000.0f*ACCELERATION*3600.0f*10000.0f)/(float)STEPS_PER_M_X)); - uint32_t res = int_sqrt((steps) * 10000) * part; - return res / 10000; + part = (uint32_t)sqrtf((2000.0f*3600.0f*ACCELERATION*16384.0f)/(float)STEPS_PER_M_X); + uint32_t res = int_sqrt((steps) << 14) * part; + return res >> 14; } /** @@ -69,7 +69,7 @@ uint32_t dda_steps_to_velocity(uint32_t steps) { * @param F2 feed rate of second move */ int dda_jerk_size_2d_real(int32_t x1, int32_t y1, uint32_t F1, int32_t x2, int32_t y2, uint32_t F2) { - const int maxlen = 10000; + const int maxlen = 16384; // Normalize vectors so their length will be fixed // Note: approx_distance is not precise enough and may result in violent direction changes //sersendf_P(PSTR("Input vectors: (%ld, %ld) and (%ld, %ld)\r\n"),x1,y1,x2,y2); @@ -302,15 +302,16 @@ void dda_join_moves(DDA *prev, DDA *current) { // Calculating the look-ahead settings can take a while; before modifying // the previous move, we need to locally store any values and write them // 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_F, prev_F_in_steps, prev_F_start_in_steps, prev_F_end_in_steps; uint32_t prev_rampup, prev_rampdown, prev_total_steps; - uint32_t crossF, currF; + uint32_t crossF, crossF_in_steps; 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; + uint32_t this_F, this_F_in_steps, this_F_start_in_steps, this_rampup, this_rampdown, this_total_steps; + uint8_t this_id; 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 @@ -321,25 +322,32 @@ void dda_join_moves(DDA *prev, DDA *current) { if ( ! prev || prev->nullmove || current->crossF == 0) return; + // Show the proposed crossing speed - this might get adjusted below. + if (DEBUG_DDA && (debug_flags & DEBUG_DDA)) + sersendf_P(PSTR("Initial crossing speed: %lu\n"), crossF); + // 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 ATOMIC_START prev_id = prev->id; prev_F = prev->endpoint.F; - prev_F_start = prev->F_start; - prev_F_end = prev->F_end; + prev_F_start_in_steps = prev->start_steps; + prev_F_end_in_steps = prev->end_steps; prev_rampup = prev->rampup_steps; prev_rampdown = prev->rampdown_steps; prev_total_steps = prev->total_steps; crossF = current->crossF; + this_id = current->id; + this_F = current->endpoint.F; + this_total_steps = current->total_steps; ATOMIC_END // Here we have to distinguish between feedrate along the movement // direction and feedrate of the fast axis. They can differ by a factor // of 2. // Along direction: F, crossF. - // Fast axis already: F_start, F_end. + // Along fast axis already: start_steps, end_steps. // // All calculations here are done along the fast axis, so recalculate // F and crossF to match this, too. @@ -374,185 +382,81 @@ void dda_join_moves(DDA *prev, DDA *current) { sersendf_P(PSTR("WTF? No current fast axis found\n")); } crossF = muldiv(fast_um, crossF, current->distance); - currF = muldiv(fast_um, current->endpoint.F, current->distance); + this_F = muldiv(fast_um, current->endpoint.F, current->distance); + + prev_F_in_steps = ACCELERATE_RAMP_LEN(prev_F); + this_F_in_steps = ACCELERATE_RAMP_LEN(this_F); + crossF_in_steps = ACCELERATE_RAMP_LEN(crossF); // Show the proposed crossing speed - this might get adjusted below - if (DEBUG_DDA && (debug_flags & DEBUG_DDA)) - sersendf_P(PSTR("Initial crossing speed: %lu\n"), crossF); + serprintf(PSTR("Initial crossing speed: %lu\r\n"), crossF_in_steps); - // 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. - // Note: these ramps can be longer than the move: if so we can not reach top speed. - uint32_t up = ACCELERATE_RAMP_LEN(prev_F) - ACCELERATE_RAMP_LEN(prev_F_start); - uint32_t down = ACCELERATE_RAMP_LEN(prev_F) - ACCELERATE_RAMP_LEN(crossF); - // Test if both the ramp up and ramp down fit within the move - if(up+down > prev_total_steps) { - // Test if we can reach the crossF rate: if the difference between both ramps is larger - // than the move itself, there is no ramp up or down from F_start to crossF... - uint32_t diff = (up>down) ? up-down : down-up; - if(diff > prev_total_steps) { - // Cannot reach crossF from F_start, lower crossF and adjust both ramp-up and down - down = 0; - // Before we can determine how fast we can go in this move, we need the number of - // steps needed to reach the entry speed. - uint32_t prestep = ACCELERATE_RAMP_LEN(prev_F_start); - // Calculate what feed rate we can reach during this move - crossF = dda_steps_to_velocity(prestep+prev_total_steps); - // Make sure we do not exceed the target speeds - if(crossF > prev_F) crossF = prev_F; - if(crossF > currF) crossF = currF; - // The problem with the 'dda_steps_to_velocity' is that it will produce a - // rounded result. Use it to obtain an exact amount of steps needed to reach - // that speed and set that as the ramp up; we might stop accelerating for a - // couple of steps but that is better than introducing errors in the moves. - up = ACCELERATE_RAMP_LEN(crossF) - prestep; + // Compute the maximum speed we can reach for crossing. + crossF_in_steps = MIN(crossF_in_steps, this_total_steps); + crossF_in_steps = MIN(crossF_in_steps, prev_total_steps + prev_F_start_in_steps); - #ifdef LOOKAHEAD_DEBUG - // Sanity check: the ramp up should never exceed the move length - if(up > prev_total_steps) { - sersendf_P(PSTR("FATAL ERROR during prev ramp scale, ramp is too long: up:%lu ; len:%lu ; target speed: %lu\r\n"), - up, prev_total_steps, crossF); - sersendf_P(PSTR("F_start:%lu ; F:%lu ; crossF:%lu\r\n"), - prev_F_start, prev_F, crossF); - dda_emergency_shutdown(PSTR("LA prev ramp scale, ramp is too long")); - } - #endif - // Show the result on the speed on the clipping of the ramp - serprintf(PSTR("Prev speed & crossing speed truncated to: %lu\r\n"), crossF); - } else { - // Can reach crossF; determine the apex between ramp up and ramp down - // In other words: calculate how long we can accelerate before decelerating to exit at crossF - // Note: while the number of steps is exponentially proportional to the velocity, - // the acceleration is linear: we can simply remove the same number of steps of both ramps. - uint32_t diff = (up + down - prev_total_steps) / 2; - up -= diff; - down -= diff; - } + if (crossF_in_steps == 0) + return; - #ifdef LOOKAHEAD_DEBUG - // Sanity check: make sure the speed limits are maintained - if(prev_F_start > prev_F || crossF > prev_F) { - serprintf(PSTR("Prev target speed exceeded!: prev_F_start:%lu ; prev_F:%lu ; prev_F_end:%lu\r\n"), prev_F_start, prev_F, crossF); - dda_emergency_shutdown(PSTR("Prev target speed exceeded")); - } - #endif + // Build ramps for previous move. + if (crossF_in_steps == prev_F_in_steps) { + prev_rampup = prev_F_in_steps - prev_F_start_in_steps; + prev_rampdown = 0; } - // Save the results - prev_rampup = up; - prev_rampdown = prev_total_steps - down; - prev_F_end = crossF; - prev_end = ACCELERATE_RAMP_LEN(prev_F_end); + else if (crossF_in_steps < prev_F_start_in_steps) { + uint32_t extra, limit; - #ifdef LOOKAHEAD_DEBUG - // Sanity check: make sure the speed limits are maintained - if(crossF > currF) { - serprintf(PSTR("This target speed exceeded!: F_start:%lu ; F:%lu ; prev_F_end:%lu\r\n"), crossF, currF); - dda_emergency_shutdown(PSTR("This target speed exceeded")); + prev_rampup = 0; + prev_rampdown = prev_F_start_in_steps - crossF_in_steps; + extra = (prev_total_steps - prev_rampdown) >> 1; + limit = prev_F_in_steps - prev_F_start_in_steps; + extra = MIN(extra, limit); + + prev_rampup += extra; + prev_rampdown += extra; } - #endif + else { + uint32_t extra, limit; - // Forward check 2: test if we can actually reach the target speed in this move. - // If not: determine obtainable speed and adjust crossF accordingly. If that - // happens, a third (reverse) pass is needed to lower the speeds in the previous move... - //ramp_scaler = ACCELERATE_SCALER(current->lead); // Use scaler for current leading axis - up = ACCELERATE_RAMP_LEN(currF) - ACCELERATE_RAMP_LEN(crossF); - down = ACCELERATE_RAMP_LEN(currF); - // Test if both the ramp up and ramp down fit within the move - if(up+down > current->total_steps) { - // Test if we can reach the crossF rate - // Note: this is the inverse of the previous move: we need to exit at 0 speed as - // this is the last move in the queue. Implies that down >= up - if(down-up > current->total_steps) { - serprintf(PSTR("This move can not reach crossF - lower it\r\n")); - // Cannot reach crossF, lower it and adjust ramps - // Note: after this, the previous move needs to be modified to match crossF. - up = 0; - // Calculate what crossing rate we can reach: total/down * F - crossF = dda_steps_to_velocity(current->total_steps); - // Speed limit: never exceed the target rate - if(crossF > currF) crossF = currF; - // crossF will be conservative: calculate the actual ramp down length - down = ACCELERATE_RAMP_LEN(crossF); + prev_rampup = crossF_in_steps - prev_F_start_in_steps; + prev_rampdown = 0; + extra = (prev_total_steps - prev_rampup) >> 1; + limit = prev_F_in_steps - crossF_in_steps; + extra = MIN(extra, limit); - #ifdef LOOKAHEAD_DEBUG - // Make sure we can break to a full stop before the move ends - if(down > current->total_steps) { - sersendf_P(PSTR("FATAL ERROR during ramp scale, ramp is too long: down:%lu ; len:%lu ; target speed: %lu\r\n"), - down, current->total_steps, crossF); - dda_emergency_shutdown(PSTR("LA current ramp scale, ramp is too long")); - } - #endif - } else { - serprintf(PSTR("This: crossF is usable but we will not reach Fmax\r\n")); - // Can reach crossF; determine the apex between ramp up and ramp down - // In other words: calculate how long we can accelerate before decelerating to start at crossF - // and end at F = 0 - uint32_t diff = (down + up - current->total_steps) / 2; - up -= diff; - down -= diff; - serprintf(PSTR("Apex: %lu - new up: %lu - new down: %lu\r\n"), diff, up, down); - - // sanity stuff: calculate the speeds for these ramps - serprintf(PSTR("Ramp up speed: %lu mm/s\r\n"), dda_steps_to_velocity(up+prev->rampup_steps)); - serprintf(PSTR("Ramp down speed: %lu mm/s\r\n"), dda_steps_to_velocity(down)); - } + prev_rampup += extra; + prev_rampdown += extra; } - // Save the results - this_rampup = up; - this_rampdown = current->total_steps - down; - this_F_start = crossF; - this_start = ACCELERATE_RAMP_LEN(this_F_start); - serprintf(PSTR("Actual crossing speed: %lu\r\n"), crossF); + prev_rampdown = prev_total_steps - prev_rampdown; + prev_F_end_in_steps = crossF_in_steps; - // Potential reverse processing: - // Make sure the crossing speed is the same, if its not, we need to slow the previous move to - // the current crossing speed (note: the crossing speed could only be lowered). - // This can happen when this move is a short move and the previous move was a larger or faster move: - // since we need to be able to stop if this is the last move, we lowered the crossing speed - // between this move and the previous move... - if(prev_F_end != crossF) { - // Third reverse pass: slow the previous move to end at the target crossing speed. - //ramp_scaler = ACCELERATE_SCALER(current->lead); //todo: prev_lead // Use scaler for previous leading axis (again) - // Note: use signed values so we can check if results go below zero - // Note 2: when up2 and/or down2 are below zero from the start, you found a bug in the logic above. - int32_t up2 = ACCELERATE_RAMP_LEN(prev_F) - ACCELERATE_RAMP_LEN(prev_F_start); - int32_t down2 = ACCELERATE_RAMP_LEN(prev_F) - ACCELERATE_RAMP_LEN(crossF); - - // Test if both the ramp up and ramp down fit within the move - if(up2+down2 > prev_total_steps) { - int32_t diff = (up2 + down2 - (int32_t)prev_total_steps) / 2; - up2 -= diff; - down2 -= diff; - - #ifdef LOOKAHEAD_DEBUG - if(up2 < 0 || down2 < 0) { - // Cannot reach crossF from prev_F_start - this should not happen! - sersendf_P(PSTR("FATAL ERROR during reverse pass ramp scale, ramps are too long: up:%ld ; down:%ld; len:%lu ; F_start: %lu ; crossF: %lu\r\n"), - up2, down2, prev_total_steps, prev_F_start, crossF); - sersendf_P(PSTR("Original up: %ld - down %ld (diff=%ld)\r\n"),up2+diff,down2+diff,diff); - dda_emergency_shutdown(PSTR("reverse pass ramp scale, can not reach F_end from F_start")); - } - #endif - } - // Assign the results - prev_rampup = up2; - prev_rampdown = prev_total_steps - down2; - prev_F_end = crossF; - prev_end = ACCELERATE_RAMP_LEN(prev_F_end); + // Build ramps for current move. + if (crossF_in_steps == this_F_in_steps) { + this_rampup = 0; + this_rampdown = crossF_in_steps; } + else { + this_rampup = 0; + this_rampdown = crossF_in_steps; - #ifdef LOOKAHEAD_DEBUG - if(crossF > current->endpoint.F || crossF > prev_F) - dda_emergency_shutdown(PSTR("Lookahead exceeded speed limits in crossing!")); + uint32_t extra = (this_total_steps - this_rampdown) >> 1; + uint32_t limit = this_F_in_steps - crossF_in_steps; + extra = MIN(extra, limit); - // When debugging, print the 2 moves we joined - // Legenda: Fs=F_start, len=# of steps, up/down=# steps in ramping, Fe=F_end - serprintf(PSTR("LA: (%lu) Fs=%lu, len=%lu, up=%lu, down=%lu, Fe=%lu <=> (%lu) Fs=%lu, len=%lu, up=%lu, down=%lu, Fe=0\r\n\r\n"), - moveno-1, prev->F_start, prev->total_steps, prev->rampup_steps, - prev->total_steps-prev->rampdown_steps, prev->F_end, - moveno, current->F_start, current->total_steps, current->rampup_steps, - current->total_steps - this_rampdown); - #endif + this_rampup += extra; + this_rampdown += extra; + } + this_rampdown = this_total_steps - this_rampdown; + this_F_start_in_steps = crossF_in_steps; + + serprintf(PSTR("prev_F_start: %lu\r\n"), prev_F_start_in_steps); + serprintf(PSTR("prev_F: %lu\r\n"), prev_F_in_steps); + serprintf(PSTR("prev_rampup: %lu\r\n"), prev_rampup); + serprintf(PSTR("prev_rampdown: %lu\r\n"), prev_total_steps - prev_rampdown); + serprintf(PSTR("crossF: %lu\r\n"), crossF_in_steps); + serprintf(PSTR("this_rampup: %lu\r\n"), this_rampup); + serprintf(PSTR("this_rampdown: %lu\r\n"), this_total_steps - this_rampdown); + serprintf(PSTR("this_F: %lu\r\n"), this_F_in_steps); uint8_t timeout = 0; @@ -563,17 +467,14 @@ void dda_join_moves(DDA *prev, DDA *current) { // Determine if we are fast enough - if not, just leave the moves // Note: to test if the previous move was already executed and replaced by a new // move, we compare the DDA id. - if(prev->live == 0 && prev->id == prev_id) { - prev->F_end = prev_F_end; - prev->end_steps = prev_end; + if(prev->live == 0 && prev->id == prev_id && current->live == 0 && current->id == this_id) { + prev->end_steps = prev_F_end_in_steps; prev->rampup_steps = prev_rampup; prev->rampdown_steps = prev_rampdown; current->rampup_steps = this_rampup; current->rampdown_steps = this_rampdown; - current->F_end = 0; current->end_steps = 0; - current->F_start = this_F_start; - current->start_steps = this_start; + current->start_steps = this_F_start_in_steps; la_cnt++; } else timeout = 1;