From 986b2868032a3cfcc94b7e07143d58e6b1eb58d8 Mon Sep 17 00:00:00 2001 From: bubnikv Date: Fri, 22 Jul 2016 10:37:06 +0200 Subject: [PATCH] Reworked calculation of the trapezoidal ramps inside the planner. The old implementation seems to be buggy, it accesses segments outside the queue, causing jerks and skipped steps. --- Firmware/Configuration_adv.h | 10 - Firmware/Marlin_main.cpp | 2 + Firmware/planner.cpp | 494 +++++++++++++++++------------------ Firmware/planner.h | 52 +++- Firmware/stepper.cpp | 3 +- 5 files changed, 286 insertions(+), 275 deletions(-) diff --git a/Firmware/Configuration_adv.h b/Firmware/Configuration_adv.h index 08b4d543..54e74ba0 100644 --- a/Firmware/Configuration_adv.h +++ b/Firmware/Configuration_adv.h @@ -190,16 +190,6 @@ // If defined the movements slow down when the look ahead buffer is only half full #define SLOWDOWN -// Frequency limit -// See nophead's blog for more info -// Not working O -//#define XY_FREQUENCY_LIMIT 15 - -// Minimum planner junction speed. Sets the default minimum speed the planner plans for at the end -// of the buffer and all stops. This should not be much greater than zero and should only be changed -// if unwanted behavior is observed on a user's machine when running at very slow speeds. -#define MINIMUM_PLANNER_SPEED 0.05// (mm/sec) - // MS1 MS2 Stepper Driver Microstepping mode table #define MICROSTEP1 LOW,LOW #define MICROSTEP2 HIGH,LOW diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp index 586d7218..90e7ebda 100644 --- a/Firmware/Marlin_main.cpp +++ b/Firmware/Marlin_main.cpp @@ -2861,6 +2861,7 @@ void process_commands() break; } + /* case 46: { // M46: Prusa3D: Show the assigned IP address. @@ -2881,6 +2882,7 @@ void process_commands() } break; } + */ case 47: // M47: Prusa3D: Show end stops dialog on the display. diff --git a/Firmware/planner.cpp b/Firmware/planner.cpp index 83565177..39c4aca7 100644 --- a/Firmware/planner.cpp +++ b/Firmware/planner.cpp @@ -116,13 +116,6 @@ volatile unsigned char block_buffer_tail; // Index of the block to pro #ifdef PREVENT_DANGEROUS_EXTRUDE float extrude_min_temp=EXTRUDE_MINTEMP; #endif -#ifdef XY_FREQUENCY_LIMIT -#define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT) -// Used for the frequency limit -static unsigned char old_direction_bits = 0; // Old direction bits. Used for speed calculations -static long x_segment_time[3]={MAX_FREQ_TIME + 1,0,0}; // Segment times (in us). Used for speed calculations -static long y_segment_time[3]={MAX_FREQ_TIME + 1,0,0}; -#endif #ifdef FILAMENT_SENSOR static char meas_sample; //temporary variable to hold filament measurement sample @@ -130,22 +123,19 @@ static long y_segment_time[3]={MAX_FREQ_TIME + 1,0,0}; // Returns the index of the next block in the ring buffer // NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. -static int8_t next_block_index(int8_t block_index) { - block_index++; - if (block_index == BLOCK_BUFFER_SIZE) { +static inline int8_t next_block_index(int8_t block_index) { + if (++ block_index == BLOCK_BUFFER_SIZE) block_index = 0; - } - return(block_index); + return block_index; } // Returns the index of the previous block in the ring buffer -static int8_t prev_block_index(int8_t block_index) { - if (block_index == 0) { +static inline int8_t prev_block_index(int8_t block_index) { + if (block_index == 0) block_index = BLOCK_BUFFER_SIZE; - } - block_index--; - return(block_index); + -- block_index; + return block_index; } //=========================================================================== @@ -222,7 +212,7 @@ void calculate_trapezoid_for_block(block_t *block, float entry_factor, float exi // block->accelerate_until = accelerate_steps; // block->decelerate_after = accelerate_steps+plateau_steps; CRITICAL_SECTION_START; // Fill variables used by the stepper in a critical section - if(block->busy == false) { // Don't update variables if block is busy. + if (! block->busy) { // Don't update variables if block is busy. block->accelerate_until = accelerate_steps; block->decelerate_after = accelerate_steps+plateau_steps; block->initial_rate = initial_rate; @@ -235,141 +225,12 @@ void calculate_trapezoid_for_block(block_t *block, float entry_factor, float exi CRITICAL_SECTION_END; } -// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the -// acceleration within the allotted distance. -FORCE_INLINE float max_allowable_speed(float acceleration, float target_velocity, float distance) { - return sqrt(target_velocity*target_velocity-2*acceleration*distance); -} - -// "Junction jerk" in this context is the immediate change in speed at the junction of two blocks. -// This method will calculate the junction jerk as the euclidean distance between the nominal -// velocities of the respective blocks. -//inline float junction_jerk(block_t *before, block_t *after) { -// return sqrt( -// pow((before->speed_x-after->speed_x), 2)+pow((before->speed_y-after->speed_y), 2)); -//} - - -// The kernel called by planner_recalculate() when scanning the plan from last to first entry. -void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) { - if(!current) { - return; - } - - if (next) { - // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising. - // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and - // check for maximum allowable speed reductions to ensure maximum possible planned speed. - if (current->entry_speed != current->max_entry_speed) { - - // If nominal length true, max junction speed is guaranteed to be reached. Only compute - // for max allowable speed if block is decelerating and nominal length is false. - if ((!current->nominal_length_flag) && (current->max_entry_speed > next->entry_speed)) { - current->entry_speed = min( current->max_entry_speed, - max_allowable_speed(-current->acceleration,next->entry_speed,current->millimeters)); - } - else { - current->entry_speed = current->max_entry_speed; - } - current->recalculate_flag = true; - - } - } // Skip last block. Already initialized and set for recalculation. -} - -// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This -// implements the reverse pass. -void planner_reverse_pass() { - uint8_t block_index = block_buffer_head; - - //Make a local copy of block_buffer_tail, because the interrupt can alter it - CRITICAL_SECTION_START; - unsigned char tail = block_buffer_tail; - CRITICAL_SECTION_END - - if(((block_buffer_head-tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1)) > 3) { - block_index = (block_buffer_head - 3) & (BLOCK_BUFFER_SIZE - 1); - block_t *block[3] = { - NULL, NULL, NULL }; - while(block_index != tail) { - block_index = prev_block_index(block_index); - block[2]= block[1]; - block[1]= block[0]; - block[0] = &block_buffer[block_index]; - planner_reverse_pass_kernel(block[0], block[1], block[2]); - } - } -} - -// The kernel called by planner_recalculate() when scanning the plan from first to last entry. -void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) { - if(!previous) { - return; - } - - // If the previous block is an acceleration block, but it is not long enough to complete the - // full speed change within the block, we need to adjust the entry speed accordingly. Entry - // speeds have already been reset, maximized, and reverse planned by reverse planner. - // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck. - if (!previous->nominal_length_flag) { - if (previous->entry_speed < current->entry_speed) { - double entry_speed = min( current->entry_speed, - max_allowable_speed(-previous->acceleration,previous->entry_speed,previous->millimeters) ); - - // Check for junction speed change - if (current->entry_speed != entry_speed) { - current->entry_speed = entry_speed; - current->recalculate_flag = true; - } - } - } -} - -// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This -// implements the forward pass. -void planner_forward_pass() { - uint8_t block_index = block_buffer_tail; - block_t *block[3] = { - NULL, NULL, NULL }; - - while(block_index != block_buffer_head) { - block[0] = block[1]; - block[1] = block[2]; - block[2] = &block_buffer[block_index]; - planner_forward_pass_kernel(block[0],block[1],block[2]); - block_index = next_block_index(block_index); - } - planner_forward_pass_kernel(block[1], block[2], NULL); -} - -// Recalculates the trapezoid speed profiles for all blocks in the plan according to the -// entry_factor for each junction. Must be called by planner_recalculate() after -// updating the blocks. -void planner_recalculate_trapezoids() { - int8_t block_index = block_buffer_tail; - block_t *current; - block_t *next = NULL; - - while(block_index != block_buffer_head) { - current = next; - next = &block_buffer[block_index]; - if (current) { - // Recalculate if current block entry or exit junction speed has changed. - if (current->recalculate_flag || next->recalculate_flag) { - // NOTE: Entry and exit factors always > 0 by all previous logic operations. - calculate_trapezoid_for_block(current, current->entry_speed/current->nominal_speed, - next->entry_speed/current->nominal_speed); - current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed - } - } - block_index = next_block_index( block_index ); - } - // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. - if(next != NULL) { - calculate_trapezoid_for_block(next, next->entry_speed/next->nominal_speed, - MINIMUM_PLANNER_SPEED/next->nominal_speed); - next->recalculate_flag = false; - } +// Calculates the maximum allowable entry speed, when you must be able to reach target_velocity using the +// decceleration within the allotted distance. +FORCE_INLINE float max_allowable_entry_speed(float decceleration, float target_velocity, float distance) +{ + // assert(decceleration < 0); + return sqrt(target_velocity*target_velocity-2*decceleration*distance); } // Recalculates the motion plan according to the following algorithm: @@ -388,11 +249,99 @@ void planner_recalculate_trapezoids() { // the set limit. Finally it will: // // 3. Recalculate trapezoids for all blocks. +void planner_recalculate(const float &safe_final_speed) +{ + // Reverse pass + // Make a local copy of block_buffer_tail, because the interrupt can alter it + // by consuming the blocks, therefore shortening the queue. + unsigned char tail = block_buffer_tail; + uint8_t block_index; + block_t *prev, *current, *next; -void planner_recalculate() { - planner_reverse_pass(); - planner_forward_pass(); - planner_recalculate_trapezoids(); +// SERIAL_ECHOLNPGM("planner_recalculate - 1"); + + // At least three blocks are in the queue? + unsigned char n_blocks = (block_buffer_head + BLOCK_BUFFER_SIZE - tail) & (BLOCK_BUFFER_SIZE - 1); + if (n_blocks >= 3) { + // Initialize the last tripple of blocks. + block_index = prev_block_index(block_buffer_head); + next = block_buffer + block_index; + current = block_buffer + (block_index = prev_block_index(block_index)); + // No need to recalculate the last block, it has already been set by the plan_buffer_line() function. + // Vojtech thinks, that one shall not touch the entry speed of the very first block as well, because + // 1) it may already be running at the stepper interrupt, + // 2) there is no way to limit it when going in the forward direction. + while (block_index != tail) { + if (current->flag & BLOCK_FLAG_START_FROM_FULL_HALT) { + // Don't modify the entry velocity of the starting block. + // Also don't modify the trapezoids before this block, they are finalized already, prepared + // for the stepper interrupt routine to use them. + tail = block_index; + // Update the number of blocks to process. + n_blocks = (block_buffer_head + BLOCK_BUFFER_SIZE - tail) & (BLOCK_BUFFER_SIZE - 1); + SERIAL_ECHOLNPGM("BLOCK_FLAG_START_FROM_FULL_HALT"); + break; + } + // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising. + // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and + // check for maximum allowable speed reductions to ensure maximum possible planned speed. + if (current->entry_speed != current->max_entry_speed) { + // assert(current->entry_speed < current->max_entry_speed); + // Entry speed could be increased up to the max_entry_speed, limited by the length of the current + // segment and the maximum acceleration allowed for this segment. + // If nominal length true, max junction speed is guaranteed to be reached even if decelerating to a jerk-from-zero velocity. + // Only compute for max allowable speed if block is decelerating and nominal length is false. + current->entry_speed = ((current->flag & BLOCK_FLAG_NOMINAL_LENGTH) || current->max_entry_speed <= next->entry_speed) ? + current->max_entry_speed : + min(current->max_entry_speed, max_allowable_entry_speed(-current->acceleration,next->entry_speed,current->millimeters)); + current->flag |= BLOCK_FLAG_RECALCULATE; + } + next = current; + current = block_buffer + (block_index = prev_block_index(block_index)); + } + } + +// SERIAL_ECHOLNPGM("planner_recalculate - 2"); + + // Forward pass and recalculate the trapezoids. + if (n_blocks >= 2) { + // Better to limit the velocities using the already processed block, if it is available, so rather use the saved tail. + block_index = tail; + prev = block_buffer + block_index; + current = block_buffer + (block_index = next_block_index(block_index)); + do { + // If the previous block is an acceleration block, but it is not long enough to complete the + // full speed change within the block, we need to adjust the entry speed accordingly. Entry + // speeds have already been reset, maximized, and reverse planned by reverse planner. + // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck. + if (! (prev->flag & BLOCK_FLAG_NOMINAL_LENGTH) && prev->entry_speed < current->entry_speed) { + float entry_speed = min(current->entry_speed, max_allowable_entry_speed(-prev->acceleration,prev->entry_speed,prev->millimeters)); + // Check for junction speed change + if (current->entry_speed != entry_speed) { + current->entry_speed = entry_speed; + current->flag |= BLOCK_FLAG_RECALCULATE; + } + } + // Recalculate if current block entry or exit junction speed has changed. + if ((prev->flag | current->flag) & BLOCK_FLAG_RECALCULATE) { + // NOTE: Entry and exit factors always > 0 by all previous logic operations. + calculate_trapezoid_for_block(prev, prev->entry_speed/prev->nominal_speed, current->entry_speed/prev->nominal_speed); + // Reset current only to ensure next trapezoid is computed. + prev->flag &= ~BLOCK_FLAG_RECALCULATE; + } + prev = current; + current = block_buffer + (block_index = next_block_index(block_index)); + } while (block_index != block_buffer_head); + } + +// SERIAL_ECHOLNPGM("planner_recalculate - 3"); + + // Last/newest block in buffer. Exit speed is set with safe_final_speed. Always recalculated. + current = block_buffer + prev_block_index(block_buffer_head); + calculate_trapezoid_for_block(current, current->entry_speed/current->nominal_speed, safe_final_speed/current->nominal_speed); + current->flag &= ~BLOCK_FLAG_RECALCULATE; + +// SERIAL_ECHOLNPGM("planner_recalculate - 4"); } void plan_init() { @@ -618,7 +567,7 @@ void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate // Prepare to set up new block block_t *block = &block_buffer[block_buffer_head]; - // Mark block as not busy (Not executed by the stepper interrupt) + // Mark block as not busy (Not executed by the stepper interrupt, could be still tinkered with.) block->busy = false; // Number of steps for each axis @@ -783,30 +732,20 @@ Having the real displacement of the head, we can calculate the total movement le // Calculate speed in mm/second for each axis. No divide by zero due to previous checks. float inverse_second = feed_rate * inverse_millimeters; - int moves_queued=(block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1); + int moves_queued = moves_planned(); // slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill -#ifdef OLD_SLOWDOWN - if(moves_queued < (BLOCK_BUFFER_SIZE * 0.5) && moves_queued > 1) - feed_rate = feed_rate*moves_queued / (BLOCK_BUFFER_SIZE * 0.5); -#endif - #ifdef SLOWDOWN - // segment time im micro seconds - unsigned long segment_time = lround(1000000.0/inverse_second); - if ((moves_queued > 1) && (moves_queued < (BLOCK_BUFFER_SIZE * 0.5))) - { - if (segment_time < minsegmenttime) - { // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more. - inverse_second=1000000.0/(segment_time+lround(2*(minsegmenttime-segment_time)/moves_queued)); - #ifdef XY_FREQUENCY_LIMIT - segment_time = lround(1000000.0/inverse_second); - #endif - } + //FIXME Vojtech: Why moves_queued > 1? Why not >=1? + // Can we somehow differentiate the filling of the buffer at the start of a g-code from a buffer draining situation? + if (moves_queued > 1 && moves_queued < (BLOCK_BUFFER_SIZE >> 1)) { + // segment time in micro seconds + unsigned long segment_time = lround(1000000.0/inverse_second); + if (segment_time < minsegmenttime) + // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more. + inverse_second=1000000.0/(segment_time+lround(2*(minsegmenttime-segment_time)/moves_queued)); } -#endif - // END OF SLOW DOWN SECTION - +#endif // SLOWDOWN block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0 block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0 @@ -864,41 +803,6 @@ Having the real displacement of the head, we can calculate the total movement le speed_factor = min(speed_factor, max_feedrate[i] / fabs(current_speed[i])); } - // Max segement time in us. -#ifdef XY_FREQUENCY_LIMIT -#define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT) - // Check and limit the xy direction change frequency - unsigned char direction_change = block->direction_bits ^ old_direction_bits; - old_direction_bits = block->direction_bits; - segment_time = lround((float)segment_time / speed_factor); - - if((direction_change & (1<acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 // Limit acceleration per axis + //FIXME Vojtech: One shall rather limit a projection of the acceleration vector instead of using the limit. if(((float)block->acceleration_st * (float)block->steps_x / (float)block->step_event_count) > axis_steps_per_sqr_second[X_AXIS]) block->acceleration_st = axis_steps_per_sqr_second[X_AXIS]; if(((float)block->acceleration_st * (float)block->steps_y / (float)block->step_event_count) > axis_steps_per_sqr_second[Y_AXIS]) @@ -972,36 +877,126 @@ Having the real displacement of the head, we can calculate the total movement le } } #endif - // Start with a safe speed - float vmax_junction = max_xy_jerk/2; - float vmax_junction_factor = 1.0; - if(fabs(current_speed[Z_AXIS]) > max_z_jerk/2) - vmax_junction = min(vmax_junction, max_z_jerk/2); - if(fabs(current_speed[E_AXIS]) > max_e_jerk/2) - vmax_junction = min(vmax_junction, max_e_jerk/2); + // Start with a safe speed. + //Vojtech: This code tries to limit the initial jerk to half of the maximum jerk value. + //The code is not quite correct. It is pessimistic as it shall limit a projection of the jerk into each axis, + //but when the current code clamps, it clamps as if the movement is done in a single axis only. + float vmax_junction = max_xy_jerk/2.f; + if(fabs(current_speed[Z_AXIS]) > max_z_jerk/2.f) + vmax_junction = min(vmax_junction, max_z_jerk/2.f); + if(fabs(current_speed[E_AXIS]) > max_e_jerk/2.f) + vmax_junction = min(vmax_junction, max_e_jerk/2.f); vmax_junction = min(vmax_junction, block->nominal_speed); + // Safe speed is the speed, from which the machine may halt to stop immediately. float safe_speed = vmax_junction; - if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) { - float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2)); - // if((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) { - vmax_junction = block->nominal_speed; - // } - if (jerk > max_xy_jerk) { - vmax_junction_factor = (max_xy_jerk/jerk); - } - if(fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk) { - vmax_junction_factor= min(vmax_junction_factor, (max_z_jerk/fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]))); - } - if(fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]) > max_e_jerk) { - vmax_junction_factor = min(vmax_junction_factor, (max_e_jerk/fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]))); - } - vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed + //FIXME Vojtech: Why only if at least two lines are planned in the queue? + // Is it because we don't want to tinker with the first buffer line, which + // is likely to be executed by the stepper interrupt routine soon? + if (moves_queued > 1 && previous_nominal_speed > 0.0001f) { +#if 1 + float jerk; + { + float dx = current_speed[X_AXIS]-previous_speed[X_AXIS]; + float dy = current_speed[Y_AXIS]-previous_speed[Y_AXIS]; + jerk = sqrt(dx*dx+dy*dy); + } + float vmax_junction_factor = 1.0; + // if((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) { + vmax_junction = block->nominal_speed; + // } + if (jerk > max_xy_jerk) + vmax_junction_factor = max_xy_jerk/jerk; + jerk = fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]); + if (jerk > max_z_jerk) + vmax_junction_factor = min(vmax_junction_factor, max_z_jerk/jerk); + jerk = fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]); + if (jerk > max_e_jerk) + vmax_junction_factor = min(vmax_junction_factor, max_e_jerk/jerk); + //FIXME Vojtech: Why is this asymmetric in regard to the previous nominal speed and the current nominal speed? + vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed +#else + // Estimate a maximum velocity allowed at a joint of two successive segments. + // If this maximum velocity allowed is lower than the minimum of the entry / exit safe velocities, + // then the machine is not coasting anymore and the safe entry / exit velocities shall be used. + + // The junction velocity will be shared between successive segments. Limit the junction velocity to their minimum. + bool prev_speed_larger = previous_nominal_speed > block->nominal_speed; + float smaller_speed_factor = prev_speed_larger ? (block->nominal_speed / previous_nominal_speed) : (previous_nominal_speed / block->nominal_speed); + // Pick the smaller of the nominal speeds. Higher speed shall not be achieved at the junction during coasting. + vmax_junction = prev_speed_larger ? block->nominal_speed : previous_nominal_speed; + // Factor to multiply the previous / current nominal velocities to get componentwise limited velocities. + float v_factor_exit = prev_speed_larger ? smaller_speed_factor : 1.f; + float v_factor_entry = prev_speed_larger ? 1.f : smaller_speed_factor; + // First limit the jerk in the XY plane. + float jerk; + { + // Estimate the jerk as if the entry / exit velocity of the two successive segment was limited to the minimum of their nominal velocities. + // If coasting, then the segment transition velocity will define the exit / entry velocities of the successive segments + // and the jerk defined by the following formula will be always lower. + float dx = prev_speed_larger ? (current_speed[X_AXIS] - smaller_speed_factor * previous_speed[X_AXIS]) : (smaller_speed_factor * current_speed[X_AXIS] - previous_speed[X_AXIS]); + float dy = prev_speed_larger ? (current_speed[Y_AXIS] - smaller_speed_factor * previous_speed[Y_AXIS]) : (smaller_speed_factor * current_speed[Y_AXIS] - previous_speed[Y_AXIS]); + jerk = sqrt(dx*dx+dy*dy); + } + if (jerk > max_xy_jerk) { + // Limit the entry / exit velocities to respect the XY jerk limits. + v_factor_exit = v_factor_entry = max_xy_jerk / jerk; + if (prev_speed_larger) + v_factor_exit *= smaller_speed_factor; + else + v_factor_entry *= smaller_speed_factor; + } + // Now limit the Z and E axes. We have to differentiate coasting from the reversal of an axis movement, or a full stop. + float v_exit = previous_speed[Z_AXIS] * v_factor_exit; + float v_entry = current_speed [Z_AXIS] * v_factor_entry; + jerk = (v_exit > v_entry) ? + ((v_entry > 0.f || v_exit < 0.f) ? + // coasting + (v_exit - v_entry) : + // axis reversal + max(v_exit, - v_entry)) : + // v_exit <= v_entry + ((v_entry < 0.f || v_exit > 0.f) ? + // coasting + (v_entry - v_exit) : + // axis reversal + max(- v_exit, v_entry)); + if (jerk > max_z_jerk / 2.f) { + float c = (max_z_jerk / 2.f) / jerk; + v_factor_exit *= c; + v_factor_entry *= c; + } + // Limit the E axis. + v_exit = previous_speed[E_AXIS] * v_factor_exit; + v_entry = current_speed [E_AXIS] * v_factor_entry; + jerk = (v_exit > v_entry) ? + ((v_entry > 0.f || v_exit < 0.f) ? + // coasting + (v_exit - v_entry) : + // axis reversal + max(v_exit, - v_entry)) : + // v_exit <= v_entry + ((v_entry < 0.f || v_exit > 0.f) ? + // coasting + (v_entry - v_exit) : + // axis reversal + max(- v_exit, v_entry)); + if (jerk > max_e_jerk / 2.f) { + float c = (max_e_jerk / 2.f) / jerk; + v_factor_exit *= c; + v_factor_entry *= c; + } + + // Now the transition velocity is known as nominal * v_factor. Compare the transition velocity against the "safe" velocoties. + // If the transition velocity is below the exit / enter safe velocity, the machine is no more cruising, therefore + // the safe velocities shall be used. +#endif } + // Max entry speed of this block equals the max exit speed of the previous block. block->max_entry_speed = vmax_junction; - // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED. - double v_allowable = max_allowable_speed(-block->acceleration,MINIMUM_PLANNER_SPEED,block->millimeters); + // Initialize block entry speed. Compute based on deceleration to safe_speed. + double v_allowable = max_allowable_entry_speed(-block->acceleration,safe_speed,block->millimeters); block->entry_speed = min(vmax_junction, v_allowable); // Initialize planner efficiency flags @@ -1012,13 +1007,8 @@ Having the real displacement of the head, we can calculate the total movement le // block nominal speed limits both the current and next maximum junction speeds. Hence, in both // the reverse and forward planners, the corresponding block junction speed will always be at the // the maximum junction speed and may always be ignored for any speed reduction checks. - if (block->nominal_speed <= v_allowable) { - block->nominal_length_flag = true; - } - else { - block->nominal_length_flag = false; - } - block->recalculate_flag = true; // Always calculate trapezoid for new block + // Always calculate trapezoid for new block + block->flag = (block->nominal_speed <= v_allowable) ? (BLOCK_FLAG_NOMINAL_LENGTH | BLOCK_FLAG_RECALCULATE) : BLOCK_FLAG_RECALCULATE; // Update previous path unit_vector and nominal speed memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[] @@ -1052,16 +1042,19 @@ Having the real displacement of the head, we can calculate the total movement le */ #endif // ADVANCE - calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed, - safe_speed/block->nominal_speed); + calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed, safe_speed/block->nominal_speed); - // Move buffer head + // Move the buffer head. From now the block may be picked up by the stepper interrupt controller. block_buffer_head = next_buffer_head; // Update position memcpy(position, target, sizeof(target)); // position[] = target[] - planner_recalculate(); + // Recalculate the trapezoids to maximize speed at the segment transitions while respecting + // the machine limits (maximum acceleration and maximum jerk). + // This runs asynchronously with the stepper interrupt controller, which may + // interfere with the process. + planner_recalculate(safe_speed); st_wake_up(); } @@ -1128,11 +1121,6 @@ void plan_set_e_position(const float &e) st_set_e_position(position[E_AXIS]); } -uint8_t movesplanned() -{ - return (block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1); -} - #ifdef PREVENT_DANGEROUS_EXTRUDE void set_extrude_min_temp(float temp) { diff --git a/Firmware/planner.h b/Firmware/planner.h index 6cf07551..28d654aa 100644 --- a/Firmware/planner.h +++ b/Firmware/planner.h @@ -30,17 +30,34 @@ #include "vector_3.h" #endif // ENABLE_AUTO_BED_LEVELING +enum BlockFlag { + // Planner flag to recalculate trapezoids on entry junction. + // This flag has an optimization purpose only. + BLOCK_FLAG_RECALCULATE = 1, + // Planner flag for nominal speed always reached. That means, the segment is long enough, that the nominal speed + // may be reached if accelerating from a safe speed (in the regard of jerking from zero speed). + BLOCK_FLAG_NOMINAL_LENGTH = 2, + // If set, the machine will stop to a full halt at the end of this block, + // respecting the maximum allowed jerk. + BLOCK_FLAG_FULL_HALT_AT_END = 4, + // If set, the machine will start from a halt at the start of this block, + // respecting the maximum allowed jerk. + BLOCK_FLAG_START_FROM_FULL_HALT = 8, +}; + // This struct is used when buffering the setup for each linear movement "nominal" values are as specified in // the source g-code and may never actually be reached if acceleration management is active. typedef struct { // Fields used by the bresenham algorithm for tracing the line + // steps_x.y,z, step_event_count, acceleration_rate, direction_bits and active_extruder are set by plan_buffer_line(). long steps_x, steps_y, steps_z, steps_e; // Step count along each axis unsigned long step_event_count; // The number of step events required to complete this block - long accelerate_until; // The index of the step event on which to stop acceleration - long decelerate_after; // The index of the step event on which to start decelerating long acceleration_rate; // The acceleration rate used for acceleration calculation unsigned char direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) unsigned char active_extruder; // Selects the active extruder + // accelerate_until and decelerate_after are set by calculate_trapezoid_for_block() and they need to be synchronized with the stepper interrupt controller. + long accelerate_until; // The index of the step event on which to stop acceleration + long decelerate_after; // The index of the step event on which to start decelerating #ifdef ADVANCE long advance_rate; volatile long initial_advance; @@ -50,15 +67,24 @@ typedef struct { // Fields used by the motion planner to manage acceleration // float speed_x, speed_y, speed_z, speed_e; // Nominal mm/sec for each axis - float nominal_speed; // The nominal speed for this block in mm/sec - float entry_speed; // Entry speed at previous-current junction in mm/sec - float max_entry_speed; // Maximum allowable junction entry speed in mm/sec - float millimeters; // The total travel of this block in mm - float acceleration; // acceleration mm/sec^2 - unsigned char recalculate_flag; // Planner flag to recalculate trapezoids on entry junction - unsigned char nominal_length_flag; // Planner flag for nominal speed always reached + // The nominal speed for this block in mm/sec. + // This speed may or may not be reached due to the jerk and acceleration limits. + float nominal_speed; + // Entry speed at previous-current junction in mm/sec, respecting the acceleration and jerk limits. + // The entry speed limit of the current block equals the exit speed of the preceding block. + float entry_speed; + // Maximum allowable junction entry speed in mm/sec. This value is also a maximum exit speed of the previous block. + float max_entry_speed; + // The total travel of this block in mm + float millimeters; + // acceleration mm/sec^2 + float acceleration; - // Settings for the trapezoid generator + // Bit flags defined by the BlockFlag enum. + bool flag; + + // Settings for the trapezoid generator (runs inside an interrupt handler). + // Changing the following values in the planner needs to be synchronized with the interrupt handler by disabling the interrupts. unsigned long nominal_rate; // The nominal step rate for this block in step_events/sec unsigned long initial_rate; // The jerk-adjusted step rate at start of block unsigned long final_rate; // The minimal rate at exit @@ -101,7 +127,6 @@ void plan_set_e_position(const float &e); void check_axes_activity(); -uint8_t movesplanned(); //return the nr of buffered moves extern unsigned long minsegmenttime; extern float max_feedrate[NUM_AXIS]; // set the max speeds @@ -152,6 +177,11 @@ FORCE_INLINE block_t *plan_get_current_block() // Returns true if the buffer has a queued block, false otherwise FORCE_INLINE bool blocks_queued() { return (block_buffer_head != block_buffer_tail); } +//return the nr of buffered moves +FORCE_INLINE uint8_t moves_planned() { + return (block_buffer_head + BLOCK_BUFFER_SIZE - block_buffer_tail) & (BLOCK_BUFFER_SIZE - 1); +} + #ifdef PREVENT_DANGEROUS_EXTRUDE void set_extrude_min_temp(float temp); #endif diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp index eef767e1..cce0b313 100644 --- a/Firmware/stepper.cpp +++ b/Firmware/stepper.cpp @@ -343,7 +343,8 @@ ISR(TIMER1_COMPA_vect) // Anything in the buffer? current_block = plan_get_current_block(); if (current_block != NULL) { - current_block->busy = true; + // The busy flag is set by the plan_get_current_block() call. + // current_block->busy = true; trapezoid_generator_reset(); counter_x = -(current_block->step_event_count >> 1); counter_y = counter_x;