diff --git a/Firmware/Configuration.h b/Firmware/Configuration.h index 2fcdca69b..3cec04c23 100644 --- a/Firmware/Configuration.h +++ b/Firmware/Configuration.h @@ -70,7 +70,7 @@ extern PGM_P sPrinterName; //#define STRING_VERSION "1.0.2" #define STRING_VERSION_CONFIG_H __DATE__ " " __TIME__ // build date and time -#define STRING_CONFIG_H_AUTHOR "(FW390+LA_acc_/belt fix)" // Who made the changes. +#define STRING_CONFIG_H_AUTHOR "(FW390+LA_acc_t/fix+belt fix)" // Who made the changes. // SERIAL_PORT selects which serial port should be used for communication with the host. // This allows the connection of wireless adapters (for instance) to non-default port pins. diff --git a/Firmware/planner.cpp b/Firmware/planner.cpp index 4d5dd6dbb..54bc692d8 100644 --- a/Firmware/planner.cpp +++ b/Firmware/planner.cpp @@ -225,15 +225,15 @@ void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit uint32_t accel_decel_steps = accelerate_steps + decelerate_steps; // Size of Plateau of Nominal Rate. uint32_t plateau_steps = 0; - - float max_speed; + // Maximum effective speed reached in the trapezoid (step/min) + float max_rate; // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will // have to use intersection_distance() to calculate when to abort acceleration and start braking // in order to reach the final_rate exactly at the end of this block. if (accel_decel_steps < block->step_event_count.wide) { plateau_steps = block->step_event_count.wide - accel_decel_steps; - max_speed = block->nominal_speed; + max_rate = block->nominal_rate; } else { uint32_t acceleration_x4 = acceleration << 2; // Avoid negative numbers @@ -267,17 +267,15 @@ void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit accelerate_steps = block->step_event_count.wide - decelerate_steps; } - // TODO: not for production - float dist = intersection_distance(entry_speed, exit_speed, block->acceleration, block->millimeters); - max_speed = sqrt(2 * block->acceleration * dist + entry_speed*entry_speed); + max_rate = sqrt(acceleration_x2 * accelerate_steps + initial_rate_sqr); } #ifdef LIN_ADVANCE uint16_t final_adv_steps = 0; uint16_t max_adv_steps = 0; if (block->use_advance_lead) { - final_adv_steps = exit_speed * block->adv_comp; - max_adv_steps = max_speed * block->adv_comp; + final_adv_steps = final_rate * block->adv_comp; + max_adv_steps = max_rate * block->adv_comp; } #endif @@ -1144,52 +1142,6 @@ Having the real displacement of the head, we can calculate the total movement le block->acceleration_rate = (long)((float)block->acceleration_st * (16777216.0 / (F_CPU / 8.0))); -#ifdef LIN_ADVANCE - if (block->use_advance_lead) { - // the nominal speed doesn't change past this point: calculate the compression ratio for the - // segment and the required advance steps - block->adv_comp = extruder_advance_K * e_D_ratio * cs.axis_steps_per_unit[E_AXIS]; - block->max_adv_steps = block->nominal_speed * block->adv_comp; - - float advance_speed; - if (e_D_ratio > 0) - advance_speed = (extruder_advance_K * e_D_ratio * block->acceleration * cs.axis_steps_per_unit[E_AXIS]); - else - advance_speed = cs.max_jerk[E_AXIS] * cs.axis_steps_per_unit[E_AXIS]; - - // to save more space we avoid another copy of calc_timer and go through slow division, but we - // still need to replicate the *exact* same step grouping policy (see below) - if (advance_speed > MAX_STEP_FREQUENCY) advance_speed = MAX_STEP_FREQUENCY; - float advance_rate = (F_CPU / 8.0) / advance_speed; - if (advance_speed > 20000) { - block->advance_rate = advance_rate * 4; - block->advance_step_loops = 4; - } - else if (advance_speed > 10000) { - block->advance_rate = advance_rate * 2; - block->advance_step_loops = 2; - } - else - { - // never overflow the internal accumulator with very low rates - if (advance_rate < UINT16_MAX) - block->advance_rate = advance_rate; - else - block->advance_rate = UINT16_MAX; - block->advance_step_loops = 1; - } - - #if 1 - if (block->advance_step_loops != 1) - // @wavexx: we should really check for the difference between step_loops and - // advance_step_loops instead. A difference of more than 1 will lead - // to uneven speed and *should* be adjusted here by furthermore - // reducing the speed. - SERIAL_ECHOLNPGM("LA: More than 1 steps per eISR loop executed."); - #endif - } -#endif - // Start with a safe speed. // Safe speed is the speed, from which the machine may halt to stop immediately. float safe_speed = block->nominal_speed; @@ -1315,6 +1267,53 @@ Having the real displacement of the head, we can calculate the total movement le // Precalculate the division, so when all the trapezoids in the planner queue get recalculated, the division is not repeated. block->speed_factor = block->nominal_rate / block->nominal_speed; + +#ifdef LIN_ADVANCE + if (block->use_advance_lead) { + // calculate the compression ratio for the segment (the required advance steps are computed + // during trapezoid planning) + float adv_comp = extruder_advance_K * e_D_ratio * cs.axis_steps_per_unit[E_AXIS]; // (step/(mm/s)) + block->adv_comp = adv_comp / block->speed_factor; // step/(step/min) + + float advance_speed; + if (e_D_ratio > 0) + advance_speed = (extruder_advance_K * e_D_ratio * block->acceleration * cs.axis_steps_per_unit[E_AXIS]); + else + advance_speed = cs.max_jerk[E_AXIS] * cs.axis_steps_per_unit[E_AXIS]; + + // to save more space we avoid another copy of calc_timer and go through slow division, but we + // still need to replicate the *exact* same step grouping policy (see below) + if (advance_speed > MAX_STEP_FREQUENCY) advance_speed = MAX_STEP_FREQUENCY; + float advance_rate = (F_CPU / 8.0) / advance_speed; + if (advance_speed > 20000) { + block->advance_rate = advance_rate * 4; + block->advance_step_loops = 4; + } + else if (advance_speed > 10000) { + block->advance_rate = advance_rate * 2; + block->advance_step_loops = 2; + } + else + { + // never overflow the internal accumulator with very low rates + if (advance_rate < UINT16_MAX) + block->advance_rate = advance_rate; + else + block->advance_rate = UINT16_MAX; + block->advance_step_loops = 1; + } + + #ifdef LA_DEBUG + if (block->advance_step_loops > 2) + // @wavexx: we should really check for the difference between step_loops and + // advance_step_loops instead. A difference of more than 1 will lead + // to uneven speed and *should* be adjusted here by furthermore + // reducing the speed. + SERIAL_ECHOLNPGM("LA: More than 2 steps per eISR loop executed."); + #endif + } +#endif + calculate_trapezoid_for_block(block, block->entry_speed, safe_speed); if (block->step_event_count.wide <= 32767) diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp index de250ec97..b1c3d2861 100644 --- a/Firmware/stepper.cpp +++ b/Firmware/stepper.cpp @@ -848,12 +848,10 @@ FORCE_INLINE void isr() { #ifdef LIN_ADVANCE if(current_block->use_advance_lead) { - if (!nextAdvanceISR) { - // Due to E-jerk, there can be discontinuities in pressure state where an - // acceleration or deceleration can be skipped or joined with the previous block. - // If LA was not previously active, re-check the pressure level - la_state = ADV_INIT; - } + // Due to E-jerk, there can be discontinuities in pressure state where an + // acceleration or deceleration can be skipped or joined with the previous block. + // If LA was not previously active, re-check the pressure level + la_state = ADV_INIT; } #endif } @@ -865,6 +863,7 @@ FORCE_INLINE void isr() { #ifdef LIN_ADVANCE // avoid multiple instances or function calls to advance_spread if (la_state & ADV_INIT) { + LA_phase = -1; if (current_adv_steps == target_adv_steps) { // nothing to be done in this phase la_state = 0; @@ -1017,7 +1016,7 @@ FORCE_INLINE void advance_isr_scheduler() { // Schedule the next closest tick, ignoring advance if scheduled too // soon in order to avoid skewing the regular stepper acceleration - if (nextAdvanceISR != ADV_NEVER && (nextAdvanceISR + TCNT1 + 40) < nextMainISR) + if (nextAdvanceISR != ADV_NEVER && (nextAdvanceISR + 40) < nextMainISR) OCR1A = nextAdvanceISR; else OCR1A = nextMainISR;