2011-11-15 21:50:43 +00:00
/*
planner . c - buffers movement commands and manages the acceleration profile plan
2012-06-11 15:33:42 +00:00
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 < http : //www.gnu.org/licenses/>.
*/
2011-11-15 21:50:43 +00:00
/* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */
/*
2012-06-11 15:33:42 +00:00
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 )
*/
2011-11-15 21:50:43 +00:00
2011-12-06 04:33:33 +00:00
# include "Marlin.h"
2011-11-15 21:50:43 +00:00
# include "planner.h"
# include "stepper.h"
# include "temperature.h"
# include "ultralcd.h"
2012-03-03 15:51:47 +00:00
# include "language.h"
2011-11-15 21:50:43 +00:00
//===========================================================================
//=============================public variables ============================
//===========================================================================
unsigned long minsegmenttime ;
float max_feedrate [ 4 ] ; // set the max speeds
float axis_steps_per_unit [ 4 ] ;
unsigned long max_acceleration_units_per_sq_second [ 4 ] ; // Use M201 to override by software
float minimumfeedrate ;
float acceleration ; // Normal acceleration mm/s^2 THIS IS THE DEFAULT ACCELERATION for all moves. M204 SXXXX
float retract_acceleration ; // mm/s^2 filament pull-pack and push-forward while standing still in the other axis M204 TXXXX
float max_xy_jerk ; //speed than can be stopped at once, if i understand correctly.
float max_z_jerk ;
2012-03-04 15:34:58 +00:00
float max_e_jerk ;
2011-11-15 21:50:43 +00:00
float mintravelfeedrate ;
unsigned long axis_steps_per_sqr_second [ NUM_AXIS ] ;
// The current position of the tool in absolute steps
long position [ 4 ] ; //rescaled from extern when axis_steps_per_unit are changed by gcode
static float previous_speed [ 4 ] ; // Speed of previous path line segment
static float previous_nominal_speed ; // Nominal speed of previous path line segment
# ifdef AUTOTEMP
2012-06-11 15:33:42 +00:00
float autotemp_max = 250 ;
float autotemp_min = 210 ;
float autotemp_factor = 0.1 ;
bool autotemp_enabled = false ;
2011-11-15 21:50:43 +00:00
# endif
2011-11-27 13:57:12 +00:00
//===========================================================================
//=================semi-private variables, used in inline functions =====
//===========================================================================
block_t block_buffer [ BLOCK_BUFFER_SIZE ] ; // A ring buffer for motion instfructions
volatile unsigned char block_buffer_head ; // Index of the next block to be pushed
volatile unsigned char block_buffer_tail ; // Index of the block to process now
2011-11-15 21:50:43 +00:00
//===========================================================================
//=============================private variables ============================
//===========================================================================
2011-12-09 14:09:52 +00:00
# ifdef PREVENT_DANGEROUS_EXTRUDE
2013-06-02 15:36:23 +00:00
float extrude_min_temp = EXTRUDE_MINTEMP ;
2011-12-09 14:09:52 +00:00
# endif
2011-12-04 11:40:18 +00:00
# ifdef XY_FREQUENCY_LIMIT
2012-11-21 19:17:44 +00:00
# define MAX_FREQ_TIME (1000000.0 / XY_FREQUENCY_LIMIT)
2012-06-11 15:33:42 +00:00
// Used for the frequency limit
static unsigned char old_direction_bits = 0 ; // Old direction bits. Used for speed calculations
2012-11-21 19:17:44 +00:00
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 } ;
2011-12-04 11:40:18 +00:00
# endif
2011-11-15 21:50:43 +00:00
// 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 + + ;
2012-06-11 15:33:42 +00:00
if ( block_index = = BLOCK_BUFFER_SIZE ) {
block_index = 0 ;
}
2011-11-15 21:50:43 +00:00
return ( block_index ) ;
}
// Returns the index of the previous block in the ring buffer
static int8_t prev_block_index ( int8_t block_index ) {
2012-06-11 15:33:42 +00:00
if ( block_index = = 0 ) {
block_index = BLOCK_BUFFER_SIZE ;
}
2011-11-15 21:50:43 +00:00
block_index - - ;
return ( block_index ) ;
}
//===========================================================================
//=============================functions ============================
//===========================================================================
// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
// given acceleration:
2011-11-27 15:04:58 +00:00
FORCE_INLINE float estimate_acceleration_distance ( float initial_rate , float target_rate , float acceleration )
{
2011-11-15 21:50:43 +00:00
if ( acceleration ! = 0 ) {
2012-06-11 15:33:42 +00:00
return ( ( target_rate * target_rate - initial_rate * initial_rate ) /
( 2.0 * acceleration ) ) ;
2011-11-15 21:50:43 +00:00
}
else {
return 0.0 ; // acceleration was 0, set acceleration distance to 0
}
}
// This function gives you the point at which you must start braking (at the rate of -acceleration) if
// you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
// a total travel of distance. This can be used to compute the intersection point between acceleration and
// deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
2011-11-27 15:04:58 +00:00
FORCE_INLINE float intersection_distance ( float initial_rate , float final_rate , float acceleration , float distance )
{
2012-06-11 15:33:42 +00:00
if ( acceleration ! = 0 ) {
return ( ( 2.0 * acceleration * distance - initial_rate * initial_rate + final_rate * final_rate ) /
( 4.0 * acceleration ) ) ;
2011-11-15 21:50:43 +00:00
}
else {
return 0.0 ; // acceleration was 0, set intersection distance to 0
}
}
// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
void calculate_trapezoid_for_block ( block_t * block , float entry_factor , float exit_factor ) {
unsigned long initial_rate = ceil ( block - > nominal_rate * entry_factor ) ; // (step/min)
unsigned long final_rate = ceil ( block - > nominal_rate * exit_factor ) ; // (step/min)
// Limit minimal step rate (Otherwise the timer will overflow.)
2012-06-11 15:33:42 +00:00
if ( initial_rate < 120 ) {
initial_rate = 120 ;
}
if ( final_rate < 120 ) {
final_rate = 120 ;
}
2011-11-15 21:50:43 +00:00
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 ) ) ;
2012-06-11 15:33:42 +00:00
2011-11-15 21:50:43 +00:00
// Calculate the size of Plateau of Nominal Rate.
int32_t plateau_steps = block - > step_event_count - accelerate_steps - decelerate_steps ;
2012-06-11 15:33:42 +00:00
2011-11-15 21:50:43 +00:00
// 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 ) {
2012-11-06 11:06:41 +00:00
accelerate_steps = ceil ( intersection_distance ( block - > initial_rate , block - > final_rate , acceleration , block - > step_event_count ) ) ;
2011-11-15 21:50:43 +00:00
accelerate_steps = max ( accelerate_steps , 0 ) ; // Check limits due to numerical round-off
2012-11-12 08:29:12 +00:00
accelerate_steps = min ( ( uint32_t ) accelerate_steps , block - > step_event_count ) ; //(We can cast here to unsigned, because the above line ensures that we are above zero)
2011-11-15 21:50:43 +00:00
plateau_steps = 0 ;
}
2012-06-11 15:33:42 +00:00
# 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;
2011-11-15 21:50:43 +00:00
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 ;
2012-06-11 15:33:42 +00:00
# ifdef ADVANCE
block - > initial_advance = initial_advance ;
block - > final_advance = final_advance ;
# endif //ADVANCE
2011-11-15 21:50:43 +00:00
}
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.
2011-11-27 15:04:58 +00:00
FORCE_INLINE float max_allowable_speed ( float acceleration , float target_velocity , float distance ) {
2011-11-15 21:50:43 +00:00
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 ) {
2012-06-11 15:33:42 +00:00
if ( ! current ) {
return ;
}
if ( next ) {
2011-11-15 21:50:43 +00:00
// 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 ) {
2012-06-11 15:33:42 +00:00
2011-11-15 21:50:43 +00:00
// 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 ,
2012-06-11 15:33:42 +00:00
max_allowable_speed ( - current - > acceleration , next - > entry_speed , current - > millimeters ) ) ;
}
else {
2011-11-15 21:50:43 +00:00
current - > entry_speed = current - > max_entry_speed ;
}
current - > recalculate_flag = true ;
2012-06-11 15:33:42 +00:00
2011-11-15 21:50:43 +00:00
}
} // 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 ( ) {
2011-12-04 11:40:18 +00:00
uint8_t block_index = block_buffer_head ;
2012-06-11 15:33:42 +00:00
//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 ) {
2011-11-15 21:50:43 +00:00
block_index = ( block_buffer_head - 3 ) & ( BLOCK_BUFFER_SIZE - 1 ) ;
2012-06-11 15:33:42 +00:00
block_t * block [ 3 ] = {
NULL , NULL , NULL } ;
while ( block_index ! = tail ) {
2011-11-15 21:50:43 +00:00
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 ) {
2012-06-11 15:33:42 +00:00
if ( ! previous ) {
return ;
}
2011-11-15 21:50:43 +00:00
// 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 ,
2012-06-11 15:33:42 +00:00
max_allowable_speed ( - previous - > acceleration , previous - > entry_speed , previous - > millimeters ) ) ;
2011-11-15 21:50:43 +00:00
// 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 ( ) {
2011-12-04 11:40:18 +00:00
uint8_t block_index = block_buffer_tail ;
2012-06-11 15:33:42 +00:00
block_t * block [ 3 ] = {
NULL , NULL , NULL } ;
2011-11-15 21:50:43 +00:00
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 ;
2012-06-11 15:33:42 +00:00
2011-11-15 21:50:43 +00:00
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 ,
2012-06-11 15:33:42 +00:00
next - > entry_speed / current - > nominal_speed ) ;
2011-11-15 21:50:43 +00:00
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 ,
2012-06-11 15:33:42 +00:00
MINIMUM_PLANNER_SPEED / next - > nominal_speed ) ;
2011-11-15 21:50:43 +00:00
next - > recalculate_flag = false ;
}
}
// Recalculates the motion plan according to the following algorithm:
//
// 1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_factor)
// so that:
// a. The junction jerk is within the set limit
// b. No speed reduction within one block requires faster deceleration than the one, true constant
// acceleration.
// 2. Go over every block in chronological order and dial down junction speed reduction values if
// a. The speed increase within one block would require faster accelleration than the one, true
// constant acceleration.
//
// When these stages are complete all blocks have an entry_factor that will allow all speed changes to
// be performed using only the one, true constant acceleration, and where no junction jerk is jerkier than
// the set limit. Finally it will:
//
// 3. Recalculate trapezoids for all blocks.
void planner_recalculate ( ) {
planner_reverse_pass ( ) ;
planner_forward_pass ( ) ;
planner_recalculate_trapezoids ( ) ;
}
void plan_init ( ) {
block_buffer_head = 0 ;
block_buffer_tail = 0 ;
memset ( position , 0 , sizeof ( position ) ) ; // clear position
previous_speed [ 0 ] = 0.0 ;
previous_speed [ 1 ] = 0.0 ;
previous_speed [ 2 ] = 0.0 ;
previous_speed [ 3 ] = 0.0 ;
previous_nominal_speed = 0.0 ;
}
# ifdef AUTOTEMP
void getHighESpeed ( )
{
2011-11-19 16:07:44 +00:00
static float oldt = 0 ;
2012-04-15 17:17:33 +00:00
if ( ! autotemp_enabled ) {
2011-11-19 14:37:10 +00:00
return ;
2012-04-15 17:17:33 +00:00
}
if ( degTargetHotend0 ( ) + 2 < autotemp_min ) { //probably temperature set to zero.
2011-11-15 21:50:43 +00:00
return ; //do nothing
2012-04-15 17:17:33 +00:00
}
2012-06-11 15:33:42 +00:00
2012-04-15 17:17:33 +00:00
float high = 0.0 ;
2011-12-04 11:40:18 +00:00
uint8_t block_index = block_buffer_tail ;
2012-06-11 15:33:42 +00:00
2011-11-15 21:50:43 +00:00
while ( block_index ! = block_buffer_head ) {
2012-04-15 17:17:33 +00:00
if ( ( block_buffer [ block_index ] . steps_x ! = 0 ) | |
2012-06-11 15:33:42 +00:00
( block_buffer [ block_index ] . steps_y ! = 0 ) | |
( block_buffer [ block_index ] . steps_z ! = 0 ) ) {
2012-04-15 17:17:33 +00:00
float se = ( float ( block_buffer [ block_index ] . steps_e ) / float ( block_buffer [ block_index ] . step_event_count ) ) * block_buffer [ block_index ] . nominal_speed ;
//se; mm/sec;
if ( se > high )
{
high = se ;
}
2011-11-15 21:50:43 +00:00
}
block_index = ( block_index + 1 ) & ( BLOCK_BUFFER_SIZE - 1 ) ;
}
2012-06-11 15:33:42 +00:00
2011-11-19 14:37:10 +00:00
float g = autotemp_min + high * autotemp_factor ;
2011-11-19 16:07:44 +00:00
float t = g ;
if ( t < autotemp_min )
t = autotemp_min ;
if ( t > autotemp_max )
t = autotemp_max ;
if ( oldt > t )
{
t = AUTOTEMP_OLDWEIGHT * oldt + ( 1 - AUTOTEMP_OLDWEIGHT ) * t ;
}
oldt = t ;
2011-11-15 21:50:43 +00:00
setTargetHotend0 ( t ) ;
}
# endif
2012-11-21 19:17:44 +00:00
void check_axes_activity ( )
2012-11-12 14:49:40 +00:00
{
2011-11-15 21:50:43 +00:00
unsigned char x_active = 0 ;
unsigned char y_active = 0 ;
unsigned char z_active = 0 ;
unsigned char e_active = 0 ;
2012-12-10 03:28:09 +00:00
unsigned char tail_fan_speed = fanSpeed ;
2013-05-14 21:56:32 +00:00
# ifdef BARICUDA
2013-05-16 17:00:57 +00:00
unsigned char tail_valve_pressure = ValvePressure ;
unsigned char tail_e_to_p_pressure = EtoPPressure ;
2013-05-14 21:56:32 +00:00
# endif
2011-11-15 21:50:43 +00:00
block_t * block ;
2012-11-21 19:17:44 +00:00
if ( block_buffer_tail ! = block_buffer_head )
2012-11-12 14:49:40 +00:00
{
2011-12-04 11:40:18 +00:00
uint8_t block_index = block_buffer_tail ;
2012-03-04 12:05:26 +00:00
tail_fan_speed = block_buffer [ block_index ] . fan_speed ;
2013-05-14 21:56:32 +00:00
# ifdef BARICUDA
2013-05-14 20:25:53 +00:00
tail_valve_pressure = block_buffer [ block_index ] . valve_pressure ;
tail_e_to_p_pressure = block_buffer [ block_index ] . e_to_p_pressure ;
2013-05-14 21:56:32 +00:00
# endif
2012-11-21 19:17:44 +00:00
while ( block_index ! = block_buffer_head )
2012-11-12 14:49:40 +00:00
{
2011-11-15 21:50:43 +00:00
block = & block_buffer [ block_index ] ;
if ( block - > steps_x ! = 0 ) x_active + + ;
if ( block - > steps_y ! = 0 ) y_active + + ;
if ( block - > steps_z ! = 0 ) z_active + + ;
if ( block - > steps_e ! = 0 ) e_active + + ;
block_index = ( block_index + 1 ) & ( BLOCK_BUFFER_SIZE - 1 ) ;
}
}
if ( ( DISABLE_X ) & & ( x_active = = 0 ) ) disable_x ( ) ;
if ( ( DISABLE_Y ) & & ( y_active = = 0 ) ) disable_y ( ) ;
2012-02-05 20:14:55 +00:00
if ( ( DISABLE_Z ) & & ( z_active = = 0 ) ) disable_z ( ) ;
2012-11-21 19:17:44 +00:00
if ( ( DISABLE_E ) & & ( e_active = = 0 ) )
2012-11-12 14:49:40 +00:00
{
2012-06-11 15:33:42 +00:00
disable_e0 ( ) ;
disable_e1 ( ) ;
disable_e2 ( ) ;
}
2013-05-22 11:33:23 +00:00
# if defined(FAN_PIN) && FAN_PIN > -1
2013-06-09 10:52:49 +00:00
# ifdef FAN_KICKSTART_TIME
static unsigned long fan_kick_end ;
if ( tail_fan_speed ) {
if ( fan_kick_end = = 0 ) {
// Just starting up fan - run at full power.
fan_kick_end = millis ( ) + FAN_KICKSTART_TIME ;
tail_fan_speed = 255 ;
} else if ( fan_kick_end > millis ( ) )
// Fan still spinning up.
tail_fan_speed = 255 ;
} else {
fan_kick_end = 0 ;
}
# endif //FAN_KICKSTART_TIME
# ifdef FAN_SOFT_PWM
fanSpeedSoftPwm = tail_fan_speed ;
# else
analogWrite ( FAN_PIN , tail_fan_speed ) ;
2013-01-10 10:08:45 +00:00
# endif //!FAN_SOFT_PWM
# endif //FAN_PIN > -1
2012-06-11 15:33:42 +00:00
# ifdef AUTOTEMP
getHighESpeed ( ) ;
# endif
2013-05-14 20:25:53 +00:00
2013-05-14 21:56:32 +00:00
# ifdef BARICUDA
2013-05-22 11:33:23 +00:00
# if defined(HEATER_1_PIN) && HEATER_1_PIN > -1
2013-05-14 21:16:10 +00:00
analogWrite ( HEATER_1_PIN , tail_valve_pressure ) ;
2013-05-14 21:56:32 +00:00
# endif
2013-05-14 21:16:10 +00:00
2013-05-22 11:33:23 +00:00
# if defined(HEATER_2_PIN) && HEATER_2_PIN > -1
2013-05-14 21:16:10 +00:00
analogWrite ( HEATER_2_PIN , tail_e_to_p_pressure ) ;
2013-05-14 21:56:32 +00:00
# endif
2013-05-14 20:25:53 +00:00
# endif
2011-11-15 21:50:43 +00:00
}
float junction_deviation = 0.1 ;
// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in
// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
// calculation the caller must also provide the physical length of the line in millimeters.
2012-02-29 10:38:12 +00:00
void plan_buffer_line ( const float & x , const float & y , const float & z , const float & e , float feed_rate , const uint8_t & extruder )
2011-11-15 21:50:43 +00:00
{
// Calculate the buffer head after we push this byte
int next_buffer_head = next_block_index ( block_buffer_head ) ;
// If the buffer is full: good! That means we are well ahead of the robot.
// Rest here until there is room in the buffer.
2012-11-21 19:17:44 +00:00
while ( block_buffer_tail = = next_buffer_head )
2012-11-12 14:49:40 +00:00
{
2011-11-15 21:50:43 +00:00
manage_heater ( ) ;
2012-08-21 12:48:29 +00:00
manage_inactivity ( ) ;
2012-12-03 11:52:00 +00:00
lcd_update ( ) ;
2011-11-15 21:50:43 +00:00
}
2012-06-11 15:33:42 +00:00
2011-11-15 21:50:43 +00:00
// 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
long target [ 4 ] ;
target [ X_AXIS ] = lround ( x * axis_steps_per_unit [ X_AXIS ] ) ;
target [ Y_AXIS ] = lround ( y * axis_steps_per_unit [ Y_AXIS ] ) ;
target [ Z_AXIS ] = lround ( z * axis_steps_per_unit [ Z_AXIS ] ) ;
2011-12-09 14:09:52 +00:00
target [ E_AXIS ] = lround ( e * axis_steps_per_unit [ E_AXIS ] ) ;
2012-06-11 15:33:42 +00:00
2012-11-12 14:49:40 +00:00
# ifdef PREVENT_DANGEROUS_EXTRUDE
2012-11-21 19:17:44 +00:00
if ( target [ E_AXIS ] ! = position [ E_AXIS ] )
2012-11-12 14:49:40 +00:00
{
2013-06-02 15:36:23 +00:00
if ( degHotend ( active_extruder ) < extrude_min_temp )
2011-12-09 14:09:52 +00:00
{
position [ E_AXIS ] = target [ E_AXIS ] ; //behave as if the move really took place, but ignore E part
SERIAL_ECHO_START ;
2012-03-03 15:51:47 +00:00
SERIAL_ECHOLNPGM ( MSG_ERR_COLD_EXTRUDE_STOP ) ;
2011-12-09 14:09:52 +00:00
}
2012-11-21 19:17:44 +00:00
2012-11-12 14:49:40 +00:00
# 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 ) ;
}
2012-11-21 19:17:44 +00:00
# endif
2012-06-11 15:33:42 +00:00
}
2012-11-12 14:49:40 +00:00
# endif
2012-06-11 15:33:42 +00:00
2011-11-15 21:50:43 +00:00
// Prepare to set up new block
block_t * block = & block_buffer [ block_buffer_head ] ;
2012-06-11 15:33:42 +00:00
2011-11-15 21:50:43 +00:00
// Mark block as not busy (Not executed by the stepper interrupt)
block - > busy = false ;
// Number of steps for each axis
block - > steps_x = labs ( target [ X_AXIS ] - position [ X_AXIS ] ) ;
block - > steps_y = labs ( target [ Y_AXIS ] - position [ Y_AXIS ] ) ;
block - > steps_z = labs ( target [ Z_AXIS ] - position [ Z_AXIS ] ) ;
block - > steps_e = labs ( target [ E_AXIS ] - position [ E_AXIS ] ) ;
2012-02-24 19:42:07 +00:00
block - > steps_e * = extrudemultiply ;
2012-02-24 22:34:41 +00:00
block - > steps_e / = 100 ;
2011-11-15 21:50:43 +00:00
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
2012-11-21 19:17:44 +00:00
if ( block - > step_event_count < = dropsegments )
2012-11-12 14:49:40 +00:00
{
2012-06-11 15:33:42 +00:00
return ;
2012-11-12 14:49:40 +00:00
}
2011-11-15 21:50:43 +00:00
2012-12-03 11:13:20 +00:00
block - > fan_speed = fanSpeed ;
2013-05-14 21:56:32 +00:00
# ifdef BARICUDA
2013-05-14 20:25:53 +00:00
block - > valve_pressure = ValvePressure ;
block - > e_to_p_pressure = EtoPPressure ;
2013-05-14 21:56:32 +00:00
# endif
2012-06-11 15:33:42 +00:00
2011-12-04 18:54:07 +00:00
// Compute direction bits for this block
2011-11-15 21:50:43 +00:00
block - > direction_bits = 0 ;
2012-11-21 19:17:44 +00:00
if ( target [ X_AXIS ] < position [ X_AXIS ] )
2012-11-12 14:49:40 +00:00
{
2012-06-11 15:33:42 +00:00
block - > direction_bits | = ( 1 < < X_AXIS ) ;
}
2012-11-21 19:17:44 +00:00
if ( target [ Y_AXIS ] < position [ Y_AXIS ] )
2012-11-12 14:49:40 +00:00
{
2012-06-11 15:33:42 +00:00
block - > direction_bits | = ( 1 < < Y_AXIS ) ;
}
2012-11-21 19:17:44 +00:00
if ( target [ Z_AXIS ] < position [ Z_AXIS ] )
2012-11-12 14:49:40 +00:00
{
2012-06-11 15:33:42 +00:00
block - > direction_bits | = ( 1 < < Z_AXIS ) ;
}
2012-11-21 19:17:44 +00:00
if ( target [ E_AXIS ] < position [ E_AXIS ] )
2012-11-12 14:49:40 +00:00
{
2012-06-11 15:33:42 +00:00
block - > direction_bits | = ( 1 < < E_AXIS ) ;
}
2011-12-02 16:45:05 +00:00
block - > active_extruder = extruder ;
2012-06-11 15:33:42 +00:00
2011-11-15 21:50:43 +00:00
//enable active axes
2013-05-05 17:23:59 +00:00
# ifdef COREXY
if ( ( block - > steps_x ! = 0 ) | | ( block - > steps_y ! = 0 ) )
{
enable_x ( ) ;
enable_y ( ) ;
}
# else
2011-11-15 21:50:43 +00:00
if ( block - > steps_x ! = 0 ) enable_x ( ) ;
if ( block - > steps_y ! = 0 ) enable_y ( ) ;
2013-05-05 17:23:59 +00:00
# endif
2012-06-11 15:33:42 +00:00
# ifndef Z_LATE_ENABLE
if ( block - > steps_z ! = 0 ) enable_z ( ) ;
# endif
2011-12-06 04:33:33 +00:00
// Enable all
2012-11-21 19:17:44 +00:00
if ( block - > steps_e ! = 0 )
2012-11-12 14:49:40 +00:00
{
2012-06-11 15:33:42 +00:00
enable_e0 ( ) ;
enable_e1 ( ) ;
enable_e2 ( ) ;
}
2011-12-06 04:33:33 +00:00
2012-11-21 19:17:44 +00:00
if ( block - > steps_e = = 0 )
2012-11-12 14:49:40 +00:00
{
2012-06-11 15:33:42 +00:00
if ( feed_rate < mintravelfeedrate ) feed_rate = mintravelfeedrate ;
2012-03-04 15:34:58 +00:00
}
2012-11-21 19:17:44 +00:00
else
2012-11-12 14:49:40 +00:00
{
2012-06-11 15:33:42 +00:00
if ( feed_rate < minimumfeedrate ) feed_rate = minimumfeedrate ;
2012-03-04 15:34:58 +00:00
}
2012-06-11 15:33:42 +00:00
2011-11-15 21:50:43 +00:00
float delta_mm [ 4 ] ;
delta_mm [ X_AXIS ] = ( target [ X_AXIS ] - position [ X_AXIS ] ) / axis_steps_per_unit [ X_AXIS ] ;
delta_mm [ Y_AXIS ] = ( target [ Y_AXIS ] - position [ Y_AXIS ] ) / axis_steps_per_unit [ Y_AXIS ] ;
delta_mm [ Z_AXIS ] = ( target [ Z_AXIS ] - position [ Z_AXIS ] ) / axis_steps_per_unit [ Z_AXIS ] ;
2012-02-24 19:42:07 +00:00
delta_mm [ E_AXIS ] = ( ( target [ E_AXIS ] - position [ E_AXIS ] ) / axis_steps_per_unit [ E_AXIS ] ) * extrudemultiply / 100.0 ;
2012-11-21 19:17:44 +00:00
if ( block - > steps_x < = dropsegments & & block - > steps_y < = dropsegments & & block - > steps_z < = dropsegments )
2012-11-12 14:49:40 +00:00
{
2012-05-16 16:17:34 +00:00
block - > millimeters = fabs ( delta_mm [ E_AXIS ] ) ;
2012-06-11 15:33:42 +00:00
}
2012-11-21 19:17:44 +00:00
else
2012-11-12 14:49:40 +00:00
{
2012-02-09 18:53:06 +00:00
block - > millimeters = sqrt ( square ( delta_mm [ X_AXIS ] ) + square ( delta_mm [ Y_AXIS ] ) + square ( delta_mm [ Z_AXIS ] ) ) ;
}
2011-11-15 21:50:43 +00:00
float inverse_millimeters = 1.0 / block - > millimeters ; // Inverse millimeters to remove multiple divides
2012-06-11 15:33:42 +00:00
// Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
2011-11-15 21:50:43 +00:00
float inverse_second = feed_rate * inverse_millimeters ;
2012-06-11 15:33:42 +00:00
2012-04-15 17:17:33 +00:00
int moves_queued = ( block_buffer_head - block_buffer_tail + BLOCK_BUFFER_SIZE ) & ( BLOCK_BUFFER_SIZE - 1 ) ;
2012-06-11 15:33:42 +00:00
2012-04-15 17:17:33 +00:00
// slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill
2012-06-11 15:33:42 +00:00
# ifdef OLD_SLOWDOWN
2012-11-21 19:17:44 +00:00
if ( moves_queued < ( BLOCK_BUFFER_SIZE * 0.5 ) & & moves_queued > 1 )
2012-11-12 14:49:40 +00:00
feed_rate = feed_rate * moves_queued / ( BLOCK_BUFFER_SIZE * 0.5 ) ;
2012-06-11 15:33:42 +00:00
# endif
2011-11-27 20:12:55 +00:00
2012-06-11 15:33:42 +00:00
# ifdef SLOWDOWN
2011-12-04 11:40:18 +00:00
// segment time im micro seconds
2012-04-15 17:17:33 +00:00
unsigned long segment_time = lround ( 1000000.0 / inverse_second ) ;
2012-11-21 19:17:44 +00:00
if ( ( moves_queued > 1 ) & & ( moves_queued < ( BLOCK_BUFFER_SIZE * 0.5 ) ) )
2012-11-12 14:49:40 +00:00
{
2012-11-21 19:17:44 +00:00
if ( segment_time < minsegmenttime )
2012-11-12 14:49:40 +00:00
{ // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
2012-06-11 15:33:42 +00:00
inverse_second = 1000000.0 / ( segment_time + lround ( 2 * ( minsegmenttime - segment_time ) / moves_queued ) ) ;
2012-11-21 19:17:44 +00:00
# ifdef XY_FREQUENCY_LIMIT
segment_time = lround ( 1000000.0 / inverse_second ) ;
# endif
2011-11-15 21:50:43 +00:00
}
}
2012-06-11 15:33:42 +00:00
# endif
2011-11-15 21:50:43 +00:00
// END OF SLOW DOWN SECTION
2012-06-11 15:33:42 +00:00
2012-04-15 17:17:33 +00:00
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
2011-11-15 21:50:43 +00:00
2012-06-11 15:33:42 +00:00
// Calculate and limit speed in mm/sec for each axis
2011-11-15 21:50:43 +00:00
float current_speed [ 4 ] ;
float speed_factor = 1.0 ; //factor <=1 do decrease speed
2012-11-21 19:17:44 +00:00
for ( int i = 0 ; i < 4 ; i + + )
2012-11-12 14:49:40 +00:00
{
2012-04-15 17:17:33 +00:00
current_speed [ i ] = delta_mm [ i ] * inverse_second ;
2012-05-16 16:17:34 +00:00
if ( fabs ( current_speed [ i ] ) > max_feedrate [ i ] )
speed_factor = min ( speed_factor , max_feedrate [ i ] / fabs ( current_speed [ i ] ) ) ;
2011-11-15 21:50:43 +00:00
}
2012-06-11 15:33:42 +00:00
// Max segement time in us.
2011-11-15 21:50:43 +00:00
# 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 ;
2012-11-21 19:17:44 +00:00
segment_time = lround ( ( float ) segment_time / speed_factor ) ;
if ( ( direction_change & ( 1 < < X_AXIS ) ) = = 0 )
2012-11-12 14:49:40 +00:00
{
2012-06-11 15:33:42 +00:00
x_segment_time [ 0 ] + = segment_time ;
2011-11-15 21:50:43 +00:00
}
2012-11-21 19:17:44 +00:00
else
2012-11-12 14:49:40 +00:00
{
2011-11-15 21:50:43 +00:00
x_segment_time [ 2 ] = x_segment_time [ 1 ] ;
x_segment_time [ 1 ] = x_segment_time [ 0 ] ;
x_segment_time [ 0 ] = segment_time ;
}
2012-11-21 19:17:44 +00:00
if ( ( direction_change & ( 1 < < Y_AXIS ) ) = = 0 )
2012-11-12 14:49:40 +00:00
{
2012-06-11 15:33:42 +00:00
y_segment_time [ 0 ] + = segment_time ;
2011-11-15 21:50:43 +00:00
}
2012-11-21 19:17:44 +00:00
else
2012-11-12 14:49:40 +00:00
{
2011-11-15 21:50:43 +00:00
y_segment_time [ 2 ] = y_segment_time [ 1 ] ;
y_segment_time [ 1 ] = y_segment_time [ 0 ] ;
y_segment_time [ 0 ] = segment_time ;
}
long max_x_segment_time = max ( x_segment_time [ 0 ] , max ( x_segment_time [ 1 ] , x_segment_time [ 2 ] ) ) ;
long max_y_segment_time = max ( y_segment_time [ 0 ] , max ( y_segment_time [ 1 ] , y_segment_time [ 2 ] ) ) ;
long min_xy_segment_time = min ( max_x_segment_time , max_y_segment_time ) ;
2012-11-21 19:17:44 +00:00
if ( min_xy_segment_time < MAX_FREQ_TIME )
2012-11-12 14:49:40 +00:00
speed_factor = min ( speed_factor , speed_factor * ( float ) min_xy_segment_time / ( float ) MAX_FREQ_TIME ) ;
2011-11-15 21:50:43 +00:00
# endif
// Correct the speed
2012-11-21 19:17:44 +00:00
if ( speed_factor < 1.0 )
2012-11-12 14:49:40 +00:00
{
2012-11-21 19:17:44 +00:00
for ( unsigned char i = 0 ; i < 4 ; i + + )
2012-11-12 14:49:40 +00:00
{
2011-11-15 21:50:43 +00:00
current_speed [ i ] * = speed_factor ;
}
block - > nominal_speed * = speed_factor ;
block - > nominal_rate * = speed_factor ;
}
// Compute and limit the acceleration rate for the trapezoid generator.
float steps_per_mm = block - > step_event_count / block - > millimeters ;
2012-11-21 19:17:44 +00:00
if ( block - > steps_x = = 0 & & block - > steps_y = = 0 & & block - > steps_z = = 0 )
2012-11-12 14:49:40 +00:00
{
2011-11-15 21:50:43 +00:00
block - > acceleration_st = ceil ( retract_acceleration * steps_per_mm ) ; // convert to: acceleration steps/sec^2
}
2012-11-21 19:17:44 +00:00
else
2012-11-12 14:49:40 +00:00
{
2011-11-15 21:50:43 +00:00
block - > acceleration_st = ceil ( acceleration * steps_per_mm ) ; // convert to: acceleration steps/sec^2
// Limit acceleration per axis
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 ] )
block - > acceleration_st = axis_steps_per_sqr_second [ Y_AXIS ] ;
if ( ( ( float ) block - > acceleration_st * ( float ) block - > steps_e / ( float ) block - > step_event_count ) > axis_steps_per_sqr_second [ E_AXIS ] )
block - > acceleration_st = axis_steps_per_sqr_second [ E_AXIS ] ;
if ( ( ( float ) block - > acceleration_st * ( float ) block - > steps_z / ( float ) block - > step_event_count ) > axis_steps_per_sqr_second [ Z_AXIS ] )
block - > acceleration_st = axis_steps_per_sqr_second [ Z_AXIS ] ;
}
block - > acceleration = block - > acceleration_st / steps_per_mm ;
2013-06-09 10:52:49 +00:00
block - > acceleration_rate = ( long ) ( ( float ) block - > acceleration_st * ( 16777216.0 / ( F_CPU / 8.0 ) ) ) ;
2012-06-11 15:33:42 +00:00
2011-11-15 21:50:43 +00:00
#if 0 // Use old jerk for now
// Compute path unit vector
double unit_vec [ 3 ] ;
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 ;
2012-06-11 15:33:42 +00:00
2011-11-15 21:50:43 +00:00
// 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,
// colinear with the circle center. The circular segment joining the two paths represents the
// path of centripetal acceleration. Solve for max velocity based on max acceleration about the
// radius of the circle, defined indirectly by junction deviation. This may be also viewed as
// path width or max_jerk in the previous grbl version. This approach does not actually deviate
// from path, but used as a robust way to compute cornering speeds, as it takes into account the
// nonlinearities of both the junction angle and junction velocity.
double vmax_junction = MINIMUM_PLANNER_SPEED ; // Set default max junction speed
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
if ( ( block_buffer_head ! = block_buffer_tail ) & & ( previous_nominal_speed > 0.0 ) ) {
// 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 ]
2012-06-11 15:33:42 +00:00
- previous_unit_vec [ Y_AXIS ] * unit_vec [ Y_AXIS ]
- previous_unit_vec [ Z_AXIS ] * unit_vec [ Z_AXIS ] ;
2011-11-15 21:50:43 +00:00
// 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 ) ;
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
if ( cos_theta > - 0.95 ) {
// 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 ,
2012-06-11 15:33:42 +00:00
sqrt ( block - > acceleration * junction_deviation * sin_theta_d2 / ( 1.0 - sin_theta_d2 ) ) ) ;
2011-11-15 21:50:43 +00:00
}
}
}
# endif
// Start with a safe speed
2012-06-11 15:33:42 +00:00
float vmax_junction = max_xy_jerk / 2 ;
float vmax_junction_factor = 1.0 ;
2012-05-16 16:17:34 +00:00
if ( fabs ( current_speed [ Z_AXIS ] ) > max_z_jerk / 2 )
2012-06-11 15:33:42 +00:00
vmax_junction = min ( vmax_junction , max_z_jerk / 2 ) ;
2012-05-16 16:17:34 +00:00
if ( fabs ( current_speed [ E_AXIS ] ) > max_e_jerk / 2 )
2012-03-04 22:09:13 +00:00
vmax_junction = min ( vmax_junction , max_e_jerk / 2 ) ;
2012-06-11 15:33:42 +00:00
vmax_junction = min ( vmax_junction , block - > nominal_speed ) ;
float safe_speed = vmax_junction ;
2012-04-01 14:23:40 +00:00
if ( ( moves_queued > 1 ) & & ( previous_nominal_speed > 0.0001 ) ) {
2011-11-15 21:50:43 +00:00
float jerk = sqrt ( pow ( ( current_speed [ X_AXIS ] - previous_speed [ X_AXIS ] ) , 2 ) + pow ( ( current_speed [ Y_AXIS ] - previous_speed [ Y_AXIS ] ) , 2 ) ) ;
2012-06-11 15:33:42 +00:00
// if((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
vmax_junction = block - > nominal_speed ;
// }
2011-11-15 21:50:43 +00:00
if ( jerk > max_xy_jerk ) {
2012-06-11 15:33:42 +00:00
vmax_junction_factor = ( max_xy_jerk / jerk ) ;
2011-11-15 21:50:43 +00:00
}
2012-05-16 16:17:34 +00:00
if ( fabs ( current_speed [ Z_AXIS ] - previous_speed [ Z_AXIS ] ) > max_z_jerk ) {
2012-06-11 15:33:42 +00:00
vmax_junction_factor = min ( vmax_junction_factor , ( max_z_jerk / fabs ( current_speed [ Z_AXIS ] - previous_speed [ Z_AXIS ] ) ) ) ;
2011-11-15 21:50:43 +00:00
}
2012-05-16 16:17:34 +00:00
if ( fabs ( current_speed [ E_AXIS ] - previous_speed [ E_AXIS ] ) > max_e_jerk ) {
2012-06-11 15:33:42 +00:00
vmax_junction_factor = min ( vmax_junction_factor , ( max_e_jerk / fabs ( current_speed [ E_AXIS ] - previous_speed [ E_AXIS ] ) ) ) ;
2012-03-04 15:34:58 +00:00
}
2012-06-11 15:33:42 +00:00
vmax_junction = min ( previous_nominal_speed , vmax_junction * vmax_junction_factor ) ; // Limit speed to max previous speed
2011-11-15 21:50:43 +00:00
}
block - > max_entry_speed = vmax_junction ;
2012-06-11 15:33:42 +00:00
2011-11-15 21:50:43 +00:00
// 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 ) ;
// Initialize planner efficiency flags
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
// If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
// the current block and next block junction speeds are guaranteed to always be at their maximum
// junction speeds in deceleration and acceleration, respectively. This is due to how the current
// 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.
2012-06-11 15:33:42 +00:00
if ( block - > nominal_speed < = v_allowable ) {
block - > nominal_length_flag = true ;
}
else {
block - > nominal_length_flag = false ;
}
2011-11-15 21:50:43 +00:00
block - > recalculate_flag = true ; // Always calculate trapezoid for new block
2012-06-11 15:33:42 +00:00
2011-11-15 21:50:43 +00:00
// 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 ;
2012-04-01 14:23:40 +00:00
2012-06-11 15:33:42 +00:00
# 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 ) {
2011-11-15 21:50:43 +00:00
block - > advance_rate = 0 ;
2012-06-11 15:33:42 +00:00
}
2011-11-15 21:50:43 +00:00
else {
2012-06-11 15:33:42 +00:00
block - > advance_rate = advance / ( float ) acc_dist ;
2011-11-15 21:50:43 +00:00
}
2012-06-11 15:33:42 +00:00
}
/*
2011-12-04 18:54:07 +00:00
SERIAL_ECHO_START ;
2012-06-11 15:33:42 +00:00
SERIAL_ECHOPGM ( " advance : " ) ;
SERIAL_ECHO ( block - > advance / 256.0 ) ;
SERIAL_ECHOPGM ( " advance rate : " ) ;
SERIAL_ECHOLN ( block - > advance_rate / 256.0 ) ;
*/
# endif // ADVANCE
2011-11-15 21:50:43 +00:00
calculate_trapezoid_for_block ( block , block - > entry_speed / block - > nominal_speed ,
2012-06-11 15:33:42 +00:00
safe_speed / block - > nominal_speed ) ;
2011-11-15 21:50:43 +00:00
// Move buffer head
block_buffer_head = next_buffer_head ;
2012-06-11 15:33:42 +00:00
2011-11-15 21:50:43 +00:00
// Update position
memcpy ( position , target , sizeof ( target ) ) ; // position[] = target[]
planner_recalculate ( ) ;
2012-04-15 17:17:33 +00:00
2011-11-15 21:50:43 +00:00
st_wake_up ( ) ;
}
void plan_set_position ( const float & x , const float & y , const float & z , const float & e )
{
position [ X_AXIS ] = lround ( x * axis_steps_per_unit [ X_AXIS ] ) ;
position [ Y_AXIS ] = lround ( y * axis_steps_per_unit [ Y_AXIS ] ) ;
position [ Z_AXIS ] = lround ( z * axis_steps_per_unit [ Z_AXIS ] ) ;
position [ E_AXIS ] = lround ( e * axis_steps_per_unit [ E_AXIS ] ) ;
2011-11-20 13:50:08 +00:00
st_set_position ( position [ X_AXIS ] , position [ Y_AXIS ] , position [ Z_AXIS ] , position [ E_AXIS ] ) ;
2011-11-15 21:50:43 +00:00
previous_nominal_speed = 0.0 ; // Resets planner junction speeds. Assumes start from rest.
previous_speed [ 0 ] = 0.0 ;
previous_speed [ 1 ] = 0.0 ;
previous_speed [ 2 ] = 0.0 ;
previous_speed [ 3 ] = 0.0 ;
}
2011-11-25 12:43:06 +00:00
void plan_set_e_position ( const float & e )
{
position [ E_AXIS ] = lround ( e * axis_steps_per_unit [ E_AXIS ] ) ;
st_set_e_position ( position [ E_AXIS ] ) ;
}
2011-11-20 15:05:42 +00:00
uint8_t movesplanned ( )
{
2012-06-11 15:33:42 +00:00
return ( block_buffer_head - block_buffer_tail + BLOCK_BUFFER_SIZE ) & ( BLOCK_BUFFER_SIZE - 1 ) ;
2011-11-20 15:05:42 +00:00
}
2011-11-25 12:43:06 +00:00
2012-06-11 15:33:42 +00:00
# ifdef PREVENT_DANGEROUS_EXTRUDE
2013-06-02 15:36:23 +00:00
void set_extrude_min_temp ( float temp )
{
extrude_min_temp = temp ;
2011-12-12 19:35:46 +00:00
}
2013-06-02 15:36:23 +00:00
# endif
2012-06-11 15:33:42 +00:00
Allow Edit menu to call fn after edit; Fix PID Ki and Kd display in menus; Actually use changed PID and Max Accel values
Add new 'callback' edit-menu types that call a function after the edit is done. Use this to display and edit Ki and Kd correctly (removing the scaling first and reapplying it after). Also use it to reset maximum stepwise acceleration rates, after updating mm/s^2 rates via menus. (Previously, changes did nothing to affect planner unless saved back to EEPROM, and the machine reset).
Add calls to updatePID() so that PID loop uses updated values whether set by gcode (it already did this), or by restoring defaults, or loading from EEPROM (it didn't do those last two). Similarly, update the maximum step/s^2 accel rates when the mm/s^2 values are changed - whether by menu edits, restore defaults, or EEPROM read.
Refactor the acceleration rate update logic, and the PID scaling logic, into new functions that can be called from wherever, including the callbacks.
Add menu items to allow the z jerk and e jerk to be viewed/edited in the Control->Motion menu, as per xy jerk.
Conflicts:
Marlin/language.h
2013-03-19 14:05:11 +00:00
// Calculate the steps/s^2 acceleration rates, based on the mm/s^s
void reset_acceleration_rates ( )
{
for ( int8_t i = 0 ; i < NUM_AXIS ; i + + )
{
axis_steps_per_sqr_second [ i ] = max_acceleration_units_per_sq_second [ i ] * axis_steps_per_unit [ i ] ;
}
}