2016-03-25 07:19:46 +01:00
/**
2016-03-24 19:01:20 +01:00
* Marlin 3 D Printer Firmware
* Copyright ( C ) 2016 MarlinFirmware [ https : //github.com/MarlinFirmware/Marlin]
*
* Based on Sprinter and grbl .
* Copyright ( C ) 2011 Camiel Gubbels / Erik van der Zalm
*
* This program 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 .
*
* This program 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 this program . If not , see < http : //www.gnu.org/licenses/>.
*
*/
2015-04-27 03:44:01 +02:00
/**
* planner . cpp - Buffer movement commands and manage the acceleration profile plan
* Part of Grbl
2015-08-11 18:38:26 +02:00
*
2015-04-27 03:44:01 +02:00
* 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/>.
*
*
* 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
*
* 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 destination 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 )
*
2012-06-11 17:33:42 +02:00
*/
2011-11-15 22:50:43 +01:00
2011-12-06 05:33:33 +01:00
# include "Marlin.h"
2011-11-15 22:50:43 +01:00
# include "planner.h"
# include "stepper.h"
# include "temperature.h"
# include "ultralcd.h"
2012-03-03 16:51:47 +01:00
# include "language.h"
2011-11-15 22:50:43 +01:00
2015-07-31 07:30:29 +02:00
# if ENABLED(MESH_BED_LEVELING)
2015-03-15 10:43:26 +01:00
# include "mesh_bed_leveling.h"
2015-04-13 03:07:08 +02:00
# endif
2015-03-15 10:43:26 +01:00
2011-11-15 22:50:43 +01:00
//===========================================================================
2015-03-14 12:28:22 +01:00
//============================= public variables ============================
2011-11-15 22:50:43 +01:00
//===========================================================================
2015-04-13 03:07:08 +02:00
millis_t minsegmenttime ;
2015-04-04 06:58:48 +02:00
float max_feedrate [ NUM_AXIS ] ; // Max speeds in mm per minute
2014-12-18 17:13:08 +01:00
float axis_steps_per_unit [ NUM_AXIS ] ;
unsigned long max_acceleration_units_per_sq_second [ NUM_AXIS ] ; // Use M201 to override by software
2011-11-15 22:50:43 +01:00
float minimumfeedrate ;
2015-04-27 03:44:01 +02:00
float acceleration ; // Normal acceleration mm/s^2 DEFAULT ACCELERATION for all printing moves. M204 SXXXX
float retract_acceleration ; // Retract acceleration mm/s^2 filament pull-back and push-forward while standing still in the other axes M204 TXXXX
float travel_acceleration ; // Travel acceleration mm/s^2 DEFAULT ACCELERATION for all NON printing moves. M204 MXXXX
float max_xy_jerk ; // The largest speed change requiring no acceleration
2011-11-15 22:50:43 +01:00
float max_z_jerk ;
2012-03-04 16:34:58 +01:00
float max_e_jerk ;
2011-11-15 22:50:43 +01:00
float mintravelfeedrate ;
unsigned long axis_steps_per_sqr_second [ NUM_AXIS ] ;
2015-08-05 14:12:26 +02:00
# if ENABLED(AUTO_BED_LEVELING_FEATURE)
2015-04-27 03:44:01 +02:00
// Transform required to compensate for bed level
2015-03-21 04:42:49 +01:00
matrix_3x3 plan_bed_level_matrix = {
1.0 , 0.0 , 0.0 ,
0.0 , 1.0 , 0.0 ,
0.0 , 0.0 , 1.0
} ;
2015-08-05 14:12:26 +02:00
# endif // AUTO_BED_LEVELING_FEATURE
2013-09-29 18:20:06 +02:00
2015-07-31 07:30:29 +02:00
# if ENABLED(AUTOTEMP)
2015-03-21 04:42:49 +01:00
float autotemp_max = 250 ;
float autotemp_min = 210 ;
float autotemp_factor = 0.1 ;
bool autotemp_enabled = false ;
2011-11-15 22:50:43 +01:00
# endif
2016-03-06 03:27:45 +01:00
# if ENABLED(FAN_SOFT_PWM)
extern unsigned char fanSpeedSoftPwm [ FAN_COUNT ] ;
# endif
2011-11-27 14:57:12 +01:00
//===========================================================================
2015-04-27 03:44:01 +02:00
//============ semi-private variables, used in inline functions =============
2011-11-27 14:57:12 +01:00
//===========================================================================
2015-04-27 03:44:01 +02:00
2011-11-27 14:57:12 +01:00
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 22:50:43 +01:00
//===========================================================================
2015-04-27 03:44:01 +02:00
//============================ private variables ============================
2011-11-15 22:50:43 +01:00
//===========================================================================
2015-04-27 03:44:01 +02:00
// The current position of the tool in absolute steps
long position [ NUM_AXIS ] ; // Rescaled from extern when axis_steps_per_unit are changed by gcode
static float previous_speed [ NUM_AXIS ] ; // Speed of previous path line segment
static float previous_nominal_speed ; // Nominal speed of previous path line segment
2015-09-30 11:20:31 +02:00
uint8_t g_uc_extruder_last_move [ EXTRUDERS ] = { 0 } ;
2015-04-27 03:44:01 +02:00
2011-12-04 12:40:18 +01:00
# ifdef XY_FREQUENCY_LIMIT
2015-03-21 04:42:49 +01:00
// Used for the frequency limit
# define MAX_FREQ_TIME (1000000.0 / XY_FREQUENCY_LIMIT)
// Old direction bits. Used for speed calculations
static unsigned char old_direction_bits = 0 ;
// Segment times (in µs). Used for speed calculations
2015-10-03 08:08:58 +02:00
static long axis_segment_time [ 2 ] [ 3 ] = { { MAX_FREQ_TIME + 1 , 0 , 0 } , { MAX_FREQ_TIME + 1 , 0 , 0 } } ;
2011-12-04 12:40:18 +01:00
# endif
2011-11-15 22:50:43 +01:00
2015-09-30 11:20:31 +02:00
# if ENABLED(DUAL_X_CARRIAGE)
extern bool extruder_duplication_enabled ;
# endif
2015-04-27 03:44:01 +02:00
//===========================================================================
//================================ functions ================================
//===========================================================================
2015-03-21 04:42:49 +01:00
// Get the next / previous index of the next block in the ring buffer
// NOTE: Using & here (not %) because BLOCK_BUFFER_SIZE is always a power of 2
FORCE_INLINE int8_t next_block_index ( int8_t block_index ) { return BLOCK_MOD ( block_index + 1 ) ; }
FORCE_INLINE int8_t prev_block_index ( int8_t block_index ) { return BLOCK_MOD ( block_index - 1 ) ; }
2011-11-15 22:50:43 +01:00
2015-08-11 18:38:26 +02:00
// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
2011-11-15 22:50:43 +01:00
// given acceleration:
2015-03-21 04:42:49 +01:00
FORCE_INLINE float estimate_acceleration_distance ( float initial_rate , float target_rate , float acceleration ) {
if ( acceleration = = 0 ) return 0 ; // acceleration was 0, set acceleration distance to 0
return ( target_rate * target_rate - initial_rate * initial_rate ) / ( acceleration * 2 ) ;
2011-11-15 22:50:43 +01:00
}
2015-08-11 18:38:26 +02:00
// This function gives you the point at which you must start braking (at the rate of -acceleration) if
2011-11-15 22:50:43 +01:00
// 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)
2015-03-21 04:42:49 +01:00
FORCE_INLINE float intersection_distance ( float initial_rate , float final_rate , float acceleration , float distance ) {
if ( acceleration = = 0 ) return 0 ; // acceleration was 0, set intersection distance to 0
return ( acceleration * 2 * distance - initial_rate * initial_rate + final_rate * final_rate ) / ( acceleration * 4 ) ;
2011-11-15 22:50:43 +01:00
}
// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
2015-10-03 08:08:58 +02:00
void calculate_trapezoid_for_block ( block_t * block , float entry_factor , float exit_factor ) {
2015-03-21 04:42:49 +01:00
unsigned long initial_rate = ceil ( block - > nominal_rate * entry_factor ) ; // (step/min)
unsigned long final_rate = ceil ( block - > nominal_rate * exit_factor ) ; // (step/min)
2011-11-15 22:50:43 +01:00
// Limit minimal step rate (Otherwise the timer will overflow.)
2015-04-13 03:07:08 +02:00
NOLESS ( initial_rate , 120 ) ;
NOLESS ( final_rate , 120 ) ;
2012-06-11 17:33:42 +02:00
2011-11-15 22:50:43 +01:00
long acceleration = block - > acceleration_st ;
2015-03-21 04:42:49 +01:00
int32_t accelerate_steps = ceil ( estimate_acceleration_distance ( initial_rate , block - > nominal_rate , acceleration ) ) ;
int32_t decelerate_steps = floor ( estimate_acceleration_distance ( block - > nominal_rate , final_rate , - acceleration ) ) ;
2012-06-11 17:33:42 +02:00
2011-11-15 22:50:43 +01:00
// Calculate the size of Plateau of Nominal Rate.
2015-03-21 04:42:49 +01:00
int32_t plateau_steps = block - > step_event_count - accelerate_steps - decelerate_steps ;
2012-06-11 17:33:42 +02:00
2011-11-15 22:50:43 +01: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 ) {
2013-12-16 11:40:23 +01:00
accelerate_steps = ceil ( intersection_distance ( initial_rate , final_rate , acceleration , block - > step_event_count ) ) ;
2015-03-21 04:42:49 +01:00
accelerate_steps = max ( accelerate_steps , 0 ) ; // Check limits due to numerical round-off
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 22:50:43 +01:00
plateau_steps = 0 ;
}
2015-10-03 08:08:58 +02:00
# if ENABLED(ADVANCE)
volatile long initial_advance = block - > advance * entry_factor * entry_factor ;
volatile long final_advance = block - > advance * exit_factor * exit_factor ;
# endif // ADVANCE
2012-06-11 17:33:42 +02:00
// block->accelerate_until = accelerate_steps;
// block->decelerate_after = accelerate_steps+plateau_steps;
2011-11-15 22:50:43 +01:00
CRITICAL_SECTION_START ; // Fill variables used by the stepper in a critical section
2015-03-21 04:42:49 +01:00
if ( ! block - > busy ) { // Don't update variables if block is busy.
2011-11-15 22:50:43 +01:00
block - > accelerate_until = accelerate_steps ;
2015-10-03 08:08:58 +02:00
block - > decelerate_after = accelerate_steps + plateau_steps ;
2011-11-15 22:50:43 +01:00
block - > initial_rate = initial_rate ;
block - > final_rate = final_rate ;
2015-07-31 07:30:29 +02:00
# if ENABLED(ADVANCE)
2015-03-21 04:42:49 +01:00
block - > initial_advance = initial_advance ;
block - > final_advance = final_advance ;
# endif
2011-11-15 22:50:43 +01:00
}
CRITICAL_SECTION_END ;
2015-08-11 18:38:26 +02:00
}
2011-11-15 22:50:43 +01:00
2015-08-11 18:38:26 +02:00
// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
2011-11-15 22:50:43 +01:00
// acceleration within the allotted distance.
2011-11-27 16:04:58 +01:00
FORCE_INLINE float max_allowable_speed ( float acceleration , float target_velocity , float distance ) {
2015-03-21 04:42:49 +01:00
return sqrt ( target_velocity * target_velocity - 2 * acceleration * distance ) ;
2011-11-15 22:50:43 +01:00
}
// "Junction jerk" in this context is the immediate change in speed at the junction of two blocks.
2015-08-11 18:38:26 +02:00
// This method will calculate the junction jerk as the euclidean distance between the nominal
2011-11-15 22:50:43 +01:00
// 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.
2015-10-03 08:08:58 +02:00
void planner_reverse_pass_kernel ( block_t * previous , block_t * current , block_t * next ) {
2015-03-21 04:42:49 +01:00
if ( ! current ) return ;
2015-08-11 18:38:26 +02:00
UNUSED ( previous ) ;
2012-06-11 17:33:42 +02:00
if ( next ) {
2011-11-15 22:50:43 +01: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.
2015-12-10 14:05:38 +01:00
float max_entry_speed = current - > max_entry_speed ;
if ( current - > entry_speed ! = max_entry_speed ) {
2012-06-11 17:33:42 +02:00
2011-11-15 22:50:43 +01: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.
2015-12-10 14:05:38 +01:00
if ( ! current - > nominal_length_flag & & max_entry_speed > next - > entry_speed ) {
current - > entry_speed = min ( max_entry_speed ,
2015-10-03 08:08:58 +02:00
max_allowable_speed ( - current - > acceleration , next - > entry_speed , current - > millimeters ) ) ;
2015-08-11 18:38:26 +02:00
}
2012-06-11 17:33:42 +02:00
else {
2015-12-10 14:05:38 +01:00
current - > entry_speed = max_entry_speed ;
2011-11-15 22:50:43 +01:00
}
current - > recalculate_flag = true ;
2012-06-11 17:33:42 +02:00
2011-11-15 22:50:43 +01:00
}
} // Skip last block. Already initialized and set for recalculation.
}
2015-08-11 18:38:26 +02:00
// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
2011-11-15 22:50:43 +01:00
// implements the reverse pass.
void planner_reverse_pass ( ) {
2011-12-04 12:40:18 +01:00
uint8_t block_index = block_buffer_head ;
2015-08-11 18:38:26 +02:00
2012-06-11 17:33:42 +02:00
//Make a local copy of block_buffer_tail, because the interrupt can alter it
CRITICAL_SECTION_START ;
2015-03-21 04:42:49 +01:00
unsigned char tail = block_buffer_tail ;
2012-06-11 17:33:42 +02:00
CRITICAL_SECTION_END
2015-08-11 18:38:26 +02:00
2015-03-21 04:42:49 +01:00
if ( BLOCK_MOD ( block_buffer_head - tail + BLOCK_BUFFER_SIZE ) > 3 ) { // moves queued
block_index = BLOCK_MOD ( block_buffer_head - 3 ) ;
2015-10-03 08:08:58 +02:00
block_t * block [ 3 ] = { NULL , NULL , NULL } ;
2015-03-21 04:42:49 +01:00
while ( block_index ! = tail ) {
block_index = prev_block_index ( block_index ) ;
2015-10-03 08:08:58 +02:00
block [ 2 ] = block [ 1 ] ;
block [ 1 ] = block [ 0 ] ;
2011-11-15 22:50:43 +01:00
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.
2015-10-03 08:08:58 +02:00
void planner_forward_pass_kernel ( block_t * previous , block_t * current , block_t * next ) {
2015-03-21 04:42:49 +01:00
if ( ! previous ) return ;
2015-08-11 18:38:26 +02:00
UNUSED ( next ) ;
2012-06-11 17:33:42 +02:00
2011-11-15 22:50:43 +01: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 ) {
2015-03-21 04:42:49 +01:00
double entry_speed = min ( current - > entry_speed ,
2015-10-03 08:08:58 +02:00
max_allowable_speed ( - previous - > acceleration , previous - > entry_speed , previous - > millimeters ) ) ;
2011-11-15 22:50:43 +01:00
// Check for junction speed change
if ( current - > entry_speed ! = entry_speed ) {
current - > entry_speed = entry_speed ;
current - > recalculate_flag = true ;
}
}
}
}
2015-03-21 04:42:49 +01:00
// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
2011-11-15 22:50:43 +01:00
// implements the forward pass.
void planner_forward_pass ( ) {
2011-12-04 12:40:18 +01:00
uint8_t block_index = block_buffer_tail ;
2015-10-03 08:08:58 +02:00
block_t * block [ 3 ] = { NULL , NULL , NULL } ;
2011-11-15 22:50:43 +01:00
2015-03-21 04:42:49 +01:00
while ( block_index ! = block_buffer_head ) {
2011-11-15 22:50:43 +01:00
block [ 0 ] = block [ 1 ] ;
block [ 1 ] = block [ 2 ] ;
block [ 2 ] = & block_buffer [ block_index ] ;
2015-03-21 04:42:49 +01:00
planner_forward_pass_kernel ( block [ 0 ] , block [ 1 ] , block [ 2 ] ) ;
2011-11-15 22:50:43 +01:00
block_index = next_block_index ( block_index ) ;
}
planner_forward_pass_kernel ( block [ 1 ] , block [ 2 ] , NULL ) ;
}
2015-08-11 18:38:26 +02:00
// 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
2011-11-15 22:50:43 +01:00
// updating the blocks.
void planner_recalculate_trapezoids ( ) {
int8_t block_index = block_buffer_tail ;
2015-10-03 08:08:58 +02:00
block_t * current ;
block_t * next = NULL ;
2012-06-11 17:33:42 +02:00
2015-03-21 04:42:49 +01:00
while ( block_index ! = block_buffer_head ) {
2011-11-15 22:50:43 +01:00
c urrent = 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.
2015-03-21 04:57:06 +01:00
float nom = current - > nominal_speed ;
calculate_trapezoid_for_block ( current , current - > entry_speed / nom , next - > entry_speed / nom ) ;
2011-11-15 22:50:43 +01:00
current - > recalculate_flag = false ; // Reset current only to ensure next trapezoid is computed
}
}
2015-10-03 08:08:58 +02:00
block_index = next_block_index ( block_index ) ;
2011-11-15 22:50:43 +01:00
}
// Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
2015-03-21 04:42:49 +01:00
if ( next ) {
2015-03-21 04:57:06 +01:00
float nom = next - > nominal_speed ;
2016-03-15 09:10:57 +01:00
calculate_trapezoid_for_block ( next , next - > entry_speed / nom , ( MINIMUM_PLANNER_SPEED ) / nom ) ;
2011-11-15 22:50:43 +01:00
next - > recalculate_flag = false ;
}
}
// Recalculates the motion plan according to the following algorithm:
//
2015-08-11 18:38:26 +02:00
// 1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_factor)
2011-11-15 22:50:43 +01:00
// so that:
// a. The junction jerk is within the set limit
2015-08-11 18:38:26 +02:00
// b. No speed reduction within one block requires faster deceleration than the one, true constant
2011-11-15 22:50:43 +01:00
// acceleration.
2015-08-11 18:38:26 +02:00
// 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 acceleration than the one, true
2011-11-15 22:50:43 +01:00
// constant acceleration.
//
2015-08-11 18:38:26 +02:00
// 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
2011-11-15 22:50:43 +01:00
// the set limit. Finally it will:
//
// 3. Recalculate trapezoids for all blocks.
2015-08-11 18:38:26 +02:00
void planner_recalculate ( ) {
2011-11-15 22:50:43 +01:00
planner_reverse_pass ( ) ;
planner_forward_pass ( ) ;
planner_recalculate_trapezoids ( ) ;
}
void plan_init ( ) {
2015-03-21 04:42:49 +01:00
block_buffer_head = block_buffer_tail = 0 ;
2011-11-15 22:50:43 +01:00
memset ( position , 0 , sizeof ( position ) ) ; // clear position
2015-10-03 08:08:58 +02:00
for ( int i = 0 ; i < NUM_AXIS ; i + + ) previous_speed [ i ] = 0.0 ;
2011-11-15 22:50:43 +01:00
previous_nominal_speed = 0.0 ;
}
2015-07-31 07:30:29 +02:00
# if ENABLED(AUTOTEMP)
2015-03-21 04:42:49 +01:00
void getHighESpeed ( ) {
static float oldt = 0 ;
2012-06-11 17:33:42 +02:00
2015-03-21 04:42:49 +01:00
if ( ! autotemp_enabled ) return ;
if ( degTargetHotend0 ( ) + 2 < autotemp_min ) return ; // probably temperature set to zero.
float high = 0.0 ;
uint8_t block_index = block_buffer_tail ;
2012-06-11 17:33:42 +02:00
2015-03-21 04:42:49 +01:00
while ( block_index ! = block_buffer_head ) {
2015-10-03 08:08:58 +02:00
block_t * block = & block_buffer [ block_index ] ;
2015-03-21 04:57:06 +01:00
if ( block - > steps [ X_AXIS ] | | block - > steps [ Y_AXIS ] | | block - > steps [ Z_AXIS ] ) {
float se = ( float ) block - > steps [ E_AXIS ] / block - > step_event_count * block - > nominal_speed ; // mm/sec;
2016-02-22 07:17:32 +01:00
NOLESS ( high , se ) ;
2012-04-15 19:17:33 +02:00
}
2015-03-21 04:42:49 +01:00
block_index = next_block_index ( block_index ) ;
2011-11-15 22:50:43 +01:00
}
2012-06-11 17:33:42 +02:00
2015-03-21 04:42:49 +01:00
float t = autotemp_min + high * autotemp_factor ;
2015-04-13 03:07:08 +02:00
t = constrain ( t , autotemp_min , autotemp_max ) ;
if ( oldt > t ) {
2016-03-15 09:10:57 +01:00
t * = ( 1 - ( AUTOTEMP_OLDWEIGHT ) ) ;
t + = ( AUTOTEMP_OLDWEIGHT ) * oldt ;
2015-04-13 03:07:08 +02:00
}
2015-03-21 04:42:49 +01:00
oldt = t ;
setTargetHotend0 ( t ) ;
2011-11-19 17:07:44 +01:00
}
2015-10-03 08:08:58 +02:00
# endif //AUTOTEMP
2011-11-15 22:50:43 +01:00
2015-03-21 04:42:49 +01:00
void check_axes_activity ( ) {
2015-04-14 12:41:37 +02:00
unsigned char axis_active [ NUM_AXIS ] = { 0 } ,
2016-03-06 03:27:45 +01:00
tail_fan_speed [ FAN_COUNT ] ;
# if FAN_COUNT > 0
for ( uint8_t i = 0 ; i < FAN_COUNT ; i + + ) tail_fan_speed [ i ] = fanSpeeds [ i ] ;
# endif
2015-07-31 07:30:29 +02:00
# if ENABLED(BARICUDA)
2015-03-21 04:42:49 +01:00
unsigned char tail_valve_pressure = ValvePressure ,
tail_e_to_p_pressure = EtoPPressure ;
2013-05-14 23:56:32 +02:00
# endif
2015-03-21 04:57:06 +01:00
2015-10-03 08:08:58 +02:00
block_t * block ;
2011-11-15 22:50:43 +01:00
2015-03-21 04:42:49 +01:00
if ( blocks_queued ( ) ) {
2016-03-06 03:27:45 +01:00
2011-12-04 12:40:18 +01:00
uint8_t block_index = block_buffer_tail ;
2016-03-06 03:27:45 +01:00
# if FAN_COUNT > 0
for ( uint8_t i = 0 ; i < FAN_COUNT ; i + + ) tail_fan_speed [ i ] = block_buffer [ block_index ] . fan_speed [ i ] ;
# endif
2015-07-31 07:30:29 +02:00
# if ENABLED(BARICUDA)
2015-03-21 04:57:06 +01:00
block = & block_buffer [ block_index ] ;
tail_valve_pressure = block - > valve_pressure ;
tail_e_to_p_pressure = block - > e_to_p_pressure ;
2013-05-14 23:56:32 +02:00
# endif
2016-03-06 03:27:45 +01:00
2015-03-21 04:42:49 +01:00
while ( block_index ! = block_buffer_head ) {
2011-11-15 22:50:43 +01:00
block = & block_buffer [ block_index ] ;
2015-10-03 08:08:58 +02:00
for ( int i = 0 ; i < NUM_AXIS ; i + + ) if ( block - > steps [ i ] ) axis_active [ i ] + + ;
2015-03-21 04:42:49 +01:00
block_index = next_block_index ( block_index ) ;
2011-11-15 22:50:43 +01:00
}
}
2016-03-14 06:15:45 +01:00
# if ENABLED(DISABLE_X)
if ( ! axis_active [ X_AXIS ] ) disable_x ( ) ;
# endif
# if ENABLED(DISABLE_Y)
if ( ! axis_active [ Y_AXIS ] ) disable_y ( ) ;
# endif
# if ENABLED(DISABLE_Z)
if ( ! axis_active [ Z_AXIS ] ) disable_z ( ) ;
# endif
# if ENABLED(DISABLE_E)
if ( ! axis_active [ E_AXIS ] ) {
disable_e0 ( ) ;
disable_e1 ( ) ;
disable_e2 ( ) ;
disable_e3 ( ) ;
}
# endif
2013-05-14 22:25:53 +02:00
2016-03-06 03:27:45 +01:00
# if FAN_COUNT > 0
# if defined(FAN_MIN_PWM)
# define CALC_FAN_SPEED(f) (tail_fan_speed[f] ? ( FAN_MIN_PWM + (tail_fan_speed[f] * (255 - FAN_MIN_PWM)) / 255 ) : 0)
# else
# define CALC_FAN_SPEED(f) tail_fan_speed[f]
# endif
2015-03-21 04:42:49 +01:00
# ifdef FAN_KICKSTART_TIME
2016-03-06 03:27:45 +01:00
2016-03-19 10:41:43 +01:00
static millis_t fan_kick_end [ FAN_COUNT ] = { 0 } ;
2016-03-06 03:27:45 +01:00
# define KICKSTART_FAN(f) \
if ( tail_fan_speed [ f ] ) { \
2016-03-19 10:41:43 +01:00
millis_t ms = millis ( ) ; \
2016-03-06 03:27:45 +01:00
if ( fan_kick_end [ f ] = = 0 ) { \
fan_kick_end [ f ] = ms + FAN_KICKSTART_TIME ; \
tail_fan_speed [ f ] = 255 ; \
2016-03-19 10:41:43 +01:00
} else { \
if ( fan_kick_end [ f ] > ms ) { \
tail_fan_speed [ f ] = 255 ; \
} \
2016-03-06 03:27:45 +01:00
} \
2016-03-19 10:41:43 +01:00
} else { \
fan_kick_end [ f ] = 0 ; \
2015-10-13 12:51:34 +02:00
}
2016-03-06 03:27:45 +01:00
# if HAS_FAN0
KICKSTART_FAN ( 0 ) ;
# endif
# if HAS_FAN1
KICKSTART_FAN ( 1 ) ;
# endif
# if HAS_FAN2
KICKSTART_FAN ( 2 ) ;
# endif
2015-05-18 22:26:17 +02:00
# endif //FAN_KICKSTART_TIME
2016-03-06 03:27:45 +01:00
2015-07-31 07:30:29 +02:00
# if ENABLED(FAN_SOFT_PWM)
2016-03-06 03:27:45 +01:00
# if HAS_FAN0
fanSpeedSoftPwm [ 0 ] = CALC_FAN_SPEED ( 0 ) ;
# endif
# if HAS_FAN1
fanSpeedSoftPwm [ 1 ] = CALC_FAN_SPEED ( 1 ) ;
# endif
# if HAS_FAN2
fanSpeedSoftPwm [ 2 ] = CALC_FAN_SPEED ( 2 ) ;
# endif
2015-03-21 04:42:49 +01:00
# else
2016-03-06 03:27:45 +01:00
# if HAS_FAN0
analogWrite ( FAN_PIN , CALC_FAN_SPEED ( 0 ) ) ;
# endif
# if HAS_FAN1
analogWrite ( FAN1_PIN , CALC_FAN_SPEED ( 1 ) ) ;
# endif
# if HAS_FAN2
analogWrite ( FAN2_PIN , CALC_FAN_SPEED ( 2 ) ) ;
# endif
# endif
# endif // FAN_COUNT > 0
2015-03-21 04:42:49 +01:00
2015-07-31 07:30:29 +02:00
# if ENABLED(AUTOTEMP)
2015-03-21 04:42:49 +01:00
getHighESpeed ( ) ;
2013-05-14 23:56:32 +02:00
# endif
2013-05-14 23:16:10 +02:00
2015-07-31 07:30:29 +02:00
# if ENABLED(BARICUDA)
2015-04-04 00:31:35 +02:00
# if HAS_HEATER_1
2015-10-03 08:08:58 +02:00
analogWrite ( HEATER_1_PIN , tail_valve_pressure ) ;
2015-03-21 04:42:49 +01:00
# endif
2015-04-04 00:31:35 +02:00
# if HAS_HEATER_2
2015-10-03 08:08:58 +02:00
analogWrite ( HEATER_2_PIN , tail_e_to_p_pressure ) ;
2015-03-21 04:42:49 +01:00
# endif
2013-05-14 23:56:32 +02:00
# endif
2011-11-15 22:50:43 +01:00
}
float junction_deviation = 0.1 ;
2015-08-11 18:38:26 +02:00
// Add a new linear movement to the buffer. steps[X_AXIS], _y and _z is the absolute position in
2011-11-15 22:50:43 +01:00
// 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.
2015-08-05 14:12:26 +02:00
# if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
2015-10-03 08:08:58 +02:00
void plan_buffer_line ( float x , float y , float z , const float & e , float feed_rate , const uint8_t extruder )
2013-09-29 18:20:06 +02:00
# else
2015-10-03 08:08:58 +02:00
void plan_buffer_line ( const float & x , const float & y , const float & z , const float & e , float feed_rate , const uint8_t extruder )
2015-08-05 14:12:26 +02:00
# endif // AUTO_BED_LEVELING_FEATURE
2011-11-15 22:50:43 +01:00
{
// Calculate the buffer head after we push this byte
int next_buffer_head = next_block_index ( block_buffer_head ) ;
2015-08-11 18:38:26 +02:00
// If the buffer is full: good! That means we are well ahead of the robot.
2011-11-15 22:50:43 +01:00
// Rest here until there is room in the buffer.
2015-05-27 05:08:21 +02:00
while ( block_buffer_tail = = next_buffer_head ) idle ( ) ;
2012-06-11 17:33:42 +02:00
2015-07-31 07:30:29 +02:00
# if ENABLED(MESH_BED_LEVELING)
2015-03-22 00:12:57 +01:00
if ( mbl . active ) z + = mbl . get_z ( x , y ) ;
2015-08-05 14:12:26 +02:00
# elif ENABLED(AUTO_BED_LEVELING_FEATURE)
2015-03-21 04:42:49 +01:00
apply_rotation_xyz ( plan_bed_level_matrix , x , y , z ) ;
# endif
2013-09-29 18:20:06 +02:00
2011-11-15 22:50:43 +01: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
2015-03-21 04:42:49 +01:00
long target [ NUM_AXIS ] ;
target [ X_AXIS ] = lround ( x * axis_steps_per_unit [ X_AXIS ] ) ;
target [ Y_AXIS ] = lround ( y * axis_steps_per_unit [ Y_AXIS ] ) ;
2015-08-11 18:38:26 +02:00
target [ Z_AXIS ] = lround ( z * axis_steps_per_unit [ Z_AXIS ] ) ;
2015-03-21 04:42:49 +01:00
target [ E_AXIS ] = lround ( e * axis_steps_per_unit [ E_AXIS ] ) ;
2015-12-04 09:37:55 +01:00
long dx = target [ X_AXIS ] - position [ X_AXIS ] ,
dy = target [ Y_AXIS ] - position [ Y_AXIS ] ,
dz = target [ Z_AXIS ] - position [ Z_AXIS ] ;
2015-06-29 22:26:09 +02:00
// DRYRUN ignores all temperature constraints and assures that the extruder is instantly satisfied
2016-03-30 04:50:01 +02:00
if ( DEBUGGING ( DRYRUN ) )
2015-06-29 22:26:09 +02:00
position [ E_AXIS ] = target [ E_AXIS ] ;
2015-12-04 09:37:55 +01:00
long de = target [ E_AXIS ] - position [ E_AXIS ] ;
2012-06-11 17:33:42 +02:00
2015-07-31 07:30:29 +02:00
# if ENABLED(PREVENT_DANGEROUS_EXTRUDE)
2015-03-21 04:42:49 +01:00
if ( de ) {
2015-06-29 22:26:09 +02:00
if ( degHotend ( extruder ) < extrude_min_temp ) {
2015-04-09 10:40:48 +02:00
position [ E_AXIS ] = target [ E_AXIS ] ; // Behave as if the move really took place, but ignore E part
2015-04-08 05:01:08 +02:00
de = 0 ; // no difference
2015-03-21 04:42:49 +01:00
SERIAL_ECHO_START ;
SERIAL_ECHOLNPGM ( MSG_ERR_COLD_EXTRUDE_STOP ) ;
}
2015-07-31 07:30:29 +02:00
# if ENABLED(PREVENT_LENGTHY_EXTRUDE)
2016-03-13 07:38:55 +01:00
if ( labs ( de ) > axis_steps_per_unit [ E_AXIS ] * ( EXTRUDE_MAXLENGTH ) ) {
2015-03-21 04:42:49 +01:00
position [ E_AXIS ] = target [ E_AXIS ] ; // Behave as if the move really took place, but ignore E part
2015-04-08 05:01:08 +02:00
de = 0 ; // no difference
2015-03-21 04:42:49 +01:00
SERIAL_ECHO_START ;
SERIAL_ECHOLNPGM ( MSG_ERR_LONG_EXTRUDE_STOP ) ;
}
# endif
2012-11-12 15:49:40 +01:00
}
# endif
2012-06-11 17:33:42 +02:00
2011-11-15 22:50:43 +01:00
// Prepare to set up new block
2015-10-03 08:08:58 +02:00
block_t * block = & block_buffer [ block_buffer_head ] ;
2012-06-11 17:33:42 +02:00
2011-11-15 22:50:43 +01:00
// Mark block as not busy (Not executed by the stepper interrupt)
block - > busy = false ;
// Number of steps for each axis
2015-07-31 07:30:29 +02:00
# if ENABLED(COREXY)
2015-03-21 04:42:49 +01:00
// corexy planning
// these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
block - > steps [ A_AXIS ] = labs ( dx + dy ) ;
block - > steps [ B_AXIS ] = labs ( dx - dy ) ;
2015-06-16 02:34:04 +02:00
block - > steps [ Z_AXIS ] = labs ( dz ) ;
2015-08-03 21:30:37 +02:00
# elif ENABLED(COREXZ)
2015-06-16 02:36:41 +02:00
// corexz planning
block - > steps [ A_AXIS ] = labs ( dx + dz ) ;
block - > steps [ Y_AXIS ] = labs ( dy ) ;
block - > steps [ C_AXIS ] = labs ( dx - dz ) ;
2015-03-21 04:42:49 +01:00
# else
// default non-h-bot planning
block - > steps [ X_AXIS ] = labs ( dx ) ;
block - > steps [ Y_AXIS ] = labs ( dy ) ;
2015-06-16 02:34:04 +02:00
block - > steps [ Z_AXIS ] = labs ( dz ) ;
2015-03-21 04:42:49 +01:00
# endif
block - > steps [ E_AXIS ] = labs ( de ) ;
2015-04-13 01:57:00 +02:00
block - > steps [ E_AXIS ] * = volumetric_multiplier [ extruder ] ;
2015-06-01 22:33:22 +02:00
block - > steps [ E_AXIS ] * = extruder_multiplier [ extruder ] ;
2015-03-21 04:42:49 +01:00
block - > steps [ E_AXIS ] / = 100 ;
block - > step_event_count = max ( block - > steps [ X_AXIS ] , max ( block - > steps [ Y_AXIS ] , max ( block - > steps [ Z_AXIS ] , block - > steps [ E_AXIS ] ) ) ) ;
2011-11-15 22:50:43 +01:00
// Bail if this is a zero-length block
2015-03-21 04:42:49 +01:00
if ( block - > step_event_count < = dropsegments ) return ;
2011-11-15 22:50:43 +01:00
2016-03-06 03:27:45 +01:00
# if FAN_COUNT > 0
for ( uint8_t i = 0 ; i < FAN_COUNT ; i + + ) block - > fan_speed [ i ] = fanSpeeds [ i ] ;
# endif
2015-07-31 07:30:29 +02:00
# if ENABLED(BARICUDA)
2015-03-21 04:42:49 +01:00
block - > valve_pressure = ValvePressure ;
block - > e_to_p_pressure = EtoPPressure ;
2013-05-14 23:56:32 +02:00
# endif
2012-06-11 17:33:42 +02:00
2015-08-11 18:38:26 +02:00
// Compute direction bits for this block
2015-03-21 04:42:49 +01:00
uint8_t db = 0 ;
2015-07-31 07:30:29 +02:00
# if ENABLED(COREXY)
2016-02-27 13:51:44 +01:00
if ( dx < 0 ) SBI ( db , X_HEAD ) ; // Save the real Extruder (head) direction in X Axis
if ( dy < 0 ) SBI ( db , Y_HEAD ) ; // ...and Y
if ( dz < 0 ) SBI ( db , Z_AXIS ) ;
if ( dx + dy < 0 ) SBI ( db , A_AXIS ) ; // Motor A direction
if ( dx - dy < 0 ) SBI ( db , B_AXIS ) ; // Motor B direction
2015-08-03 21:30:37 +02:00
# elif ENABLED(COREXZ)
2016-02-27 13:51:44 +01:00
if ( dx < 0 ) SBI ( db , X_HEAD ) ; // Save the real Extruder (head) direction in X Axis
if ( dy < 0 ) SBI ( db , Y_AXIS ) ;
if ( dz < 0 ) SBI ( db , Z_HEAD ) ; // ...and Z
if ( dx + dz < 0 ) SBI ( db , A_AXIS ) ; // Motor A direction
if ( dx - dz < 0 ) SBI ( db , C_AXIS ) ; // Motor B direction
2015-03-21 04:42:49 +01:00
# else
2016-02-27 13:51:44 +01:00
if ( dx < 0 ) SBI ( db , X_AXIS ) ;
if ( dy < 0 ) SBI ( db , Y_AXIS ) ;
if ( dz < 0 ) SBI ( db , Z_AXIS ) ;
2015-03-21 04:42:49 +01:00
# endif
2016-02-27 13:51:44 +01:00
if ( de < 0 ) SBI ( db , E_AXIS ) ;
2015-03-21 04:42:49 +01:00
block - > direction_bits = db ;
2012-06-11 17:33:42 +02:00
2011-12-02 17:45:05 +01:00
block - > active_extruder = extruder ;
2012-06-11 17:33:42 +02:00
2011-11-15 22:50:43 +01:00
//enable active axes
2015-07-31 07:30:29 +02:00
# if ENABLED(COREXY)
2015-03-21 04:42:49 +01:00
if ( block - > steps [ A_AXIS ] | | block - > steps [ B_AXIS ] ) {
enable_x ( ) ;
enable_y ( ) ;
}
2015-08-03 21:30:37 +02:00
# if DISABLED(Z_LATE_ENABLE)
2015-06-16 02:34:04 +02:00
if ( block - > steps [ Z_AXIS ] ) enable_z ( ) ;
# endif
2015-08-03 21:30:37 +02:00
# elif ENABLED(COREXZ)
2015-06-16 02:36:41 +02:00
if ( block - > steps [ A_AXIS ] | | block - > steps [ C_AXIS ] ) {
enable_x ( ) ;
enable_z ( ) ;
}
2015-07-24 05:51:43 +02:00
if ( block - > steps [ Y_AXIS ] ) enable_y ( ) ;
2013-05-05 19:23:59 +02:00
# else
2015-03-21 04:42:49 +01:00
if ( block - > steps [ X_AXIS ] ) enable_x ( ) ;
if ( block - > steps [ Y_AXIS ] ) enable_y ( ) ;
2015-08-03 21:30:37 +02:00
# if DISABLED(Z_LATE_ENABLE)
2015-06-16 02:34:04 +02:00
if ( block - > steps [ Z_AXIS ] ) enable_z ( ) ;
# endif
2013-05-05 19:23:59 +02:00
# endif
2011-12-06 05:33:33 +01:00
2014-06-02 08:13:09 +02:00
// Enable extruder(s)
2015-03-21 04:42:49 +01:00
if ( block - > steps [ E_AXIS ] ) {
if ( DISABLE_INACTIVE_EXTRUDER ) { //enable only selected extruder
2014-07-13 21:52:32 +02:00
2015-10-03 08:08:58 +02:00
for ( int i = 0 ; i < EXTRUDERS ; i + + )
2015-03-21 04:42:49 +01:00
if ( g_uc_extruder_last_move [ i ] > 0 ) g_uc_extruder_last_move [ i ] - - ;
2015-08-11 18:38:26 +02:00
2015-03-21 04:42:49 +01:00
switch ( extruder ) {
case 0 :
enable_e0 ( ) ;
2015-09-30 11:20:31 +02:00
# if ENABLED(DUAL_X_CARRIAGE)
if ( extruder_duplication_enabled ) {
enable_e1 ( ) ;
2016-03-13 07:38:55 +01:00
g_uc_extruder_last_move [ 1 ] = ( BLOCK_BUFFER_SIZE ) * 2 ;
2015-09-30 11:20:31 +02:00
}
# endif
2016-03-13 07:38:55 +01:00
g_uc_extruder_last_move [ 0 ] = ( BLOCK_BUFFER_SIZE ) * 2 ;
2015-03-21 04:42:49 +01:00
# if EXTRUDERS > 1
if ( g_uc_extruder_last_move [ 1 ] = = 0 ) disable_e1 ( ) ;
# if EXTRUDERS > 2
if ( g_uc_extruder_last_move [ 2 ] = = 0 ) disable_e2 ( ) ;
# if EXTRUDERS > 3
if ( g_uc_extruder_last_move [ 3 ] = = 0 ) disable_e3 ( ) ;
# endif
# endif
# endif
2014-07-13 21:52:32 +02:00
break ;
2015-03-21 04:42:49 +01:00
# if EXTRUDERS > 1
case 1 :
enable_e1 ( ) ;
2016-03-13 07:38:55 +01:00
g_uc_extruder_last_move [ 1 ] = ( BLOCK_BUFFER_SIZE ) * 2 ;
2015-03-21 04:42:49 +01:00
if ( g_uc_extruder_last_move [ 0 ] = = 0 ) disable_e0 ( ) ;
# if EXTRUDERS > 2
if ( g_uc_extruder_last_move [ 2 ] = = 0 ) disable_e2 ( ) ;
# if EXTRUDERS > 3
if ( g_uc_extruder_last_move [ 3 ] = = 0 ) disable_e3 ( ) ;
# endif
# endif
break ;
# if EXTRUDERS > 2
case 2 :
enable_e2 ( ) ;
2016-03-13 07:38:55 +01:00
g_uc_extruder_last_move [ 2 ] = ( BLOCK_BUFFER_SIZE ) * 2 ;
2015-03-21 04:42:49 +01:00
if ( g_uc_extruder_last_move [ 0 ] = = 0 ) disable_e0 ( ) ;
if ( g_uc_extruder_last_move [ 1 ] = = 0 ) disable_e1 ( ) ;
# if EXTRUDERS > 3
if ( g_uc_extruder_last_move [ 3 ] = = 0 ) disable_e3 ( ) ;
# endif
break ;
# if EXTRUDERS > 3
case 3 :
enable_e3 ( ) ;
2016-03-13 07:38:55 +01:00
g_uc_extruder_last_move [ 3 ] = ( BLOCK_BUFFER_SIZE ) * 2 ;
2015-03-21 04:42:49 +01:00
if ( g_uc_extruder_last_move [ 0 ] = = 0 ) disable_e0 ( ) ;
if ( g_uc_extruder_last_move [ 1 ] = = 0 ) disable_e1 ( ) ;
if ( g_uc_extruder_last_move [ 2 ] = = 0 ) disable_e2 ( ) ;
break ;
# endif // EXTRUDERS > 3
# endif // EXTRUDERS > 2
# endif // EXTRUDERS > 1
2014-06-02 08:13:09 +02:00
}
}
2015-03-21 04:42:49 +01:00
else { // enable all
2014-06-02 08:13:09 +02:00
enable_e0 ( ) ;
enable_e1 ( ) ;
2015-01-23 23:13:06 +01:00
enable_e2 ( ) ;
enable_e3 ( ) ;
2014-06-02 08:13:09 +02:00
}
2012-06-11 17:33:42 +02:00
}
2011-12-06 05:33:33 +01:00
2015-04-13 03:07:08 +02:00
if ( block - > steps [ E_AXIS ] )
NOLESS ( feed_rate , minimumfeedrate ) ;
else
NOLESS ( feed_rate , mintravelfeedrate ) ;
2012-06-11 17:33:42 +02:00
2015-03-21 04:42:49 +01:00
/**
2015-08-11 18:38:26 +02:00
* This part of the code calculates the total length of the movement .
2015-03-21 04:42:49 +01:00
* For cartesian bots , the X_AXIS is the real X movement and same for Y_AXIS .
* But for corexy bots , that is not true . The " X_AXIS " and " Y_AXIS " motors ( that should be named to A_AXIS
* and B_AXIS ) cannot be used for X and Y length , because A = X + Y and B = X - Y .
2015-08-11 18:38:26 +02:00
* So we need to create other 2 " AXIS " , named X_HEAD and Y_HEAD , meaning the real displacement of the Head .
2015-03-21 04:42:49 +01:00
* Having the real displacement of the head , we can calculate the total movement length and apply the desired speed .
2015-08-11 18:38:26 +02:00
*/
2015-07-31 07:30:29 +02:00
# if ENABLED(COREXY)
2015-01-06 19:39:48 +01:00
float delta_mm [ 6 ] ;
2015-03-21 04:42:49 +01:00
delta_mm [ X_HEAD ] = dx / axis_steps_per_unit [ A_AXIS ] ;
delta_mm [ Y_HEAD ] = dy / axis_steps_per_unit [ B_AXIS ] ;
2015-06-16 02:34:04 +02:00
delta_mm [ Z_AXIS ] = dz / axis_steps_per_unit [ Z_AXIS ] ;
2015-03-21 04:42:49 +01:00
delta_mm [ A_AXIS ] = ( dx + dy ) / axis_steps_per_unit [ A_AXIS ] ;
delta_mm [ B_AXIS ] = ( dx - dy ) / axis_steps_per_unit [ B_AXIS ] ;
2015-08-03 21:30:37 +02:00
# elif ENABLED(COREXZ)
2015-06-16 02:36:41 +02:00
float delta_mm [ 6 ] ;
delta_mm [ X_HEAD ] = dx / axis_steps_per_unit [ A_AXIS ] ;
delta_mm [ Y_AXIS ] = dy / axis_steps_per_unit [ Y_AXIS ] ;
delta_mm [ Z_HEAD ] = dz / axis_steps_per_unit [ C_AXIS ] ;
delta_mm [ A_AXIS ] = ( dx + dz ) / axis_steps_per_unit [ A_AXIS ] ;
delta_mm [ C_AXIS ] = ( dx - dz ) / axis_steps_per_unit [ C_AXIS ] ;
2015-03-21 04:42:49 +01:00
# else
float delta_mm [ 4 ] ;
delta_mm [ X_AXIS ] = dx / axis_steps_per_unit [ X_AXIS ] ;
delta_mm [ Y_AXIS ] = dy / axis_steps_per_unit [ Y_AXIS ] ;
2015-06-16 02:34:04 +02:00
delta_mm [ Z_AXIS ] = dz / axis_steps_per_unit [ Z_AXIS ] ;
2013-06-09 13:10:06 +02:00
# endif
2015-06-01 22:33:22 +02:00
delta_mm [ E_AXIS ] = ( de / axis_steps_per_unit [ E_AXIS ] ) * volumetric_multiplier [ extruder ] * extruder_multiplier [ extruder ] / 100.0 ;
2015-03-21 04:42:49 +01:00
if ( block - > steps [ X_AXIS ] < = dropsegments & & block - > steps [ Y_AXIS ] < = dropsegments & & block - > steps [ Z_AXIS ] < = dropsegments ) {
2012-05-16 18:17:34 +02:00
block - > millimeters = fabs ( delta_mm [ E_AXIS ] ) ;
2015-08-11 18:38:26 +02:00
}
2015-03-21 04:42:49 +01:00
else {
block - > millimeters = sqrt (
2015-07-31 07:30:29 +02:00
# if ENABLED(COREXY)
2015-06-16 02:34:04 +02:00
square ( delta_mm [ X_HEAD ] ) + square ( delta_mm [ Y_HEAD ] ) + square ( delta_mm [ Z_AXIS ] )
2015-07-31 07:30:29 +02:00
# elif ENABLED(COREXZ)
2015-06-16 02:36:41 +02:00
square ( delta_mm [ X_HEAD ] ) + square ( delta_mm [ Y_AXIS ] ) + square ( delta_mm [ Z_HEAD ] )
2015-03-21 04:42:49 +01:00
# else
2015-06-16 02:34:04 +02:00
square ( delta_mm [ X_AXIS ] ) + square ( delta_mm [ Y_AXIS ] ) + square ( delta_mm [ Z_AXIS ] )
2015-03-21 04:42:49 +01:00
# endif
) ;
2012-02-09 19:53:06 +01:00
}
2015-08-11 18:38:26 +02:00
float inverse_millimeters = 1.0 / block - > millimeters ; // Inverse millimeters to remove multiple divides
2012-06-11 17:33:42 +02:00
2016-03-03 08:49:50 +01:00
// Calculate moves/second for this move. No divide by zero due to previous checks.
2011-11-15 22:50:43 +01:00
float inverse_second = feed_rate * inverse_millimeters ;
2012-06-11 17:33:42 +02:00
2015-03-21 04:42:49 +01:00
int moves_queued = movesplanned ( ) ;
2012-06-11 17:33:42 +02:00
2015-03-27 08:32:58 +01:00
// Slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
2015-07-31 07:30:29 +02:00
# if ENABLED(OLD_SLOWDOWN) || ENABLED(SLOWDOWN)
2016-03-13 07:38:55 +01:00
bool mq = moves_queued > 1 & & moves_queued < ( BLOCK_BUFFER_SIZE ) / 2 ;
2015-07-31 07:30:29 +02:00
# if ENABLED(OLD_SLOWDOWN)
2016-03-13 07:38:55 +01:00
if ( mq ) feed_rate * = 2.0 * moves_queued / ( BLOCK_BUFFER_SIZE ) ;
2015-03-27 08:32:58 +01:00
# endif
2015-07-31 07:30:29 +02:00
# if ENABLED(SLOWDOWN)
2015-03-27 08:32:58 +01:00
// segment time im micro seconds
unsigned long segment_time = lround ( 1000000.0 / inverse_second ) ;
if ( mq ) {
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
}
2015-03-21 04:42:49 +01:00
}
2015-03-27 08:32:58 +01:00
# endif
2015-03-21 04:42:49 +01:00
# endif
2011-11-15 22:50:43 +01:00
2012-04-15 19:17:33 +02: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 22:50:43 +01:00
2016-03-30 05:28:23 +02:00
# if ENABLED(FILAMENT_WIDTH_SENSOR)
2016-04-07 04:29:08 +02:00
static float filwidth_e_count = 0 , filwidth_delay_dist = 0 ;
2015-03-21 04:42:49 +01:00
//FMM update ring buffer used for delay with filament measurements
2016-04-07 04:29:08 +02:00
if ( extruder = = FILAMENT_SENSOR_EXTRUDER_NUM & & filwidth_delay_index2 > = 0 ) { //only for extruder with filament sensor and if ring buffer is initialized
const int MMD_CM = MAX_MEASUREMENT_DELAY + 1 , MMD_MM = MMD_CM * 10 ;
// increment counters with next move in e axis
filwidth_e_count + = delta_mm [ E_AXIS ] ;
filwidth_delay_dist + = delta_mm [ E_AXIS ] ;
2015-03-21 04:42:49 +01:00
2016-04-07 04:29:08 +02:00
// Only get new measurements on forward E movement
if ( filwidth_e_count > 0.0001 ) {
2015-03-21 04:42:49 +01:00
2016-04-07 04:29:08 +02:00
// Loop the delay distance counter (modulus by the mm length)
while ( filwidth_delay_dist > = MMD_MM ) filwidth_delay_dist - = MMD_MM ;
2014-08-07 02:30:57 +02:00
2016-04-07 04:29:08 +02:00
// Convert into an index into the measurement array
filwidth_delay_index1 = ( int ) ( filwidth_delay_dist / 10.0 + 0.0001 ) ;
2015-03-21 04:42:49 +01:00
2016-04-07 04:29:08 +02:00
// If the index has changed (must have gone forward)...
if ( filwidth_delay_index1 ! = filwidth_delay_index2 ) {
filwidth_e_count = 0 ; // Reset the E movement counter
int8_t meas_sample = widthFil_to_size_ratio ( ) - 100 ; // Subtract 100 to reduce magnitude - to store in a signed char
do {
filwidth_delay_index2 = ( filwidth_delay_index2 + 1 ) % MMD_CM ; // The next unused slot
measurement_delay [ filwidth_delay_index2 ] = meas_sample ; // Store the measurement
} while ( filwidth_delay_index1 ! = filwidth_delay_index2 ) ; // More slots to fill?
2015-03-21 04:42:49 +01:00
}
}
}
# endif
2014-08-07 02:30:57 +02:00
2012-06-11 17:33:42 +02:00
// Calculate and limit speed in mm/sec for each axis
2015-03-21 04:42:49 +01:00
float current_speed [ NUM_AXIS ] ;
2011-11-15 22:50:43 +01:00
float speed_factor = 1.0 ; //factor <=1 do decrease speed
2015-03-21 04:42:49 +01:00
for ( int i = 0 ; i < NUM_AXIS ; i + + ) {
2012-04-15 19:17:33 +02:00
current_speed [ i ] = delta_mm [ i ] * inverse_second ;
2015-03-21 04:42:49 +01:00
float cs = fabs ( current_speed [ i ] ) , mf = max_feedrate [ i ] ;
if ( cs > mf ) speed_factor = min ( speed_factor , mf / cs ) ;
2011-11-15 22:50:43 +01:00
}
2012-06-11 17:33:42 +02:00
// Max segement time in us.
2015-03-21 04:42:49 +01:00
# ifdef 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 ) ;
2015-08-11 18:38:26 +02:00
2015-03-21 04:42:49 +01:00
long xs0 = axis_segment_time [ X_AXIS ] [ 0 ] ,
xs1 = axis_segment_time [ X_AXIS ] [ 1 ] ,
xs2 = axis_segment_time [ X_AXIS ] [ 2 ] ,
ys0 = axis_segment_time [ Y_AXIS ] [ 0 ] ,
ys1 = axis_segment_time [ Y_AXIS ] [ 1 ] ,
ys2 = axis_segment_time [ Y_AXIS ] [ 2 ] ;
2016-02-27 13:51:44 +01:00
if ( TEST ( direction_change , X_AXIS ) ) {
2015-03-21 04:42:49 +01:00
xs2 = axis_segment_time [ X_AXIS ] [ 2 ] = xs1 ;
xs1 = axis_segment_time [ X_AXIS ] [ 1 ] = xs0 ;
xs0 = 0 ;
}
xs0 = axis_segment_time [ X_AXIS ] [ 0 ] = xs0 + segment_time ;
2011-11-15 22:50:43 +01:00
2016-02-27 13:51:44 +01:00
if ( TEST ( direction_change , Y_AXIS ) ) {
2015-03-21 04:42:49 +01:00
ys2 = axis_segment_time [ Y_AXIS ] [ 2 ] = axis_segment_time [ Y_AXIS ] [ 1 ] ;
ys1 = axis_segment_time [ Y_AXIS ] [ 1 ] = axis_segment_time [ Y_AXIS ] [ 0 ] ;
ys0 = 0 ;
}
ys0 = axis_segment_time [ Y_AXIS ] [ 0 ] = ys0 + segment_time ;
long max_x_segment_time = max ( xs0 , max ( xs1 , xs2 ) ) ,
max_y_segment_time = max ( ys0 , max ( ys1 , ys2 ) ) ,
min_xy_segment_time = min ( max_x_segment_time , max_y_segment_time ) ;
if ( min_xy_segment_time < MAX_FREQ_TIME ) {
2016-03-15 09:10:57 +01:00
float low_sf = speed_factor * min_xy_segment_time / ( MAX_FREQ_TIME ) ;
2015-03-21 04:42:49 +01:00
speed_factor = min ( speed_factor , low_sf ) ;
2011-11-15 22:50:43 +01:00
}
2015-03-21 04:42:49 +01:00
# endif // XY_FREQUENCY_LIMIT
2015-08-11 18:38:26 +02:00
// Correct the speed
2015-03-21 04:42:49 +01:00
if ( speed_factor < 1.0 ) {
for ( unsigned char i = 0 ; i < NUM_AXIS ; i + + ) current_speed [ i ] * = speed_factor ;
2011-11-15 22:50:43 +01:00
block - > nominal_speed * = speed_factor ;
block - > nominal_rate * = speed_factor ;
}
2015-08-11 18:38:26 +02:00
// Compute and limit the acceleration rate for the trapezoid generator.
2015-03-21 04:42:49 +01:00
float steps_per_mm = block - > step_event_count / block - > millimeters ;
2016-03-03 08:49:50 +01:00
unsigned long bsx = block - > steps [ X_AXIS ] , bsy = block - > steps [ Y_AXIS ] , bsz = block - > steps [ Z_AXIS ] , bse = block - > steps [ E_AXIS ] ;
2015-03-21 04:42:49 +01:00
if ( bsx = = 0 & & bsy = = 0 & & bsz = = 0 ) {
2011-11-15 22:50:43 +01:00
block - > acceleration_st = ceil ( retract_acceleration * steps_per_mm ) ; // convert to: acceleration steps/sec^2
}
2015-03-21 04:42:49 +01:00
else if ( bse = = 0 ) {
2015-03-11 17:19:02 +01:00
block - > acceleration_st = ceil ( travel_acceleration * steps_per_mm ) ; // convert to: acceleration steps/sec^2
}
2015-03-21 04:42:49 +01:00
else {
2011-11-15 22:50:43 +01:00
block - > acceleration_st = ceil ( acceleration * steps_per_mm ) ; // convert to: acceleration steps/sec^2
}
2015-03-11 19:07:35 +01:00
// Limit acceleration per axis
2015-03-21 04:42:49 +01:00
unsigned long acc_st = block - > acceleration_st ,
xsteps = axis_steps_per_sqr_second [ X_AXIS ] ,
ysteps = axis_steps_per_sqr_second [ Y_AXIS ] ,
zsteps = axis_steps_per_sqr_second [ Z_AXIS ] ,
2016-03-03 08:49:50 +01:00
esteps = axis_steps_per_sqr_second [ E_AXIS ] ,
allsteps = block - > step_event_count ;
if ( xsteps < ( acc_st * bsx ) / allsteps ) acc_st = ( xsteps * allsteps ) / bsx ;
if ( ysteps < ( acc_st * bsy ) / allsteps ) acc_st = ( ysteps * allsteps ) / bsy ;
if ( zsteps < ( acc_st * bsz ) / allsteps ) acc_st = ( zsteps * allsteps ) / bsz ;
if ( esteps < ( acc_st * bse ) / allsteps ) acc_st = ( esteps * allsteps ) / bse ;
2015-08-11 18:38:26 +02:00
2015-03-21 04:42:49 +01:00
block - > acceleration_st = acc_st ;
block - > acceleration = acc_st / steps_per_mm ;
block - > acceleration_rate = ( long ) ( acc_st * 16777216.0 / ( F_CPU / 8.0 ) ) ;
#if 0 // Use old jerk for now
// Compute path unit vector
double unit_vec [ 3 ] ;
2015-10-03 08:08:58 +02:00
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 ;
2015-03-21 04:42:49 +01: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,
2016-03-06 05:41:15 +01:00
// collinear with the circle center. The circular segment joining the two paths represents the
2015-03-21 04:42:49 +01:00
// 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 ]
2015-10-03 08:08:58 +02:00
- previous_unit_vec [ Y_AXIS ] * unit_vec [ Y_AXIS ]
- previous_unit_vec [ Z_AXIS ] * unit_vec [ Z_AXIS ] ;
2015-03-21 04:42:49 +01:00
// Skip and use default max junction speed for 0 degree acute junction.
if ( cos_theta < 0.95 ) {
2015-10-03 08:08:58 +02:00
vmax_junction = min ( previous_nominal_speed , block - > nominal_speed ) ;
2015-03-21 04:42:49 +01:00
// 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
2015-10-03 08:08:58 +02:00
double sin_theta_d2 = sqrt ( 0.5 * ( 1.0 - cos_theta ) ) ; // Trig half angle identity. Always positive.
2015-03-21 04:42:49 +01:00
vmax_junction = min ( vmax_junction ,
2015-10-03 08:08:58 +02:00
sqrt ( block - > acceleration * junction_deviation * sin_theta_d2 / ( 1.0 - sin_theta_d2 ) ) ) ;
2015-03-21 04:42:49 +01:00
}
2011-11-15 22:50:43 +01:00
}
}
2015-03-21 04:42:49 +01:00
# endif
2011-11-15 22:50:43 +01:00
// Start with a safe speed
2015-03-21 04:42:49 +01:00
float vmax_junction = max_xy_jerk / 2 ;
2015-08-11 18:38:26 +02:00
float vmax_junction_factor = 1.0 ;
2015-03-21 04:42:49 +01:00
float mz2 = max_z_jerk / 2 , me2 = max_e_jerk / 2 ;
float csz = current_speed [ Z_AXIS ] , cse = current_speed [ E_AXIS ] ;
if ( fabs ( csz ) > mz2 ) vmax_junction = min ( vmax_junction , mz2 ) ;
if ( fabs ( cse ) > me2 ) vmax_junction = min ( vmax_junction , me2 ) ;
2012-06-11 17:33:42 +02:00
vmax_junction = min ( vmax_junction , block - > nominal_speed ) ;
float safe_speed = vmax_junction ;
2012-04-01 16:23:40 +02:00
if ( ( moves_queued > 1 ) & & ( previous_nominal_speed > 0.0001 ) ) {
2015-12-04 09:30:13 +01:00
float dsx = current_speed [ X_AXIS ] - previous_speed [ X_AXIS ] ,
dsy = current_speed [ Y_AXIS ] - previous_speed [ Y_AXIS ] ,
dsz = fabs ( csz - previous_speed [ Z_AXIS ] ) ,
dse = fabs ( cse - previous_speed [ E_AXIS ] ) ,
jerk = sqrt ( dsx * dsx + dsy * dsy ) ;
2015-03-21 04:42:49 +01:00
// if ((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
2012-06-11 17:33:42 +02:00
vmax_junction = block - > nominal_speed ;
// }
2015-03-21 04:42:49 +01:00
if ( jerk > max_xy_jerk ) vmax_junction_factor = max_xy_jerk / jerk ;
2015-12-04 09:30:13 +01:00
if ( dsz > max_z_jerk ) vmax_junction_factor = min ( vmax_junction_factor , max_z_jerk / dsz ) ;
if ( dse > max_e_jerk ) vmax_junction_factor = min ( vmax_junction_factor , max_e_jerk / dse ) ;
2015-03-21 04:42:49 +01:00
2012-06-11 17:33:42 +02:00
vmax_junction = min ( previous_nominal_speed , vmax_junction * vmax_junction_factor ) ; // Limit speed to max previous speed
2011-11-15 22:50:43 +01:00
}
block - > max_entry_speed = vmax_junction ;
2012-06-11 17:33:42 +02:00
2011-11-15 22:50:43 +01:00
// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
2015-03-21 04:42:49 +01:00
double v_allowable = max_allowable_speed ( - block - > acceleration , MINIMUM_PLANNER_SPEED , block - > millimeters ) ;
2011-11-15 22:50:43 +01:00
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.
2015-08-11 18:38:26 +02:00
block - > nominal_length_flag = ( block - > nominal_speed < = v_allowable ) ;
2011-11-15 22:50:43 +01:00
block - > recalculate_flag = true ; // Always calculate trapezoid for new block
2012-06-11 17:33:42 +02:00
2011-11-15 22:50:43 +01:00
// Update previous path unit_vector and nominal speed
2015-03-21 04:42:49 +01:00
for ( int i = 0 ; i < NUM_AXIS ; i + + ) previous_speed [ i ] = current_speed [ i ] ;
2011-11-15 22:50:43 +01:00
previous_nominal_speed = block - > nominal_speed ;
2012-04-01 16:23:40 +02:00
2015-07-31 07:30:29 +02:00
# if ENABLED(ADVANCE)
2015-03-21 04:42:49 +01:00
// Calculate advance rate
if ( ! bse | | ( ! bsx & & ! bsy & & ! bsz ) ) {
2011-11-15 22:50:43 +01:00
block - > advance_rate = 0 ;
2015-03-21 04:42:49 +01:00
block - > advance = 0 ;
}
2011-11-15 22:50:43 +01:00
else {
2015-03-21 04:42:49 +01:00
long acc_dist = estimate_acceleration_distance ( 0 , block - > nominal_rate , block - > acceleration_st ) ;
2016-03-13 07:38:55 +01:00
float advance = ( ( STEPS_PER_CUBIC_MM_E ) * ( EXTRUDER_ADVANCE_K ) ) * ( cse * cse * ( EXTRUSION_AREA ) * ( EXTRUSION_AREA ) ) * 256 ;
2015-03-21 04:42:49 +01:00
block - > advance = advance ;
block - > advance_rate = acc_dist ? advance / ( float ) acc_dist : 0 ;
2011-11-15 22:50:43 +01:00
}
2016-03-25 07:19:46 +01:00
/**
2015-03-21 04:42:49 +01:00
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
2011-11-15 22:50:43 +01:00
2015-03-21 04:42:49 +01:00
calculate_trapezoid_for_block ( block , block - > entry_speed / block - > nominal_speed , safe_speed / block - > nominal_speed ) ;
2012-06-11 17:33:42 +02:00
2011-11-15 22:50:43 +01:00
// Move buffer head
block_buffer_head = next_buffer_head ;
2012-06-11 17:33:42 +02:00
2011-11-15 22:50:43 +01:00
// Update position
2015-03-21 04:42:49 +01:00
for ( int i = 0 ; i < NUM_AXIS ; i + + ) position [ i ] = target [ i ] ;
2011-11-15 22:50:43 +01:00
planner_recalculate ( ) ;
2012-04-15 19:17:33 +02:00
2011-11-15 22:50:43 +01:00
st_wake_up ( ) ;
2015-03-21 04:42:49 +01:00
} // plan_buffer_line()
2013-09-29 18:20:06 +02:00
2015-08-05 14:12:26 +02:00
# if ENABLED(AUTO_BED_LEVELING_FEATURE) && DISABLED(DELTA)
2015-03-22 00:12:57 +01:00
vector_3 plan_get_position ( ) {
2016-02-12 03:17:17 +01:00
vector_3 position = vector_3 ( st_get_axis_position_mm ( X_AXIS ) , st_get_axis_position_mm ( Y_AXIS ) , st_get_axis_position_mm ( Z_AXIS ) ) ;
2013-09-29 18:20:06 +02:00
2015-03-22 00:12:57 +01:00
//position.debug("in plan_get position");
2015-05-04 01:03:05 +02:00
//plan_bed_level_matrix.debug("in plan_get_position");
2015-03-22 00:12:57 +01:00
matrix_3x3 inverse = matrix_3x3 : : transpose ( plan_bed_level_matrix ) ;
//inverse.debug("in plan_get inverse");
position . apply_rotation ( inverse ) ;
//position.debug("after rotation");
2015-03-21 04:42:49 +01:00
2015-03-22 00:12:57 +01:00
return position ;
}
2015-08-05 14:12:26 +02:00
# endif // AUTO_BED_LEVELING_FEATURE && !DELTA
2015-03-21 04:42:49 +01:00
2015-08-05 14:12:26 +02:00
# if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
2015-10-03 08:08:58 +02:00
void plan_set_position ( float x , float y , float z , const float & e )
2013-09-29 18:20:06 +02:00
# else
2015-10-03 08:08:58 +02:00
void plan_set_position ( const float & x , const float & y , const float & z , const float & e )
2015-08-05 14:12:26 +02:00
# endif // AUTO_BED_LEVELING_FEATURE || MESH_BED_LEVELING
2015-03-21 04:42:49 +01:00
{
2015-07-31 07:30:29 +02:00
# if ENABLED(MESH_BED_LEVELING)
2015-03-22 00:12:57 +01:00
if ( mbl . active ) z + = mbl . get_z ( x , y ) ;
2015-08-05 14:12:26 +02:00
# elif ENABLED(AUTO_BED_LEVELING_FEATURE)
2015-04-08 10:05:39 +02:00
apply_rotation_xyz ( plan_bed_level_matrix , x , y , z ) ;
2015-03-21 04:42:49 +01:00
# endif
2013-09-29 18:20:06 +02:00
2015-12-04 09:41:13 +01:00
long nx = position [ X_AXIS ] = lround ( x * axis_steps_per_unit [ X_AXIS ] ) ,
ny = position [ Y_AXIS ] = lround ( y * axis_steps_per_unit [ Y_AXIS ] ) ,
nz = position [ Z_AXIS ] = lround ( z * axis_steps_per_unit [ Z_AXIS ] ) ,
ne = position [ E_AXIS ] = lround ( e * axis_steps_per_unit [ E_AXIS ] ) ;
2015-03-21 04:42:49 +01:00
st_set_position ( nx , ny , nz , ne ) ;
previous_nominal_speed = 0.0 ; // Resets planner junction speeds. Assumes start from rest.
2011-11-15 22:50:43 +01:00
2015-10-03 08:08:58 +02:00
for ( int i = 0 ; i < NUM_AXIS ; i + + ) previous_speed [ i ] = 0.0 ;
2015-03-21 04:42:49 +01:00
}
2011-11-25 13:43:06 +01:00
2015-10-03 08:08:58 +02:00
void plan_set_e_position ( const float & e ) {
2015-08-11 18:38:26 +02:00
position [ E_AXIS ] = lround ( e * axis_steps_per_unit [ E_AXIS ] ) ;
2015-03-21 04:42:49 +01:00
st_set_e_position ( position [ E_AXIS ] ) ;
2011-11-20 16:05:42 +01:00
}
2011-11-25 13:43:06 +01: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 15:05:11 +01:00
// Calculate the steps/s^2 acceleration rates, based on the mm/s^s
2015-03-21 04:42:49 +01:00
void reset_acceleration_rates ( ) {
for ( int i = 0 ; i < NUM_AXIS ; i + + )
axis_steps_per_sqr_second [ i ] = max_acceleration_units_per_sq_second [ i ] * axis_steps_per_unit [ i ] ;
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 15:05:11 +01:00
}