diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index bb1e84be1..93bd8225a 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -1,56 +1,56 @@ /* planner.c - buffers movement commands and manages the acceleration profile plan - Part of Grbl - - Copyright (c) 2009-2011 Simen Svale Skogsrud - - Grbl is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - Grbl is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with Grbl. If not, see . -*/ + Part of Grbl + + Copyright (c) 2009-2011 Simen Svale Skogsrud + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . + */ /* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */ /* - Reasoning behind the mathematics in this module (in the key of 'Mathematica'): - - s == speed, a == acceleration, t == time, d == distance + Reasoning behind the mathematics in this module (in the key of 'Mathematica'): + + s == speed, a == acceleration, t == time, d == distance + + Basic definitions: + + Speed[s_, a_, t_] := s + (a*t) + Travel[s_, a_, t_] := Integrate[Speed[s, a, t], t] + + Distance to reach a specific speed with a constant acceleration: + + Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, d, t] + d -> (m^2 - s^2)/(2 a) --> estimate_acceleration_distance() + + Speed after a given distance of travel with constant acceleration: + + Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, m, t] + m -> Sqrt[2 a d + s^2] + + DestinationSpeed[s_, a_, d_] := Sqrt[2 a d + s^2] + + When to start braking (di) to reach a specified destionation speed (s2) after accelerating + from initial speed s1 without ever stopping at a plateau: + + Solve[{DestinationSpeed[s1, a, di] == DestinationSpeed[s2, a, d - di]}, di] + di -> (2 a d - s1^2 + s2^2)/(4 a) --> intersection_distance() + + IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a) + */ - Basic definitions: - - Speed[s_, a_, t_] := s + (a*t) - Travel[s_, a_, t_] := Integrate[Speed[s, a, t], t] - - Distance to reach a specific speed with a constant acceleration: - - Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, d, t] - d -> (m^2 - s^2)/(2 a) --> estimate_acceleration_distance() - - Speed after a given distance of travel with constant acceleration: - - Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, m, t] - m -> Sqrt[2 a d + s^2] - - DestinationSpeed[s_, a_, d_] := Sqrt[2 a d + s^2] - - When to start braking (di) to reach a specified destionation speed (s2) after accelerating - from initial speed s1 without ever stopping at a plateau: - - Solve[{DestinationSpeed[s1, a, di] == DestinationSpeed[s2, a, d - di]}, di] - di -> (2 a d - s1^2 + s2^2)/(4 a) --> intersection_distance() - - IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a) -*/ - #include "Marlin.h" #include "planner.h" #include "stepper.h" @@ -83,10 +83,10 @@ static float previous_nominal_speed; // Nominal speed of previous path line segm extern volatile int extrudemultiply; // Sets extrude multiply factor (in percent) #ifdef AUTOTEMP - float autotemp_max=250; - float autotemp_min=210; - float autotemp_factor=0.1; - bool autotemp_enabled=false; +float autotemp_max=250; +float autotemp_min=210; +float autotemp_factor=0.1; +bool autotemp_enabled=false; #endif //=========================================================================== @@ -100,27 +100,33 @@ volatile unsigned char block_buffer_tail; // Index of the block to pro //=============================private variables ============================ //=========================================================================== #ifdef PREVENT_DANGEROUS_EXTRUDE - bool allow_cold_extrude=false; +bool allow_cold_extrude=false; #endif #ifdef 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]={0,0,0}; // Segment times (in us). Used for speed calculations - static long y_segment_time[3]={0,0,0}; +// 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]={ + 0,0,0}; // Segment times (in us). Used for speed calculations +static long y_segment_time[3]={ + 0,0,0}; #endif // 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) { block_index = 0; } + if (block_index == BLOCK_BUFFER_SIZE) { + block_index = 0; + } 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) { block_index = BLOCK_BUFFER_SIZE; } + if (block_index == 0) { + block_index = BLOCK_BUFFER_SIZE; + } block_index--; return(block_index); } @@ -134,8 +140,8 @@ static int8_t prev_block_index(int8_t block_index) { FORCE_INLINE float estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration) { if (acceleration!=0) { - return((target_rate*target_rate-initial_rate*initial_rate)/ - (2.0*acceleration)); + return((target_rate*target_rate-initial_rate*initial_rate)/ + (2.0*acceleration)); } else { return 0.0; // acceleration was 0, set acceleration distance to 0 @@ -149,9 +155,9 @@ FORCE_INLINE float estimate_acceleration_distance(float initial_rate, float targ FORCE_INLINE float intersection_distance(float initial_rate, float final_rate, float acceleration, float distance) { - if (acceleration!=0) { - return((2.0*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/ - (4.0*acceleration) ); + if (acceleration!=0) { + return((2.0*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/ + (4.0*acceleration) ); } else { return 0.0; // acceleration was 0, set intersection distance to 0 @@ -165,46 +171,50 @@ void calculate_trapezoid_for_block(block_t *block, float entry_factor, float exi unsigned long final_rate = ceil(block->nominal_rate*exit_factor); // (step/min) // Limit minimal step rate (Otherwise the timer will overflow.) - if(initial_rate <120) {initial_rate=120; } - if(final_rate < 120) {final_rate=120; } - + if(initial_rate <120) { + initial_rate=120; + } + if(final_rate < 120) { + final_rate=120; + } + long acceleration = block->acceleration_st; int32_t accelerate_steps = ceil(estimate_acceleration_distance(block->initial_rate, block->nominal_rate, acceleration)); int32_t decelerate_steps = floor(estimate_acceleration_distance(block->nominal_rate, block->final_rate, -acceleration)); - + // Calculate the size of Plateau of Nominal Rate. int32_t plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps; - + // 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 (plateau_steps < 0) { accelerate_steps = ceil( - intersection_distance(block->initial_rate, block->final_rate, acceleration, block->step_event_count)); + intersection_distance(block->initial_rate, block->final_rate, acceleration, block->step_event_count)); accelerate_steps = max(accelerate_steps,0); // Check limits due to numerical round-off accelerate_steps = min(accelerate_steps,block->step_event_count); plateau_steps = 0; } - #ifdef ADVANCE - volatile long initial_advance = block->advance*entry_factor*entry_factor; - volatile long final_advance = block->advance*exit_factor*exit_factor; - #endif // ADVANCE - - // block->accelerate_until = accelerate_steps; - // block->decelerate_after = accelerate_steps+plateau_steps; +#ifdef ADVANCE + volatile long initial_advance = block->advance*entry_factor*entry_factor; + volatile long final_advance = block->advance*exit_factor*exit_factor; +#endif // ADVANCE + + // 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. block->accelerate_until = accelerate_steps; block->decelerate_after = accelerate_steps+plateau_steps; block->initial_rate = initial_rate; block->final_rate = final_rate; - #ifdef ADVANCE - block->initial_advance = initial_advance; - block->final_advance = final_advance; - #endif //ADVANCE +#ifdef ADVANCE + block->initial_advance = initial_advance; + block->final_advance = final_advance; +#endif //ADVANCE } CRITICAL_SECTION_END; } @@ -226,24 +236,27 @@ FORCE_INLINE float max_allowable_speed(float acceleration, float target_velocity // 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(!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 { + 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. } @@ -252,10 +265,17 @@ void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *n // implements the reverse pass. void planner_reverse_pass() { uint8_t block_index = block_buffer_head; - if(((block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1)) > 3) { + + //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 != block_buffer_tail) { + 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]; @@ -267,8 +287,10 @@ void planner_reverse_pass() { // 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(!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. @@ -276,7 +298,7 @@ void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *n 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) ); + max_allowable_speed(-previous->acceleration,previous->entry_speed,previous->millimeters) ); // Check for junction speed change if (current->entry_speed != entry_speed) { @@ -291,7 +313,8 @@ void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *n // implements the forward pass. void planner_forward_pass() { uint8_t block_index = block_buffer_tail; - block_t *block[3] = { NULL, NULL, NULL }; + block_t *block[3] = { + NULL, NULL, NULL }; while(block_index != block_buffer_head) { block[0] = block[1]; @@ -310,7 +333,7 @@ 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]; @@ -319,7 +342,7 @@ void planner_recalculate_trapezoids() { 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); + next->entry_speed/current->nominal_speed); current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed } } @@ -328,7 +351,7 @@ void planner_recalculate_trapezoids() { // 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); + MINIMUM_PLANNER_SPEED/next->nominal_speed); next->recalculate_flag = false; } } @@ -380,14 +403,14 @@ void getHighESpeed() if(degTargetHotend0()+2high) @@ -397,7 +420,7 @@ void getHighESpeed() } block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1); } - + float g=autotemp_min+high*autotemp_factor; float t=g; if(t -1 - if (FanSpeed != 0){ - analogWrite(FAN_PIN,FanSpeed); // If buffer is empty use current fan speed - } - #endif +#if FAN_PIN > -1 + if (FanSpeed != 0){ + analogWrite(FAN_PIN,FanSpeed); // If buffer is empty use current fan speed + } +#endif } if((DISABLE_X) && (x_active == 0)) disable_x(); if((DISABLE_Y) && (y_active == 0)) disable_y(); if((DISABLE_Z) && (z_active == 0)) disable_z(); - if((DISABLE_E) && (e_active == 0)) { disable_e0();disable_e1();disable_e2(); } - #if FAN_PIN > -1 + if((DISABLE_E) && (e_active == 0)) { + disable_e0(); + disable_e1(); + disable_e2(); + } +#if FAN_PIN > -1 if((FanSpeed == 0) && (fan_speed ==0)) { analogWrite(FAN_PIN, 0); } @@ -454,10 +481,10 @@ void check_axes_activity() { if (FanSpeed != 0 && tail_fan_speed !=0) { analogWrite(FAN_PIN,tail_fan_speed); } - #endif - #ifdef AUTOTEMP - getHighESpeed(); - #endif +#endif +#ifdef AUTOTEMP + getHighESpeed(); +#endif } @@ -477,7 +504,7 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa manage_inactivity(1); LCD_STATUS; } - + // The target position of the tool in absolute steps // Calculate target position in absolute steps //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow @@ -486,28 +513,28 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]); target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]); target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]); - - #ifdef PREVENT_DANGEROUS_EXTRUDE - if(target[E_AXIS]!=position[E_AXIS]) + +#ifdef PREVENT_DANGEROUS_EXTRUDE + if(target[E_AXIS]!=position[E_AXIS]) if(degHotend(active_extruder)axis_steps_per_unit[E_AXIS]*EXTRUDE_MAXLENGTH) - { - position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part - SERIAL_ECHO_START; - SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP); - } - #endif - #endif - +#ifdef PREVENT_LENGTHY_EXTRUDE + if(labs(target[E_AXIS]-position[E_AXIS])>axis_steps_per_unit[E_AXIS]*EXTRUDE_MAXLENGTH) + { + position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part + SERIAL_ECHO_START; + SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP); + } +#endif +#endif + // 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) block->busy = false; @@ -521,36 +548,50 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e))); // Bail if this is a zero-length block - if (block->step_event_count <= dropsegments) { return; }; + if (block->step_event_count <= dropsegments) { + return; + }; block->fan_speed = FanSpeed; - + // Compute direction bits for this block block->direction_bits = 0; - if (target[X_AXIS] < position[X_AXIS]) { block->direction_bits |= (1<direction_bits |= (1<direction_bits |= (1<direction_bits |= (1<direction_bits |= (1<direction_bits |= (1<direction_bits |= (1<direction_bits |= (1<active_extruder = extruder; - + //enable active axes if(block->steps_x != 0) enable_x(); if(block->steps_y != 0) enable_y(); - #ifndef Z_LATE_ENABLE - if(block->steps_z != 0) enable_z(); - #endif +#ifndef Z_LATE_ENABLE + if(block->steps_z != 0) enable_z(); +#endif // Enable all - if(block->steps_e != 0) { enable_e0();enable_e1();enable_e2(); } + if(block->steps_e != 0) { + enable_e0(); + enable_e1(); + enable_e2(); + } if (block->steps_e == 0) { - if(feed_ratesteps_x <=dropsegments && block->steps_y <=dropsegments && block->steps_z <=dropsegments ) { block->millimeters = fabs(delta_mm[E_AXIS]); - } else { + } + else { block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS])); } float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides - - // 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); - - // 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 + // 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); + + // 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)); + inverse_second=1000000.0/(segment_time+lround(2*(minsegmenttime-segment_time)/moves_queued)); } } - #endif +#endif // END OF SLOW DOWN SECTION - + 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 - // Calculate and limit speed in mm/sec for each axis + // Calculate and limit speed in mm/sec for each axis float current_speed[4]; float speed_factor = 1.0; //factor <=1 do decrease speed for(int i=0; i < 4; i++) { @@ -597,7 +639,7 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa speed_factor = min(speed_factor, max_feedrate[i] / fabs(current_speed[i])); } -// Max segement time in us. + // Max segement time in us. #ifdef XY_FREQUENCY_LIMIT #define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT) @@ -606,7 +648,7 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa old_direction_bits = block->direction_bits; if((direction_change & (1<acceleration = block->acceleration_st / steps_per_mm; block->acceleration_rate = (long)((float)block->acceleration_st * 8.388608); - + #if 0 // Use old jerk for now // Compute path unit vector double unit_vec[3]; @@ -663,7 +705,7 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters; unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters; unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters; - + // Compute maximum allowable entry speed at junction by centripetal acceleration approximation. // Let a circle be tangent to both previous and current path line segments, where the junction // deviation is defined as the distance from the junction to the closest edge of the circle, @@ -680,9 +722,9 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa // Compute cosine of angle between previous and current path. (prev_unit_vec is negative) // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity. double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] - - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] - - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ; - + - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] + - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ; + // Skip and use default max junction speed for 0 degree acute junction. if (cos_theta < 0.95) { vmax_junction = min(previous_nominal_speed,block->nominal_speed); @@ -691,36 +733,39 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa // Compute maximum junction velocity based on maximum acceleration and junction deviation double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. vmax_junction = min(vmax_junction, - sqrt(block->acceleration * junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ); + sqrt(block->acceleration * junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ); } } } #endif // Start with a safe speed - float vmax_junction = max_xy_jerk/2; + 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 = max_z_jerk/2; - vmax_junction = min(vmax_junction, block->nominal_speed); + 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); - + vmax_junction = min(vmax_junction, block->nominal_speed); + 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((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 *= (max_xy_jerk/jerk); + vmax_junction_factor = (max_xy_jerk/jerk); } if(fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk) { - vmax_junction *= (max_z_jerk/fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS])); + 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 *= (max_e_jerk/fabs(current_speed[E_AXIS] - previous_speed[E_AXIS])); + 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 } 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); block->entry_speed = min(vmax_junction, v_allowable); @@ -733,48 +778,52 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa // 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; } + 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 - + // Update previous path unit_vector and nominal speed memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[] previous_nominal_speed = block->nominal_speed; - - #ifdef ADVANCE - // Calculate advance rate - if((block->steps_e == 0) || (block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)) { + +#ifdef ADVANCE + // Calculate advance rate + if((block->steps_e == 0) || (block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)) { + block->advance_rate = 0; + block->advance = 0; + } + else { + long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st); + float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) * + (current_speed[E_AXIS] * current_speed[E_AXIS] * EXTRUTION_AREA * EXTRUTION_AREA)*256; + block->advance = advance; + if(acc_dist == 0) { block->advance_rate = 0; - block->advance = 0; - } + } else { - long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st); - float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) * - (current_speed[E_AXIS] * current_speed[E_AXIS] * EXTRUTION_AREA * EXTRUTION_AREA)*256; - block->advance = advance; - if(acc_dist == 0) { - block->advance_rate = 0; - } - else { - block->advance_rate = advance / (float)acc_dist; - } + block->advance_rate = advance / (float)acc_dist; } - /* + } + /* SERIAL_ECHO_START; - SERIAL_ECHOPGM("advance :"); - SERIAL_ECHO(block->advance/256.0); - SERIAL_ECHOPGM("advance rate :"); - SERIAL_ECHOLN(block->advance_rate/256.0); - */ - #endif // ADVANCE + SERIAL_ECHOPGM("advance :"); + SERIAL_ECHO(block->advance/256.0); + SERIAL_ECHOPGM("advance rate :"); + SERIAL_ECHOLN(block->advance_rate/256.0); + */ +#endif // ADVANCE calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed, - MINIMUM_PLANNER_SPEED/block->nominal_speed); - + safe_speed/block->nominal_speed); + // Move buffer head block_buffer_head = next_buffer_head; - + // Update position memcpy(position, target, sizeof(target)); // position[] = target[] @@ -805,12 +854,13 @@ void plan_set_e_position(const float &e) uint8_t movesplanned() { - return (block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1); + return (block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1); } void allow_cold_extrudes(bool allow) { - #ifdef PREVENT_DANGEROUS_EXTRUDE - allow_cold_extrude=allow; - #endif +#ifdef PREVENT_DANGEROUS_EXTRUDE + allow_cold_extrude=allow; +#endif } + diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp index bb53e069b..08ed8edd9 100644 --- a/Marlin/stepper.cpp +++ b/Marlin/stepper.cpp @@ -261,12 +261,10 @@ FORCE_INLINE void trapezoid_generator_reset() { #endif deceleration_time = 0; // step_rate to timer interval + OCR1A_nominal = calc_timer(current_block->nominal_rate); acc_step_rate = current_block->initial_rate; acceleration_time = calc_timer(acc_step_rate); OCR1A = acceleration_time; - OCR1A_nominal = calc_timer(current_block->nominal_rate); - - // SERIAL_ECHO_START; // SERIAL_ECHOPGM("advance :"); diff --git a/Marlin/ultralcd.pde b/Marlin/ultralcd.pde index 1b9390298..7c495cae2 100644 --- a/Marlin/ultralcd.pde +++ b/Marlin/ultralcd.pde @@ -957,7 +957,7 @@ enum { #if EXTRUDERS > 2 ItemCT_nozzle2, #endif -#if defined BED_USES_THERMISTOR || BED_USES_AD595 +#if defined BED_USES_THERMISTOR || defined BED_USES_AD595 ItemCT_bed, #endif ItemCT_fan, @@ -1212,7 +1212,7 @@ void MainMenu::showControlTemp() }break; #endif //autotemp - #if defined BED_USES_THERMISTOR || BED_USES_AD595 + #if defined BED_USES_THERMISTOR || defined BED_USES_AD595 case ItemCT_bed: { if(force_lcd_update)