2016-03-25 06:19:46 +00:00
/**
2016-03-24 18:01:20 +00:00
* Marlin 3 D Printer Firmware
2020-02-03 14:00:57 +00:00
* Copyright ( c ) 2020 MarlinFirmware [ https : //github.com/MarlinFirmware/Marlin]
2016-03-24 18:01:20 +00:00
*
* Based on Sprinter and grbl .
2019-06-28 04:57:50 +00:00
* Copyright ( c ) 2011 Camiel Gubbels / Erik van der Zalm
2016-03-24 18:01:20 +00:00
*
* 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/>.
*
*/
2018-10-10 14:45:20 +00:00
# pragma once
2016-03-24 18:01:20 +00:00
2016-03-25 06:19:46 +00:00
/**
2016-05-04 00:00:28 +00:00
* planner . h
*
* Buffer movement commands and manage the acceleration profile plan
*
* Derived from Grbl
* Copyright ( c ) 2009 - 2011 Simen Svale Skogsrud
*/
2011-11-15 21:50:43 +00:00
2020-01-03 01:01:38 +00:00
# include "../MarlinCore.h"
2011-11-15 21:50:43 +00:00
2017-09-08 20:35:25 +00:00
# include "motion.h"
2018-05-09 05:17:53 +00:00
# include "../gcode/queue.h"
2017-09-08 20:35:25 +00:00
# if ENABLED(DELTA)
# include "delta.h"
# endif
2018-05-01 12:05:18 +00:00
# if ABL_PLANAR
2019-09-29 09:25:39 +00:00
# include "../libs/vector_3.h" // for matrix_3x3
2016-04-28 01:06:32 +00:00
# endif
2018-09-17 02:24:15 +00:00
# if ENABLED(FWRETRACT)
# include "../feature/fwretract.h"
# endif
2018-10-16 08:38:57 +00:00
# if ENABLED(MIXING_EXTRUDER)
# include "../feature/mixing.h"
# endif
2019-10-15 21:10:20 +00:00
# if HAS_CUTTER
2020-04-03 00:31:08 +00:00
# include "../feature/spindle_laser_types.h"
2019-10-15 21:10:20 +00:00
# endif
2019-09-29 09:25:39 +00:00
// Feedrate for manual moves
# ifdef MANUAL_FEEDRATE
2020-02-03 06:00:35 +00:00
constexpr xyze_feedrate_t _mf = MANUAL_FEEDRATE ,
manual_feedrate_mm_s { _mf . x / 60.0f , _mf . y / 60.0f , _mf . z / 60.0f , _mf . e / 60.0f } ;
2019-09-29 09:25:39 +00:00
# endif
2020-04-24 01:49:11 +00:00
# if IS_KINEMATIC && HAS_JUNCTION_DEVIATION
2020-03-25 22:55:36 +00:00
# define HAS_DIST_MM_ARG 1
# endif
2018-03-07 04:35:22 +00:00
enum BlockFlagBit : char {
2016-10-30 21:05:14 +00:00
// Recalculate trapezoids on entry junction. For optimization.
BLOCK_BIT_RECALCULATE ,
// Nominal speed always reached.
// i.e., The segment is long enough, so the nominal speed is reachable if accelerating
// from a safe speed (in consideration of jerking from zero speed).
BLOCK_BIT_NOMINAL_LENGTH ,
2017-12-08 05:03:36 +00:00
// The block is segment 2+ of a longer move
2018-05-03 22:45:13 +00:00
BLOCK_BIT_CONTINUED ,
// Sync the stepper counts from the block
BLOCK_BIT_SYNC_POSITION
2016-10-30 21:05:14 +00:00
} ;
2016-10-11 04:17:49 +00:00
2018-03-07 04:35:22 +00:00
enum BlockFlag : char {
2016-10-30 21:05:14 +00:00
BLOCK_FLAG_RECALCULATE = _BV ( BLOCK_BIT_RECALCULATE ) ,
BLOCK_FLAG_NOMINAL_LENGTH = _BV ( BLOCK_BIT_NOMINAL_LENGTH ) ,
2018-05-03 22:45:13 +00:00
BLOCK_FLAG_CONTINUED = _BV ( BLOCK_BIT_CONTINUED ) ,
BLOCK_FLAG_SYNC_POSITION = _BV ( BLOCK_BIT_SYNC_POSITION )
2016-10-11 04:17:49 +00:00
} ;
2020-04-03 00:31:08 +00:00
# if ENABLED(LASER_POWER_INLINE)
typedef struct {
uint8_t status , // See planner settings for meaning
power ; // Ditto; When in trapezoid mode this is nominal power
# if ENABLED(LASER_POWER_INLINE_TRAPEZOID)
uint8_t power_entry ; // Entry power for the laser
# if DISABLED(LASER_POWER_INLINE_TRAPEZOID_CONT)
uint8_t power_exit ; // Exit power for the laser
uint32_t entry_per , // Steps per power increment (to avoid floats in stepper calcs)
exit_per ; // Steps per power decrement
# endif
# endif
} block_laser_t ;
# endif
2016-04-28 01:06:32 +00:00
/**
* struct block_t
*
* A single entry in the planner buffer .
* Tracks linear movement over multiple axes .
*
* The " nominal " values are as - specified by gcode , and
* may never actually be reached due to acceleration limits .
*/
2018-10-16 12:28:52 +00:00
typedef struct block_t {
2016-04-28 01:06:32 +00:00
2018-06-27 23:11:16 +00:00
volatile uint8_t flag ; // Block flags (See BlockFlag enum above) - Modified by ISR and main thread!
2016-10-30 21:08:46 +00:00
2018-05-09 05:17:53 +00:00
// Fields used by the motion planner to manage acceleration
float nominal_speed_sqr , // The nominal speed for this block in (mm/sec)^2
entry_speed_sqr , // Entry speed at previous-current junction in (mm/sec)^2
max_entry_speed_sqr , // Maximum allowable junction entry speed in (mm/sec)^2
millimeters , // The total travel of this block in mm
acceleration ; // acceleration mm/sec^2
2016-04-28 01:06:32 +00:00
2018-05-09 05:17:53 +00:00
union {
2019-09-29 09:25:39 +00:00
abce_ulong_t steps ; // Step count along each axis
abce_long_t position ; // New position to force when this sync block is executed
2018-05-09 05:17:53 +00:00
} ;
2016-10-30 21:08:46 +00:00
uint32_t step_event_count ; // The number of step events required to complete this block
2016-04-28 01:06:32 +00:00
2018-09-11 04:09:26 +00:00
# if EXTRUDERS > 1
2018-10-16 08:38:57 +00:00
uint8_t extruder ; // The extruder to move (if E move)
2018-10-16 12:28:52 +00:00
# else
static constexpr uint8_t extruder = 0 ;
2018-09-11 04:09:26 +00:00
# endif
2018-05-09 05:17:53 +00:00
2016-06-28 22:06:56 +00:00
# if ENABLED(MIXING_EXTRUDER)
2019-02-03 05:30:26 +00:00
MIXER_BLOCK_FIELD ; // Normalized color for the mixing steppers
2016-06-28 22:06:56 +00:00
# endif
2018-04-07 01:48:06 +00:00
// Settings for the trapezoid generator
2018-05-09 05:17:53 +00:00
uint32_t accelerate_until , // The index of the step event on which to stop acceleration
decelerate_after ; // The index of the step event on which to start decelerating
2018-04-07 01:48:06 +00:00
2018-05-26 07:02:39 +00:00
# if ENABLED(S_CURVE_ACCELERATION)
2018-06-10 23:02:54 +00:00
uint32_t cruise_rate , // The actual cruise rate to use, between end of the acceleration phase and start of deceleration phase
acceleration_time , // Acceleration time and deceleration time in STEP timer counts
deceleration_time ,
acceleration_time_inverse , // Inverse of acceleration and deceleration periods, expressed as integer. Scale depends on CPU being used
2018-04-11 23:13:42 +00:00
deceleration_time_inverse ;
2018-04-07 01:48:06 +00:00
# else
2018-05-09 05:17:53 +00:00
uint32_t acceleration_rate ; // The acceleration rate used for acceleration calculation
2018-04-07 01:48:06 +00:00
# endif
2016-04-28 01:06:32 +00:00
2016-10-30 21:08:46 +00:00
uint8_t direction_bits ; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
2016-04-28 01:06:32 +00:00
2016-05-04 19:10:42 +00:00
// Advance extrusion
# if ENABLED(LIN_ADVANCE)
bool use_advance_lead ;
2018-06-03 03:59:21 +00:00
uint16_t advance_speed , // STEP timer value for extruder speed offset ISR
2018-02-23 06:53:29 +00:00
max_adv_steps , // max. advance steps to get cruising speed pressure (not always nominal_speed!)
final_adv_steps ; // advance steps due to exit speed
float e_D_ratio ;
2011-11-15 21:50:43 +00:00
# endif
2016-10-30 21:08:46 +00:00
uint32_t nominal_rate , // The nominal step rate for this block in step_events/sec
initial_rate , // The jerk-adjusted step rate at start of block
final_rate , // The minimal rate at exit
acceleration_steps_per_s2 ; // acceleration steps/sec^2
2016-03-06 02:27:45 +00:00
2019-10-15 21:10:20 +00:00
# if HAS_CUTTER
cutter_power_t cutter_power ; // Power level for Spindle, Laser, etc.
# endif
2016-03-06 02:27:45 +00:00
# if FAN_COUNT > 0
2018-10-07 20:34:41 +00:00
uint8_t fan_speed [ FAN_COUNT ] ;
2016-03-06 02:27:45 +00:00
# endif
2015-07-31 05:30:29 +00:00
# if ENABLED(BARICUDA)
2017-07-18 03:01:20 +00:00
uint8_t valve_pressure , e_to_p_pressure ;
2013-06-06 22:49:25 +00:00
# endif
2016-12-15 15:21:32 +00:00
2019-08-17 02:17:10 +00:00
# if HAS_SPI_LCD
uint32_t segment_time_us ;
# endif
2016-03-06 02:27:45 +00:00
2019-09-10 23:52:41 +00:00
# if ENABLED(POWER_LOSS_RECOVERY)
uint32_t sdpos ;
# endif
2020-04-03 00:31:08 +00:00
# if ENABLED(LASER_POWER_INLINE)
block_laser_t laser ;
# endif
2011-11-15 21:50:43 +00:00
} block_t ;
2020-04-22 21:35:03 +00:00
# if ANY(LIN_ADVANCE, SCARA_FEEDRATE_SCALING, GRADIENT_MIX, LCD_SHOW_E_TOTAL)
# define HAS_POSITION_FLOAT 1
# endif
2018-04-05 20:47:56 +00:00
2015-03-21 03:42:49 +00:00
# define BLOCK_MOD(n) ((n)&(BLOCK_BUFFER_SIZE-1))
2013-09-29 16:20:06 +00:00
2020-04-03 00:31:08 +00:00
# if ENABLED(LASER_POWER_INLINE)
typedef struct {
/**
* Laser status bitmask ; most bits are unused ;
* 0 : Planner buffer enable
* 1 : Laser enable
* 2 : Reserved for direction
*/
uint8_t status ;
/**
* Laser power : 0 or 255 in case of PWM - less laser ,
* or the OCR value ;
*
* Using OCR instead of raw power ,
* as it avoids floating points during move loop
*/
uint8_t power ;
} settings_laser_t ;
# endif
2018-10-10 14:45:20 +00:00
typedef struct {
2019-09-26 06:28:09 +00:00
uint32_t max_acceleration_mm_per_s2 [ XYZE_N ] , // (mm/s^2) M201 XYZE
min_segment_time_us ; // (µs) M205 B
float axis_steps_per_mm [ XYZE_N ] ; // (steps) M92 XYZE - Steps per millimeter
feedRate_t max_feedrate_mm_s [ XYZE_N ] ; // (mm/s) M203 XYZE - Max speeds
float acceleration , // (mm/s^2) M204 S - Normal acceleration. DEFAULT ACCELERATION for all printing moves.
retract_acceleration , // (mm/s^2) M204 R - Retract acceleration. Filament pull-back and push-forward while standing still in the other axes
travel_acceleration ; // (mm/s^2) M204 T - Travel acceleration. DEFAULT ACCELERATION for all NON printing moves.
feedRate_t min_feedrate_mm_s , // (mm/s) M205 S - Minimum linear feedrate
min_travel_feedrate_mm_s ; // (mm/s) M205 T - Minimum travel feedrate
2020-04-03 00:31:08 +00:00
# if ENABLED(LASER_POWER_INLINE)
settings_laser_t laser ;
# endif
2018-10-10 14:45:20 +00:00
} planner_settings_t ;
2018-11-29 17:39:31 +00:00
# if DISABLED(SKEW_CORRECTION)
2018-10-10 14:45:20 +00:00
# define XY_SKEW_FACTOR 0
# define XZ_SKEW_FACTOR 0
# define YZ_SKEW_FACTOR 0
# endif
typedef struct {
# if ENABLED(SKEW_CORRECTION_GCODE)
float xy ;
# if ENABLED(SKEW_CORRECTION_FOR_Z)
float xz , yz ;
# else
const float xz = XZ_SKEW_FACTOR , yz = YZ_SKEW_FACTOR ;
# endif
# else
const float xy = XY_SKEW_FACTOR ,
xz = XZ_SKEW_FACTOR , yz = YZ_SKEW_FACTOR ;
# endif
} skew_factor_t ;
2016-04-28 01:06:32 +00:00
class Planner {
public :
2013-09-29 16:20:06 +00:00
2016-04-28 01:06:32 +00:00
/**
2017-12-09 03:38:45 +00:00
* The move buffer , calculated in stepper steps
*
* block_buffer is a ring buffer . . .
*
* head , tail : indexes for write , read
* head = = tail : the buffer is empty
* head ! = tail : blocks are in the buffer
* head = = ( tail - 1 ) % size : the buffer is full
*
2017-12-09 13:41:19 +00:00
* Writer of head is Planner : : buffer_segment ( ) .
2017-12-09 03:38:45 +00:00
* Reader of tail is Stepper : : isr ( ) . Always consider tail busy / read - only
2016-04-28 01:06:32 +00:00
*/
2016-05-26 18:22:51 +00:00
static block_t block_buffer [ BLOCK_BUFFER_SIZE ] ;
2017-12-09 03:38:45 +00:00
static volatile uint8_t block_buffer_head , // Index of the next block to be pushed
2018-06-27 23:11:16 +00:00
block_buffer_nonbusy , // Index of the first non busy block
block_buffer_planned , // Index of the optimally planned block
2017-12-09 03:38:45 +00:00
block_buffer_tail ; // Index of the busy block, if any
2018-05-13 03:49:54 +00:00
static uint16_t cleaning_buffer_counter ; // A counter to disable queuing of blocks
2018-06-27 23:11:16 +00:00
static uint8_t delay_before_delivering ; // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks
2016-05-26 18:22:51 +00:00
2016-12-04 04:02:27 +00:00
# if ENABLED(DISTINCT_E_FACTORS)
2017-12-09 03:38:45 +00:00
static uint8_t last_extruder ; // Respond to extruder change
2016-12-04 04:02:27 +00:00
# endif
2019-09-10 07:20:49 +00:00
# if EXTRUDERS
static int16_t flow_percentage [ EXTRUDERS ] ; // Extrusion factor for each extruder
static float e_factor [ EXTRUDERS ] ; // The flow percentage and volumetric multiplier combine to scale E movement
# endif
2017-12-20 01:44:11 +00:00
# if DISABLED(NO_VOLUMETRICS)
static float filament_size [ EXTRUDERS ] , // diameter of filament (in millimeters), typically around 1.75 or 2.85, 0 disables the volumetric calculations for the extruder
volumetric_area_nominal , // Nominal cross-sectional area
volumetric_multiplier [ EXTRUDERS ] ; // Reciprocal of cross-sectional area of filament (in mm^2). Pre-calculated to reduce computation in the planner
// May be auto-adjusted by a filament width sensor
# endif
2017-09-18 10:51:45 +00:00
2018-10-10 14:45:20 +00:00
static planner_settings_t settings ;
static uint32_t max_acceleration_steps_per_s2 [ XYZE_N ] ; // (steps/s^2) Derived from mm_per_s2
static float steps_to_mm [ XYZE_N ] ; // Millimeters per step
2016-04-28 01:06:32 +00:00
2020-04-24 01:49:11 +00:00
# if HAS_JUNCTION_DEVIATION
2018-06-11 23:49:08 +00:00
static float junction_deviation_mm ; // (mm) M205 J
# if ENABLED(LIN_ADVANCE)
2018-09-17 02:24:15 +00:00
static float max_e_jerk // Calculated from junction_deviation_mm
2020-04-24 22:41:20 +00:00
TERN_ ( DISTINCT_E_FACTORS , [ EXTRUDERS ] ) ;
2018-09-17 02:24:15 +00:00
# endif
# endif
# if HAS_CLASSIC_JERK
2020-04-24 22:41:20 +00:00
// (mm/s^2) M205 XYZ(E) - The largest speed change requiring no acceleration.
static TERN ( HAS_LINEAR_E_JERK , xyz_pos_t , xyze_pos_t ) max_jerk ;
2018-06-10 23:02:54 +00:00
# endif
2017-10-13 22:21:25 +00:00
# if HAS_LEVELING
2017-11-30 21:55:08 +00:00
static bool leveling_active ; // Flag that bed leveling is enabled
2017-05-21 00:23:39 +00:00
# if ABL_PLANAR
static matrix_3x3 bed_level_matrix ; // Transform to compensate for bed level
# endif
2017-10-13 22:21:25 +00:00
# if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
static float z_fade_height , inverse_z_fade_height ;
# endif
2017-12-06 00:41:56 +00:00
# else
static constexpr bool leveling_active = false ;
2016-11-26 05:32:47 +00:00
# endif
2017-04-22 03:30:36 +00:00
# if ENABLED(LIN_ADVANCE)
2018-09-11 03:37:32 +00:00
static float extruder_advance_K [ EXTRUDERS ] ;
2018-04-05 20:47:56 +00:00
# endif
2020-03-03 03:52:53 +00:00
/**
* The current position of the tool in absolute steps
* Recalculated if any axis_steps_per_mm are changed by gcode
*/
static xyze_long_t position ;
2018-04-05 20:47:56 +00:00
# if HAS_POSITION_FLOAT
2019-09-29 09:25:39 +00:00
static xyze_pos_t position_float ;
2017-04-22 03:30:36 +00:00
# endif
2018-09-17 02:24:15 +00:00
# if IS_KINEMATIC
2019-09-29 09:25:39 +00:00
static xyze_pos_t position_cart ;
2018-09-17 02:24:15 +00:00
# endif
2018-10-10 14:45:20 +00:00
static skew_factor_t skew_factor ;
2017-12-01 22:42:23 +00:00
2019-06-10 22:25:43 +00:00
# if ENABLED(SD_ABORT_ON_ENDSTOP_HIT)
2018-05-09 05:17:53 +00:00
static bool abort_on_endstop_hit ;
# endif
2016-04-28 01:06:32 +00:00
private :
2013-09-29 16:20:06 +00:00
2016-04-28 01:06:32 +00:00
/**
* Speed of previous path line segment
*/
2019-09-29 09:25:39 +00:00
static xyze_float_t previous_speed ;
2016-04-28 01:06:32 +00:00
/**
2018-05-09 05:17:53 +00:00
* Nominal speed of previous path line segment ( mm / s ) ^ 2
2016-04-28 01:06:32 +00:00
*/
2018-05-09 05:17:53 +00:00
static float previous_nominal_speed_sqr ;
2016-12-15 15:21:32 +00:00
2016-11-26 05:32:47 +00:00
/**
* Limit where 64 bit math is necessary for acceleration calculation
*/
static uint32_t cutoff_long ;
2016-04-28 01:06:32 +00:00
2017-10-13 16:07:09 +00:00
# if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
2017-11-03 04:59:42 +00:00
static float last_fade_z ;
2017-10-13 16:07:09 +00:00
# endif
2016-04-28 01:06:32 +00:00
# if ENABLED(DISABLE_INACTIVE_EXTRUDER)
/**
* Counters to manage disabling inactive extruders
*/
2016-05-26 18:22:51 +00:00
static uint8_t g_uc_extruder_last_move [ EXTRUDERS ] ;
2016-04-28 01:06:32 +00:00
# endif // DISABLE_INACTIVE_EXTRUDER
# ifdef XY_FREQUENCY_LIMIT
// Used for the frequency limit
2017-10-29 23:21:15 +00:00
# define MAX_FREQ_TIME_US (uint32_t)(1000000.0 / XY_FREQUENCY_LIMIT)
2016-04-28 01:06:32 +00:00
// Old direction bits. Used for speed calculations
2016-05-26 18:22:51 +00:00
static unsigned char old_direction_bits ;
2016-04-28 01:06:32 +00:00
// Segment times (in µs). Used for speed calculations
2019-09-29 09:25:39 +00:00
static xy_ulong_t axis_segment_time_us [ 3 ] ;
2016-10-31 15:17:30 +00:00
# endif
2016-04-28 01:06:32 +00:00
2019-07-29 23:45:01 +00:00
# if HAS_SPI_LCD
2016-12-09 13:10:55 +00:00
volatile static uint32_t block_buffer_runtime_us ; //Theoretical block buffer runtime in µs
2016-11-21 16:49:16 +00:00
# endif
2016-04-28 01:06:32 +00:00
public :
2016-05-26 18:22:51 +00:00
/**
* Instance Methods
*/
2016-04-28 01:06:32 +00:00
Planner ( ) ;
void init ( ) ;
2016-05-26 18:22:51 +00:00
/**
* Static ( class ) Methods
*/
static void reset_acceleration_rates ( ) ;
2016-07-24 02:36:26 +00:00
static void refresh_positioning ( ) ;
2019-10-02 00:59:48 +00:00
static void set_max_acceleration ( const uint8_t axis , float targetValue ) ;
static void set_max_feedrate ( const uint8_t axis , float targetValue ) ;
static void set_max_jerk ( const AxisEnum axis , float targetValue ) ;
2016-04-28 01:06:32 +00:00
2019-09-10 07:20:49 +00:00
# if EXTRUDERS
FORCE_INLINE static void refresh_e_factor ( const uint8_t e ) {
2020-04-23 02:03:28 +00:00
e_factor [ e ] = flow_percentage [ e ] * 0.01f * TERN ( NO_VOLUMETRICS , 1.0f , volumetric_multiplier [ e ] ) ;
2019-09-10 07:20:49 +00:00
}
2020-04-23 02:03:28 +00:00
static inline void set_flow ( const uint8_t e , const int16_t flow ) {
flow_percentage [ e ] = flow ;
refresh_e_factor ( e ) ;
}
2019-09-10 07:20:49 +00:00
# endif
2017-11-10 08:26:49 +00:00
2016-04-28 01:06:32 +00:00
// Manage fans, paste pressure, etc.
2016-05-26 18:22:51 +00:00
static void check_axes_activity ( ) ;
2015-03-29 03:33:21 +00:00
2017-09-18 10:51:45 +00:00
// Update multipliers based on new diameter measurements
static void calculate_volumetric_multipliers ( ) ;
2017-12-13 08:32:34 +00:00
# if ENABLED(FILAMENT_WIDTH_SENSOR)
2019-09-10 23:48:58 +00:00
void apply_filament_width_sensor ( const int8_t encoded_ratio ) ;
static inline float volumetric_percent ( const bool vol ) {
return 100.0f * ( vol
? volumetric_area_nominal / volumetric_multiplier [ FILAMENT_SENSOR_EXTRUDER_NUM ]
: volumetric_multiplier [ FILAMENT_SENSOR_EXTRUDER_NUM ]
) ;
}
2017-12-13 08:32:34 +00:00
# endif
2017-12-20 01:44:11 +00:00
# if DISABLED(NO_VOLUMETRICS)
FORCE_INLINE static void set_filament_size ( const uint8_t e , const float & v ) {
filament_size [ e ] = v ;
// make sure all extruders have some sane value for the filament size
2020-03-14 04:18:16 +00:00
LOOP_L_N ( i , COUNT ( filament_size ) )
2017-12-20 01:44:11 +00:00
if ( ! filament_size [ i ] ) filament_size [ i ] = DEFAULT_NOMINAL_FILAMENT_DIA ;
}
# endif
2017-09-18 10:51:45 +00:00
2017-10-13 15:39:11 +00:00
# if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
/**
* Get the Z leveling fade factor based on the given Z height ,
* re - calculating only when needed .
*
* Returns 1.0 if planner . z_fade_height is 0.0 .
* Returns 0.0 if Z is past the specified ' Fade Height ' .
*/
2018-08-14 04:55:12 +00:00
static inline float fade_scaling_factor_for_z ( const float & rz ) {
2018-07-01 20:20:28 +00:00
static float z_fade_factor = 1 ;
2019-09-26 06:15:35 +00:00
if ( ! z_fade_height ) return 1 ;
if ( rz > = z_fade_height ) return 0 ;
if ( last_fade_z ! = rz ) {
last_fade_z = rz ;
z_fade_factor = 1 - rz * inverse_z_fade_height ;
2017-10-13 15:39:11 +00:00
}
2019-09-26 06:15:35 +00:00
return z_fade_factor ;
2017-10-13 15:39:11 +00:00
}
2018-07-01 20:20:28 +00:00
FORCE_INLINE static void force_fade_recalc ( ) { last_fade_z = - 999.999f ; }
2017-10-13 16:07:09 +00:00
2017-10-13 22:21:25 +00:00
FORCE_INLINE static void set_z_fade_height ( const float & zfh ) {
z_fade_height = zfh > 0 ? zfh : 0 ;
inverse_z_fade_height = RECIPROCAL ( z_fade_height ) ;
force_fade_recalc ( ) ;
}
2017-11-03 04:59:42 +00:00
FORCE_INLINE static bool leveling_active_at_z ( const float & rz ) {
return ! z_fade_height | | rz < z_fade_height ;
2017-10-13 22:21:25 +00:00
}
2017-10-13 15:39:11 +00:00
# else
2019-10-01 02:44:07 +00:00
FORCE_INLINE static float fade_scaling_factor_for_z ( const float & ) { return 1 ; }
2017-10-13 15:39:11 +00:00
2019-10-01 02:44:07 +00:00
FORCE_INLINE static bool leveling_active_at_z ( const float & ) { return true ; }
2017-10-13 22:21:25 +00:00
2017-10-13 15:39:11 +00:00
# endif
2017-12-09 13:50:55 +00:00
# if ENABLED(SKEW_CORRECTION)
FORCE_INLINE static void skew ( float & cx , float & cy , const float & cz ) {
if ( WITHIN ( cx , X_MIN_POS + 1 , X_MAX_POS ) & & WITHIN ( cy , Y_MIN_POS + 1 , Y_MAX_POS ) ) {
2018-10-10 14:45:20 +00:00
const float sx = cx - cy * skew_factor . xy - cz * ( skew_factor . xz - ( skew_factor . xy * skew_factor . yz ) ) ,
sy = cy - cz * skew_factor . yz ;
2017-12-09 13:50:55 +00:00
if ( WITHIN ( sx , X_MIN_POS , X_MAX_POS ) & & WITHIN ( sy , Y_MIN_POS , Y_MAX_POS ) ) {
cx = sx ; cy = sy ;
}
}
}
2019-09-29 09:25:39 +00:00
FORCE_INLINE static void skew ( xyz_pos_t & raw ) { skew ( raw . x , raw . y , raw . z ) ; }
2017-12-09 13:50:55 +00:00
FORCE_INLINE static void unskew ( float & cx , float & cy , const float & cz ) {
if ( WITHIN ( cx , X_MIN_POS , X_MAX_POS ) & & WITHIN ( cy , Y_MIN_POS , Y_MAX_POS ) ) {
2018-10-10 14:45:20 +00:00
const float sx = cx + cy * skew_factor . xy + cz * skew_factor . xz ,
sy = cy + cz * skew_factor . yz ;
2017-12-09 13:50:55 +00:00
if ( WITHIN ( sx , X_MIN_POS , X_MAX_POS ) & & WITHIN ( sy , Y_MIN_POS , Y_MAX_POS ) ) {
cx = sx ; cy = sy ;
}
}
}
2019-09-29 09:25:39 +00:00
FORCE_INLINE static void unskew ( xyz_pos_t & raw ) { unskew ( raw . x , raw . y , raw . z ) ; }
2017-12-09 13:50:55 +00:00
# endif // SKEW_CORRECTION
2018-09-17 02:24:15 +00:00
# if HAS_LEVELING
2016-04-28 01:06:32 +00:00
/**
2016-09-12 02:40:44 +00:00
* Apply leveling to transform a cartesian position
* as it will be given to the planner and steppers .
2016-04-28 01:06:32 +00:00
*/
2019-09-29 09:25:39 +00:00
static void apply_leveling ( xyz_pos_t & raw ) ;
static void unapply_leveling ( xyz_pos_t & raw ) ;
FORCE_INLINE static void force_unapply_leveling ( xyz_pos_t & raw ) {
leveling_active = true ;
unapply_leveling ( raw ) ;
leveling_active = false ;
}
2016-09-12 02:40:44 +00:00
# endif
2016-12-15 15:21:32 +00:00
2018-09-17 02:24:15 +00:00
# if ENABLED(FWRETRACT)
static void apply_retract ( float & rz , float & e ) ;
2019-09-29 09:25:39 +00:00
FORCE_INLINE static void apply_retract ( xyze_pos_t & raw ) { apply_retract ( raw . z , raw . e ) ; }
2018-09-17 02:24:15 +00:00
static void unapply_retract ( float & rz , float & e ) ;
2019-09-29 09:25:39 +00:00
FORCE_INLINE static void unapply_retract ( xyze_pos_t & raw ) { unapply_retract ( raw . z , raw . e ) ; }
2018-09-17 02:24:15 +00:00
# endif
# if HAS_POSITION_MODIFIERS
2019-09-29 09:25:39 +00:00
FORCE_INLINE static void apply_modifiers ( xyze_pos_t & pos
2018-09-17 02:24:15 +00:00
# if HAS_LEVELING
2020-04-22 21:35:03 +00:00
, bool leveling = ENABLED ( PLANNER_LEVELING )
2018-09-17 02:24:15 +00:00
# endif
) {
2020-04-22 21:35:03 +00:00
TERN_ ( SKEW_CORRECTION , skew ( pos ) ) ;
TERN_ ( HAS_LEVELING , if ( leveling ) apply_leveling ( pos ) ) ;
TERN_ ( FWRETRACT , apply_retract ( pos ) ) ;
2018-09-17 02:24:15 +00:00
}
2019-09-29 09:25:39 +00:00
FORCE_INLINE static void unapply_modifiers ( xyze_pos_t & pos
2018-09-17 02:24:15 +00:00
# if HAS_LEVELING
2020-04-22 21:35:03 +00:00
, bool leveling = ENABLED ( PLANNER_LEVELING )
2018-09-17 02:24:15 +00:00
# endif
) {
2020-04-22 21:35:03 +00:00
TERN_ ( FWRETRACT , unapply_retract ( pos ) ) ;
TERN_ ( HAS_LEVELING , if ( leveling ) unapply_leveling ( pos ) ) ;
TERN_ ( SKEW_CORRECTION , unskew ( pos ) ) ;
2018-09-17 02:24:15 +00:00
}
# endif // HAS_POSITION_MODIFIERS
2018-06-27 23:11:16 +00:00
// Number of moves currently in the planner including the busy block, if any
2018-05-09 05:17:53 +00:00
FORCE_INLINE static uint8_t movesplanned ( ) { return BLOCK_MOD ( block_buffer_head - block_buffer_tail ) ; }
2018-06-27 23:11:16 +00:00
// Number of nonbusy moves currently in the planner
FORCE_INLINE static uint8_t nonbusy_movesplanned ( ) { return BLOCK_MOD ( block_buffer_head - block_buffer_nonbusy ) ; }
2018-05-09 05:17:53 +00:00
// Remove all blocks from the buffer
2018-06-27 23:11:16 +00:00
FORCE_INLINE static void clear_block_buffer ( ) { block_buffer_nonbusy = block_buffer_planned = block_buffer_head = block_buffer_tail = 0 ; }
2018-05-09 05:17:53 +00:00
// Check if movement queue is full
FORCE_INLINE static bool is_full ( ) { return block_buffer_tail = = next_block_index ( block_buffer_head ) ; }
// Get count of movement slots free
FORCE_INLINE static uint8_t moves_free ( ) { return BLOCK_BUFFER_SIZE - 1 - movesplanned ( ) ; }
2018-05-03 22:45:13 +00:00
/**
* Planner : : get_next_free_block
*
2018-05-09 05:17:53 +00:00
* - Get the next head indices ( passed by reference )
* - Wait for the number of spaces to open up in the planner
* - Return the first head block
2018-05-03 22:45:13 +00:00
*/
2018-05-21 03:20:11 +00:00
FORCE_INLINE static block_t * get_next_free_block ( uint8_t & next_buffer_head , const uint8_t count = 1 ) {
2018-05-09 05:17:53 +00:00
// Wait until there are enough slots free
while ( moves_free ( ) < count ) { idle ( ) ; }
// Return the first available block
2018-05-03 22:45:13 +00:00
next_buffer_head = next_block_index ( block_buffer_head ) ;
return & block_buffer [ block_buffer_head ] ;
}
2017-11-30 22:40:42 +00:00
/**
* Planner : : _buffer_steps
*
* Add a new linear movement to the buffer ( in terms of steps ) .
*
* target - target position in steps units
* fr_mm_s - ( target ) speed of the move
* extruder - target extruder
2018-02-04 06:26:05 +00:00
* millimeters - the length of the movement , if known
2018-05-09 05:17:53 +00:00
*
* Returns true if movement was buffered , false otherwise
2017-11-30 22:40:42 +00:00
*/
2019-09-29 09:25:39 +00:00
static bool _buffer_steps ( const xyze_long_t & target
2018-05-09 05:17:53 +00:00
# if HAS_POSITION_FLOAT
2019-09-29 09:25:39 +00:00
, const xyze_pos_t & target_float
2018-09-17 02:24:15 +00:00
# endif
2020-03-25 22:55:36 +00:00
# if HAS_DIST_MM_ARG
, const xyze_float_t & cart_dist_mm
2018-05-09 05:17:53 +00:00
# endif
2019-09-26 06:28:09 +00:00
, feedRate_t fr_mm_s , const uint8_t extruder , const float & millimeters = 0.0
2018-05-09 05:17:53 +00:00
) ;
/**
* Planner : : _populate_block
*
* Fills a new linear movement in the block ( in terms of steps ) .
*
* target - target position in steps units
* fr_mm_s - ( target ) speed of the move
* extruder - target extruder
* millimeters - the length of the movement , if known
*
* Returns true is movement is acceptable , false otherwise
*/
static bool _populate_block ( block_t * const block , bool split_move ,
2019-09-29 09:25:39 +00:00
const xyze_long_t & target
2018-04-05 20:47:56 +00:00
# if HAS_POSITION_FLOAT
2019-09-29 09:25:39 +00:00
, const xyze_pos_t & target_float
2018-02-23 06:53:29 +00:00
# endif
2020-03-25 22:55:36 +00:00
# if HAS_DIST_MM_ARG
, const xyze_float_t & cart_dist_mm
2018-09-17 02:24:15 +00:00
# endif
2019-09-26 06:28:09 +00:00
, feedRate_t fr_mm_s , const uint8_t extruder , const float & millimeters = 0.0
2018-02-23 06:53:29 +00:00
) ;
2017-11-30 22:40:42 +00:00
2018-05-03 22:45:13 +00:00
/**
* Planner : : buffer_sync_block
* Add a block to the buffer that just updates the position
*/
static void buffer_sync_block ( ) ;
2018-09-17 02:24:15 +00:00
# if IS_KINEMATIC
private :
// Allow do_homing_move to access internal functions, such as buffer_segment.
2019-09-26 06:28:09 +00:00
friend void do_homing_move ( const AxisEnum , const float , const feedRate_t ) ;
2018-09-17 02:24:15 +00:00
# endif
2016-09-12 02:40:44 +00:00
/**
2017-12-09 13:41:19 +00:00
* Planner : : buffer_segment
2016-10-06 08:56:05 +00:00
*
2017-11-30 21:55:08 +00:00
* Add a new linear movement to the buffer in axis units .
2016-10-09 18:25:25 +00:00
*
2017-11-30 21:55:08 +00:00
* Leveling and kinematics should be applied ahead of calling this .
2016-10-06 08:56:05 +00:00
*
2018-02-04 06:26:05 +00:00
* a , b , c , e - target positions in mm and / or degrees
* fr_mm_s - ( target ) speed of the move
* extruder - target extruder
* millimeters - the length of the movement , if known
2016-10-06 08:56:05 +00:00
*/
2018-09-17 02:24:15 +00:00
static bool buffer_segment ( const float & a , const float & b , const float & c , const float & e
2020-03-25 22:55:36 +00:00
# if HAS_DIST_MM_ARG
, const xyze_float_t & cart_dist_mm
2018-09-17 02:24:15 +00:00
# endif
2019-09-26 06:28:09 +00:00
, const feedRate_t & fr_mm_s , const uint8_t extruder , const float & millimeters = 0.0
2018-09-17 02:24:15 +00:00
) ;
2016-10-06 08:56:05 +00:00
2019-09-29 09:25:39 +00:00
FORCE_INLINE static bool buffer_segment ( abce_pos_t & abce
2020-03-25 22:55:36 +00:00
# if HAS_DIST_MM_ARG
, const xyze_float_t & cart_dist_mm
2016-10-06 08:56:05 +00:00
# endif
2019-09-26 06:28:09 +00:00
, const feedRate_t & fr_mm_s , const uint8_t extruder , const float & millimeters = 0.0
2018-09-17 02:24:15 +00:00
) {
2019-09-29 09:25:39 +00:00
return buffer_segment ( abce . a , abce . b , abce . c , abce . e
2020-03-25 22:55:36 +00:00
# if HAS_DIST_MM_ARG
, cart_dist_mm
2018-09-17 02:24:15 +00:00
# endif
, fr_mm_s , extruder , millimeters ) ;
2016-10-06 08:56:05 +00:00
}
2018-09-17 02:24:15 +00:00
public :
2016-10-06 08:56:05 +00:00
/**
* Add a new linear movement to the buffer .
2019-09-26 06:28:09 +00:00
* The target is cartesian . It ' s translated to
* delta / scara if needed .
2018-09-17 02:24:15 +00:00
*
* rx , ry , rz , e - target position in mm or degrees
2018-02-04 06:26:05 +00:00
* fr_mm_s - ( target ) speed of the move ( mm / s )
* extruder - target extruder
* millimeters - the length of the movement , if known
2018-09-17 22:30:04 +00:00
* inv_duration - the reciprocal if the duration of the movement , if known ( kinematic only if feeedrate scaling is enabled )
2016-10-06 08:56:05 +00:00
*/
2019-09-26 06:28:09 +00:00
static bool buffer_line ( const float & rx , const float & ry , const float & rz , const float & e , const feedRate_t & fr_mm_s , const uint8_t extruder , const float millimeters = 0.0
2018-09-17 02:24:15 +00:00
# if ENABLED(SCARA_FEEDRATE_SCALING)
, const float & inv_duration = 0.0
2016-10-06 08:56:05 +00:00
# endif
2018-09-17 02:24:15 +00:00
) ;
2019-09-29 09:25:39 +00:00
FORCE_INLINE static bool buffer_line ( const xyze_pos_t & cart , const feedRate_t & fr_mm_s , const uint8_t extruder , const float millimeters = 0.0
2018-09-17 02:24:15 +00:00
# if ENABLED(SCARA_FEEDRATE_SCALING)
, const float & inv_duration = 0.0
2016-10-06 08:56:05 +00:00
# endif
2018-09-17 02:24:15 +00:00
) {
2019-09-29 09:25:39 +00:00
return buffer_line ( cart . x , cart . y , cart . z , cart . e , fr_mm_s , extruder , millimeters
2018-09-17 02:24:15 +00:00
# if ENABLED(SCARA_FEEDRATE_SCALING)
, inv_duration
# endif
) ;
2016-10-06 08:56:05 +00:00
}
2016-04-28 01:06:32 +00:00
2016-09-12 02:40:44 +00:00
/**
* Set the planner . position and individual stepper positions .
* Used by G92 , G28 , G29 , and other procedures .
2018-09-17 22:30:04 +00:00
*
2018-09-17 02:24:15 +00:00
* The supplied position is in the cartesian coordinate space and is
* translated in to machine space as needed . Modifiers such as leveling
* and skew are also applied .
2016-09-12 02:40:44 +00:00
*
* Multiplies by axis_steps_per_mm [ ] and does necessary conversion
* for COREXY / COREXZ / COREYZ to set the corresponding stepper positions .
*
* Clears previous speed values .
*/
2018-09-17 02:24:15 +00:00
static void set_position_mm ( const float & rx , const float & ry , const float & rz , const float & e ) ;
2019-09-29 09:25:39 +00:00
FORCE_INLINE static void set_position_mm ( const xyze_pos_t & cart ) { set_position_mm ( cart . x , cart . y , cart . z , cart . e ) ; }
2018-09-17 02:24:15 +00:00
static void set_e_position_mm ( const float & e ) ;
/**
* Set the planner . position and individual stepper positions .
2018-09-17 22:30:04 +00:00
*
2018-09-17 02:24:15 +00:00
* The supplied position is in machine space , and no additional
* conversions are applied .
*/
static void set_machine_position_mm ( const float & a , const float & b , const float & c , const float & e ) ;
2019-09-29 09:25:39 +00:00
FORCE_INLINE static void set_machine_position_mm ( const abce_pos_t & abce ) { set_machine_position_mm ( abce . a , abce . b , abce . c , abce . e ) ; }
2016-04-28 01:06:32 +00:00
2018-05-12 14:59:11 +00:00
/**
* Get an axis position according to stepper position ( s )
* For CORE machines apply translation from ABC to XYZ .
*/
static float get_axis_position_mm ( const AxisEnum axis ) ;
2020-03-03 03:52:53 +00:00
static inline abce_pos_t get_axis_positions_mm ( ) {
const abce_pos_t out = {
get_axis_position_mm ( A_AXIS ) ,
get_axis_position_mm ( B_AXIS ) ,
get_axis_position_mm ( C_AXIS ) ,
get_axis_position_mm ( E_AXIS )
} ;
return out ;
}
2018-05-12 14:59:11 +00:00
// SCARA AB axes are in degrees, not mm
# if IS_SCARA
FORCE_INLINE static float get_axis_position_degrees ( const AxisEnum axis ) { return get_axis_position_mm ( axis ) ; }
# endif
2019-08-28 05:51:01 +00:00
// Called to force a quick stop of the machine (for example, when
// a Full Shutdown is required, or when endstops are hit)
2018-05-09 05:17:53 +00:00
static void quick_stop ( ) ;
// Called when an endstop is triggered. Causes the machine to stop inmediately
static void endstop_triggered ( const AxisEnum axis ) ;
2016-04-28 01:06:32 +00:00
2018-05-09 05:17:53 +00:00
// Triggered position of an axis in mm (not core-savvy)
static float triggered_position_mm ( const AxisEnum axis ) ;
// Block until all buffered steps are executed / cleaned
2018-05-12 06:38:02 +00:00
static void synchronize ( ) ;
2018-05-09 05:17:53 +00:00
// Wait for moves to finish and disable all steppers
static void finish_and_disable ( ) ;
// Periodic tick to handle cleaning timeouts
// Called from the Temperature ISR at ~1kHz
static void tick ( ) {
2020-04-12 02:27:49 +00:00
if ( cleaning_buffer_counter ) - - cleaning_buffer_counter ;
2016-04-28 01:06:32 +00:00
}
2017-12-08 05:03:36 +00:00
/**
2018-05-09 05:17:53 +00:00
* Does the buffer have any blocks queued ?
2017-12-08 05:03:36 +00:00
*/
2018-05-09 05:17:53 +00:00
FORCE_INLINE static bool has_blocks_queued ( ) { return ( block_buffer_head ! = block_buffer_tail ) ; }
2017-12-08 05:03:36 +00:00
2016-04-28 01:06:32 +00:00
/**
2020-02-14 11:14:37 +00:00
* Get the current block for processing
* and mark the block as busy .
* Return nullptr if the buffer is empty
* or if there is a first - block delay .
*
2017-12-03 02:17:51 +00:00
* WARNING : Called from Stepper ISR context !
2016-04-28 01:06:32 +00:00
*/
2020-02-14 11:14:37 +00:00
static block_t * get_current_block ( ) ;
2016-04-28 01:06:32 +00:00
2018-05-09 05:17:53 +00:00
/**
* " Discard " the block and " release " the memory .
* Called when the current block is no longer needed .
*/
FORCE_INLINE static void discard_current_block ( ) {
2018-06-27 23:11:16 +00:00
if ( has_blocks_queued ( ) )
block_buffer_tail = next_block_index ( block_buffer_tail ) ;
2018-05-09 05:17:53 +00:00
}
2019-07-29 23:45:01 +00:00
# if HAS_SPI_LCD
2020-02-14 11:14:37 +00:00
static uint16_t block_buffer_runtime ( ) ;
static void clear_block_buffer_runtime ( ) ;
2016-11-11 17:15:39 +00:00
# endif
2016-05-04 00:00:28 +00:00
# if ENABLED(AUTOTEMP)
2016-11-12 00:11:04 +00:00
static float autotemp_min , autotemp_max , autotemp_factor ;
2016-05-26 18:22:51 +00:00
static bool autotemp_enabled ;
static void getHighESpeed ( ) ;
2016-11-14 10:29:45 +00:00
static void autotemp_M104_M109 ( ) ;
2016-05-04 00:00:28 +00:00
# endif
2019-10-09 00:42:18 +00:00
# if HAS_LINEAR_E_JERK
2018-06-27 02:38:31 +00:00
FORCE_INLINE static void recalculate_max_e_jerk ( ) {
# define GET_MAX_E_JERK(N) SQRT(SQRT(0.5) * junction_deviation_mm * (N) * RECIPROCAL(1.0 - SQRT(0.5)))
2018-09-17 02:24:15 +00:00
# if ENABLED(DISTINCT_E_FACTORS)
2020-03-14 04:18:16 +00:00
LOOP_L_N ( i , EXTRUDERS )
2019-01-12 01:37:20 +00:00
max_e_jerk [ i ] = GET_MAX_E_JERK ( settings . max_acceleration_mm_per_s2 [ E_AXIS_N ( i ) ] ) ;
2018-09-17 02:24:15 +00:00
# else
2018-10-10 14:45:20 +00:00
max_e_jerk = GET_MAX_E_JERK ( settings . max_acceleration_mm_per_s2 [ E_AXIS ] ) ;
2018-06-11 23:49:08 +00:00
# endif
}
# endif
2016-05-04 00:00:28 +00:00
private :
2016-04-28 01:06:32 +00:00
/**
* Get the index of the next / previous block in the ring buffer
*/
2018-05-18 07:04:01 +00:00
static constexpr uint8_t next_block_index ( const uint8_t block_index ) { return BLOCK_MOD ( block_index + 1 ) ; }
static constexpr uint8_t prev_block_index ( const uint8_t block_index ) { return BLOCK_MOD ( block_index - 1 ) ; }
2016-04-28 01:06:32 +00:00
/**
* Calculate the distance ( not time ) it takes to accelerate
* from initial_rate to target_rate using the given acceleration :
*/
2016-10-27 10:53:24 +00:00
static float estimate_acceleration_distance ( const float & initial_rate , const float & target_rate , const float & accel ) {
2016-06-07 22:38:45 +00:00
if ( accel = = 0 ) return 0 ; // accel was 0, set acceleration distance to 0
2016-07-16 01:50:25 +00:00
return ( sq ( target_rate ) - sq ( initial_rate ) ) / ( accel * 2 ) ;
2016-04-28 01:06:32 +00:00
}
/**
2017-12-03 02:17:51 +00:00
* Return the point at which you must start braking ( at the rate of - ' accel ' ) if
2016-04-28 01:06:32 +00:00
* you start at ' initial_rate ' , accelerate ( until reaching the point ) , and want to end at
* ' final_rate ' after traveling ' distance ' .
*
* This is used to compute the intersection point between acceleration and deceleration
* in cases where the " trapezoid " has no plateau ( i . e . , never reaches maximum speed )
*/
2016-10-27 10:53:24 +00:00
static float intersection_distance ( const float & initial_rate , const float & final_rate , const float & accel , const float & distance ) {
2016-06-07 22:38:45 +00:00
if ( accel = = 0 ) return 0 ; // accel was 0, set intersection distance to 0
2016-07-16 01:50:25 +00:00
return ( accel * 2 * distance - sq ( initial_rate ) + sq ( final_rate ) ) / ( accel * 4 ) ;
2016-04-28 01:06:32 +00:00
}
/**
2018-05-09 05:17:53 +00:00
* Calculate the maximum allowable speed squared at this point , in order
* to reach ' target_velocity_sqr ' using ' acceleration ' within a given
2016-04-28 01:06:32 +00:00
* ' distance ' .
*/
2018-05-09 05:17:53 +00:00
static float max_allowable_speed_sqr ( const float & accel , const float & target_velocity_sqr , const float & distance ) {
return target_velocity_sqr - 2 * accel * distance ;
2016-04-28 01:06:32 +00:00
}
2018-05-26 07:02:39 +00:00
# if ENABLED(S_CURVE_ACCELERATION)
2018-04-07 01:48:06 +00:00
/**
* Calculate the speed reached given initial speed , acceleration and distance
*/
static float final_speed ( const float & initial_velocity , const float & accel , const float & distance ) {
return SQRT ( sq ( initial_velocity ) + 2 * accel * distance ) ;
}
# endif
2016-10-11 04:17:49 +00:00
static void calculate_trapezoid_for_block ( block_t * const block , const float & entry_factor , const float & exit_factor ) ;
2016-04-28 01:06:32 +00:00
2017-12-09 03:38:45 +00:00
static void reverse_pass_kernel ( block_t * const current , const block_t * const next ) ;
2018-05-18 07:04:01 +00:00
static void forward_pass_kernel ( const block_t * const previous , block_t * const current , uint8_t block_index ) ;
2016-04-28 01:06:32 +00:00
2016-05-26 18:22:51 +00:00
static void reverse_pass ( ) ;
static void forward_pass ( ) ;
2016-04-28 01:06:32 +00:00
2016-05-26 18:22:51 +00:00
static void recalculate_trapezoids ( ) ;
2016-04-28 01:06:32 +00:00
2016-05-26 18:22:51 +00:00
static void recalculate ( ) ;
2011-11-19 14:37:10 +00:00
2020-04-24 01:49:11 +00:00
# if HAS_JUNCTION_DEVIATION
2018-06-01 01:09:53 +00:00
2019-09-29 09:25:39 +00:00
FORCE_INLINE static void normalize_junction_vector ( xyze_float_t & vector ) {
2018-07-01 20:20:28 +00:00
float magnitude_sq = 0 ;
2018-06-30 03:42:05 +00:00
LOOP_XYZE ( idx ) if ( vector [ idx ] ) magnitude_sq + = sq ( vector [ idx ] ) ;
2019-09-29 09:25:39 +00:00
vector * = RSQRT ( magnitude_sq ) ;
2018-06-01 01:09:53 +00:00
}
2019-09-29 09:25:39 +00:00
FORCE_INLINE static float limit_value_by_axis_maximum ( const float & max_value , xyze_float_t & unit_vec ) {
2018-06-01 01:09:53 +00:00
float limit_value = max_value ;
2018-06-30 03:42:05 +00:00
LOOP_XYZE ( idx ) if ( unit_vec [ idx ] ) // Avoid divide by zero
2018-10-10 14:45:20 +00:00
NOMORE ( limit_value , ABS ( settings . max_acceleration_mm_per_s2 [ idx ] / unit_vec [ idx ] ) ) ;
2018-06-01 01:09:53 +00:00
return limit_value ;
}
2019-10-09 00:42:18 +00:00
# endif // !CLASSIC_JERK
2016-04-28 01:06:32 +00:00
} ;
2015-03-21 03:42:49 +00:00
2019-07-05 23:01:21 +00:00
# define PLANNER_XY_FEEDRATE() (_MIN(planner.settings.max_feedrate_mm_s[X_AXIS], planner.settings.max_feedrate_mm_s[Y_AXIS]))
2017-05-01 20:17:40 +00:00
2016-10-10 20:34:35 +00:00
extern Planner planner ;