/** * Marlin 3D Printer Firmware * Copyright (c) 2020 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 . * */ #pragma once /** * planner.h * * Buffer movement commands and manage the acceleration profile plan * * Derived from Grbl * Copyright (c) 2009-2011 Simen Svale Skogsrud */ #include "../MarlinCore.h" #if ENABLED(JD_HANDLE_SMALL_SEGMENTS) // Enable this option for perfect accuracy but maximum // computation. Should be fine on ARM processors. //#define JD_USE_MATH_ACOS // Disable this option to save 120 bytes of PROGMEM, // but incur increased computation and a reduction // in accuracy. #define JD_USE_LOOKUP_TABLE #endif #include "motion.h" #include "../gcode/queue.h" #if ENABLED(DELTA) #include "delta.h" #elif ENABLED(POLARGRAPH) #include "polargraph.h" #endif #if ABL_PLANAR #include "../libs/vector_3.h" // for matrix_3x3 #endif #if ENABLED(FWRETRACT) #include "../feature/fwretract.h" #endif #if ENABLED(MIXING_EXTRUDER) #include "../feature/mixing.h" #endif #if HAS_CUTTER #include "../feature/spindle_laser_types.h" #endif #if ENABLED(DIRECT_STEPPING) #include "../feature/direct_stepping.h" #define IS_PAGE(B) TEST(B->flag, BLOCK_BIT_IS_PAGE) #else #define IS_PAGE(B) false #endif #if ENABLED(EXTERNAL_CLOSED_LOOP_CONTROLLER) #include "../feature/closedloop.h" #endif // Feedrate for manual moves #ifdef MANUAL_FEEDRATE constexpr xyze_feedrate_t _mf = MANUAL_FEEDRATE, manual_feedrate_mm_s = LOGICAL_AXIS_ARRAY(_mf.e / 60.0f, _mf.x / 60.0f, _mf.y / 60.0f, _mf.z / 60.0f, _mf.i / 60.0f, _mf.j / 60.0f, _mf.k / 60.0f); #endif #if IS_KINEMATIC && HAS_JUNCTION_DEVIATION #define HAS_DIST_MM_ARG 1 #endif enum BlockFlagBit : char { // 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, // The block is segment 2+ of a longer move BLOCK_BIT_CONTINUED, // Sync the stepper counts from the block BLOCK_BIT_SYNC_POSITION // Direct stepping page #if ENABLED(DIRECT_STEPPING) , BLOCK_BIT_IS_PAGE #endif // Sync the fan speeds from the block #if ENABLED(LASER_SYNCHRONOUS_M106_M107) , BLOCK_BIT_SYNC_FANS #endif }; enum BlockFlag : char { BLOCK_FLAG_RECALCULATE = _BV(BLOCK_BIT_RECALCULATE) , BLOCK_FLAG_NOMINAL_LENGTH = _BV(BLOCK_BIT_NOMINAL_LENGTH) , BLOCK_FLAG_CONTINUED = _BV(BLOCK_BIT_CONTINUED) , BLOCK_FLAG_SYNC_POSITION = _BV(BLOCK_BIT_SYNC_POSITION) #if ENABLED(DIRECT_STEPPING) , BLOCK_FLAG_IS_PAGE = _BV(BLOCK_BIT_IS_PAGE) #endif #if ENABLED(LASER_SYNCHRONOUS_M106_M107) , BLOCK_FLAG_SYNC_FANS = _BV(BLOCK_BIT_SYNC_FANS) #endif }; #define BLOCK_MASK_SYNC ( BLOCK_FLAG_SYNC_POSITION | TERN0(LASER_SYNCHRONOUS_M106_M107, BLOCK_FLAG_SYNC_FANS) ) #if ENABLED(LASER_POWER_INLINE) typedef struct { bool isPlanned:1; bool isEnabled:1; bool dir:1; bool Reserved:6; } power_status_t; typedef struct { power_status_t status; // See planner settings for meaning uint8_t 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 /** * 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. */ typedef struct block_t { volatile uint8_t flag; // Block flags (See BlockFlag enum above) - Modified by ISR and main thread! // 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 union { abce_ulong_t steps; // Step count along each axis abce_long_t position; // New position to force when this sync block is executed }; uint32_t step_event_count; // The number of step events required to complete this block #if HAS_MULTI_EXTRUDER uint8_t extruder; // The extruder to move (if E move) #else static constexpr uint8_t extruder = 0; #endif #if ENABLED(MIXING_EXTRUDER) mixer_comp_t b_color[MIXING_STEPPERS]; // Normalized color for the mixing steppers #endif // Settings for the trapezoid generator 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 #if ENABLED(S_CURVE_ACCELERATION) 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 deceleration_time_inverse; #else uint32_t acceleration_rate; // The acceleration rate used for acceleration calculation #endif axis_bits_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) // Advance extrusion #if ENABLED(LIN_ADVANCE) bool use_advance_lead; uint16_t advance_speed, // STEP timer value for extruder speed offset ISR 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; #endif 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 #if ENABLED(DIRECT_STEPPING) page_idx_t page_idx; // Page index used for direct stepping #endif #if HAS_CUTTER cutter_power_t cutter_power; // Power level for Spindle, Laser, etc. #endif #if HAS_FAN uint8_t fan_speed[FAN_COUNT]; #endif #if ENABLED(BARICUDA) uint8_t valve_pressure, e_to_p_pressure; #endif #if HAS_WIRED_LCD uint32_t segment_time_us; #endif #if ENABLED(POWER_LOSS_RECOVERY) uint32_t sdpos; xyze_pos_t start_position; #endif #if ENABLED(LASER_POWER_INLINE) block_laser_t laser; #endif } block_t; #if ANY(LIN_ADVANCE, SCARA_FEEDRATE_SCALING, GRADIENT_MIX, LCD_SHOW_E_TOTAL, POWER_LOSS_RECOVERY) #define HAS_POSITION_FLOAT 1 #endif #define BLOCK_MOD(n) ((n)&(BLOCK_BUFFER_SIZE-1)) #if ENABLED(LASER_POWER_INLINE) typedef struct { /** * Laser status flags */ power_status_t status; /** * Laser power: 0 or 255 in case of PWM-less laser, * or the OCR (oscillator count register) value; * * Using OCR instead of raw power, because it avoids * floating point operations during the move loop. */ uint8_t power; } laser_state_t; #endif typedef struct { uint32_t max_acceleration_mm_per_s2[DISTINCT_AXES], // (mm/s^2) M201 XYZE min_segment_time_us; // (µs) M205 B float axis_steps_per_mm[DISTINCT_AXES]; // (steps) M92 XYZE - Steps per millimeter feedRate_t max_feedrate_mm_s[DISTINCT_AXES]; // (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 } planner_settings_t; #if ENABLED(IMPROVE_HOMING_RELIABILITY) struct motion_state_t { TERN(DELTA, xyz_ulong_t, xy_ulong_t) acceleration; #if HAS_CLASSIC_JERK TERN(DELTA, xyz_float_t, xy_float_t) jerk_state; #endif }; #endif #if DISABLED(SKEW_CORRECTION) #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; #if ENABLED(DISABLE_INACTIVE_EXTRUDER) typedef IF<(BLOCK_BUFFER_SIZE > 64), uint16_t, uint8_t>::type last_move_t; #endif class Planner { public: /** * 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 * * Writer of head is Planner::buffer_segment(). * Reader of tail is Stepper::isr(). Always consider tail busy / read-only */ static block_t block_buffer[BLOCK_BUFFER_SIZE]; static volatile uint8_t block_buffer_head, // Index of the next block to be pushed block_buffer_nonbusy, // Index of the first non busy block block_buffer_planned, // Index of the optimally planned block block_buffer_tail; // Index of the busy block, if any static uint16_t cleaning_buffer_counter; // A counter to disable queuing of blocks static uint8_t delay_before_delivering; // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks #if ENABLED(DISTINCT_E_FACTORS) static uint8_t last_extruder; // Respond to extruder change #endif #if ENABLED(DIRECT_STEPPING) static uint32_t last_page_step_rate; // Last page step rate given static xyze_bool_t last_page_dir; // Last page direction given #endif #if HAS_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 #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 #if ENABLED(VOLUMETRIC_EXTRUDER_LIMIT) static float volumetric_extruder_limit[EXTRUDERS], // Maximum mm^3/sec the extruder can handle volumetric_extruder_feedrate_limit[EXTRUDERS]; // Feedrate limit (mm/s) calculated from volume limit #endif static planner_settings_t settings; #if ENABLED(LASER_POWER_INLINE) static laser_state_t laser_inline; #endif static uint32_t max_acceleration_steps_per_s2[DISTINCT_AXES]; // (steps/s^2) Derived from mm_per_s2 static float mm_per_step[DISTINCT_AXES]; // Millimeters per step #if HAS_JUNCTION_DEVIATION static float junction_deviation_mm; // (mm) M205 J #if HAS_LINEAR_E_JERK static float max_e_jerk[DISTINCT_E]; // Calculated from junction_deviation_mm #endif #endif #if HAS_CLASSIC_JERK // (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; #endif #if HAS_LEVELING static bool leveling_active; // Flag that bed leveling is enabled #if ABL_PLANAR static matrix_3x3 bed_level_matrix; // Transform to compensate for bed level #endif #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) static float z_fade_height, inverse_z_fade_height; #endif #else static constexpr bool leveling_active = false; #endif #if ENABLED(LIN_ADVANCE) static float extruder_advance_K[EXTRUDERS]; #endif /** * 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; #if HAS_POSITION_FLOAT static xyze_pos_t position_float; #endif #if IS_KINEMATIC static xyze_pos_t position_cart; #endif static skew_factor_t skew_factor; #if ENABLED(SD_ABORT_ON_ENDSTOP_HIT) static bool abort_on_endstop_hit; #endif #ifdef XY_FREQUENCY_LIMIT static int8_t xy_freq_limit_hz; // Minimum XY frequency setting static float xy_freq_min_speed_factor; // Minimum speed factor setting static int32_t xy_freq_min_interval_us; // Minimum segment time based on xy_freq_limit_hz static void refresh_frequency_limit() { //xy_freq_min_interval_us = xy_freq_limit_hz ?: LROUND(1000000.0f / xy_freq_limit_hz); if (xy_freq_limit_hz) xy_freq_min_interval_us = LROUND(1000000.0f / xy_freq_limit_hz); } static void set_min_speed_factor_u8(const uint8_t v255) { xy_freq_min_speed_factor = float(ui8_to_percent(v255)) / 100; } static void set_frequency_limit(const uint8_t hz) { xy_freq_limit_hz = constrain(hz, 0, 100); refresh_frequency_limit(); } #endif private: /** * Speed of previous path line segment */ static xyze_float_t previous_speed; /** * Nominal speed of previous path line segment (mm/s)^2 */ static float previous_nominal_speed_sqr; /** * Limit where 64bit math is necessary for acceleration calculation */ static uint32_t acceleration_long_cutoff; #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) static float last_fade_z; #endif #if ENABLED(DISABLE_INACTIVE_EXTRUDER) // Counters to manage disabling inactive extruder steppers static last_move_t g_uc_extruder_last_move[E_STEPPERS]; #endif #if HAS_WIRED_LCD volatile static uint32_t block_buffer_runtime_us; // Theoretical block buffer runtime in µs #endif public: /** * Instance Methods */ Planner(); void init(); /** * Static (class) Methods */ // Recalculate steps/s^2 accelerations based on mm/s^2 settings static void reset_acceleration_rates(); /** * Recalculate 'position' and 'mm_per_step'. * Must be called whenever settings.axis_steps_per_mm changes! */ static void refresh_positioning(); // For an axis set the Maximum Acceleration in mm/s^2 static void set_max_acceleration(const uint8_t axis, float inMaxAccelMMS2); // For an axis set the Maximum Feedrate in mm/s static void set_max_feedrate(const uint8_t axis, float inMaxFeedrateMMS); // For an axis set the Maximum Jerk (instant change) in mm/s #if HAS_CLASSIC_JERK static void set_max_jerk(const AxisEnum axis, float inMaxJerkMMS); #else static void set_max_jerk(const AxisEnum, const_float_t) {} #endif #if HAS_EXTRUDERS FORCE_INLINE static void refresh_e_factor(const uint8_t e) { e_factor[e] = flow_percentage[e] * 0.01f * TERN(NO_VOLUMETRICS, 1.0f, volumetric_multiplier[e]); } static void set_flow(const uint8_t e, const int16_t flow) { flow_percentage[e] = flow; refresh_e_factor(e); } #endif // Manage fans, paste pressure, etc. static void check_axes_activity(); // Apply fan speeds #if HAS_FAN static void sync_fan_speeds(uint8_t (&fan_speed)[FAN_COUNT]); #if FAN_KICKSTART_TIME static void kickstart_fan(uint8_t (&fan_speed)[FAN_COUNT], const millis_t &ms, const uint8_t f); #else FORCE_INLINE static void kickstart_fan(uint8_t (&)[FAN_COUNT], const millis_t &, const uint8_t) {} #endif #endif #if ENABLED(FILAMENT_WIDTH_SENSOR) void apply_filament_width_sensor(const int8_t encoded_ratio); static 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] ); } #endif #if ENABLED(IMPROVE_HOMING_RELIABILITY) void enable_stall_prevention(const bool onoff); #endif #if DISABLED(NO_VOLUMETRICS) // Update multipliers based on new diameter measurements static void calculate_volumetric_multipliers(); #if ENABLED(VOLUMETRIC_EXTRUDER_LIMIT) // Update pre calculated extruder feedrate limits based on volumetric values static void calculate_volumetric_extruder_limit(const uint8_t e); static void calculate_volumetric_extruder_limits(); #endif FORCE_INLINE static void set_filament_size(const uint8_t e, const_float_t v) { filament_size[e] = v; if (v > 0) volumetric_area_nominal = CIRCLE_AREA(v * 0.5); //TODO: should it be per extruder // make sure all extruders have some sane value for the filament size LOOP_L_N(i, COUNT(filament_size)) if (!filament_size[i]) filament_size[i] = DEFAULT_NOMINAL_FILAMENT_DIA; } #endif #if ENABLED(VOLUMETRIC_EXTRUDER_LIMIT) FORCE_INLINE static void set_volumetric_extruder_limit(const uint8_t e, const_float_t v) { volumetric_extruder_limit[e] = v; calculate_volumetric_extruder_limit(e); } #endif #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'. */ static float fade_scaling_factor_for_z(const_float_t rz) { static float z_fade_factor = 1; if (!z_fade_height || rz <= 0) 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; } return z_fade_factor; } FORCE_INLINE static void force_fade_recalc() { last_fade_z = -999.999f; } FORCE_INLINE static void set_z_fade_height(const_float_t zfh) { z_fade_height = zfh > 0 ? zfh : 0; inverse_z_fade_height = RECIPROCAL(z_fade_height); force_fade_recalc(); } FORCE_INLINE static bool leveling_active_at_z(const_float_t rz) { return !z_fade_height || rz < z_fade_height; } #else FORCE_INLINE static float fade_scaling_factor_for_z(const_float_t) { return 1; } FORCE_INLINE static bool leveling_active_at_z(const_float_t) { return true; } #endif #if ENABLED(SKEW_CORRECTION) FORCE_INLINE static void skew(float &cx, float &cy, const_float_t cz) { if (COORDINATE_OKAY(cx, X_MIN_POS + 1, X_MAX_POS) && COORDINATE_OKAY(cy, Y_MIN_POS + 1, Y_MAX_POS)) { const float sx = cx - cy * skew_factor.xy - cz * (skew_factor.xz - (skew_factor.xy * skew_factor.yz)), sy = cy - cz * skew_factor.yz; if (COORDINATE_OKAY(sx, X_MIN_POS, X_MAX_POS) && COORDINATE_OKAY(sy, Y_MIN_POS, Y_MAX_POS)) { cx = sx; cy = sy; } } } FORCE_INLINE static void skew(xyz_pos_t &raw) { skew(raw.x, raw.y, raw.z); } FORCE_INLINE static void unskew(float &cx, float &cy, const_float_t cz) { if (COORDINATE_OKAY(cx, X_MIN_POS, X_MAX_POS) && COORDINATE_OKAY(cy, Y_MIN_POS, Y_MAX_POS)) { const float sx = cx + cy * skew_factor.xy + cz * skew_factor.xz, sy = cy + cz * skew_factor.yz; if (COORDINATE_OKAY(sx, X_MIN_POS, X_MAX_POS) && COORDINATE_OKAY(sy, Y_MIN_POS, Y_MAX_POS)) { cx = sx; cy = sy; } } } FORCE_INLINE static void unskew(xyz_pos_t &raw) { unskew(raw.x, raw.y, raw.z); } #endif // SKEW_CORRECTION #if HAS_LEVELING /** * Apply leveling to transform a cartesian position * as it will be given to the planner and steppers. */ 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; } #else FORCE_INLINE static void apply_leveling(xyz_pos_t&) {} FORCE_INLINE static void unapply_leveling(xyz_pos_t&) {} #endif #if ENABLED(FWRETRACT) static void apply_retract(float &rz, float &e); FORCE_INLINE static void apply_retract(xyze_pos_t &raw) { apply_retract(raw.z, raw.e); } static void unapply_retract(float &rz, float &e); FORCE_INLINE static void unapply_retract(xyze_pos_t &raw) { unapply_retract(raw.z, raw.e); } #endif #if HAS_POSITION_MODIFIERS FORCE_INLINE static void apply_modifiers(xyze_pos_t &pos, bool leveling=ENABLED(PLANNER_LEVELING)) { TERN_(SKEW_CORRECTION, skew(pos)); if (leveling) apply_leveling(pos); TERN_(FWRETRACT, apply_retract(pos)); } FORCE_INLINE static void unapply_modifiers(xyze_pos_t &pos, bool leveling=ENABLED(PLANNER_LEVELING)) { TERN_(FWRETRACT, unapply_retract(pos)); if (leveling) unapply_leveling(pos); TERN_(SKEW_CORRECTION, unskew(pos)); } #endif // HAS_POSITION_MODIFIERS // Number of moves currently in the planner including the busy block, if any FORCE_INLINE static uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail); } // Number of nonbusy moves currently in the planner FORCE_INLINE static uint8_t nonbusy_movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_nonbusy); } // Remove all blocks from the buffer FORCE_INLINE static void clear_block_buffer() { block_buffer_nonbusy = block_buffer_planned = block_buffer_head = block_buffer_tail = 0; } // 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(); } /** * Planner::get_next_free_block * * - 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 */ FORCE_INLINE static block_t* get_next_free_block(uint8_t &next_buffer_head, const uint8_t count=1) { // Wait until there are enough slots free while (moves_free() < count) { idle(); } // Return the first available block next_buffer_head = next_block_index(block_buffer_head); return &block_buffer[block_buffer_head]; } /** * 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 * millimeters - the length of the movement, if known * * Returns true if movement was buffered, false otherwise */ static bool _buffer_steps(const xyze_long_t &target OPTARG(HAS_POSITION_FLOAT, const xyze_pos_t &target_float) OPTARG(HAS_DIST_MM_ARG, const xyze_float_t &cart_dist_mm) , feedRate_t fr_mm_s, const uint8_t extruder, const_float_t millimeters=0.0 ); /** * 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, const xyze_long_t &target OPTARG(HAS_POSITION_FLOAT, const xyze_pos_t &target_float) OPTARG(HAS_DIST_MM_ARG, const xyze_float_t &cart_dist_mm) , feedRate_t fr_mm_s, const uint8_t extruder, const_float_t millimeters=0.0 ); /** * Planner::buffer_sync_block * Add a block to the buffer that just updates the position or in * case of LASER_SYNCHRONOUS_M106_M107 the fan pwm */ static void buffer_sync_block( TERN_(LASER_SYNCHRONOUS_M106_M107, uint8_t sync_flag=BLOCK_FLAG_SYNC_POSITION) ); #if IS_KINEMATIC private: // Allow do_homing_move to access internal functions, such as buffer_segment. friend void do_homing_move(const AxisEnum, const float, const feedRate_t, const bool); #endif /** * Planner::buffer_segment * * Add a new linear movement to the buffer in axis units. * * Leveling and kinematics should be applied ahead of calling this. * * 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 */ static bool buffer_segment(const abce_pos_t &abce OPTARG(HAS_DIST_MM_ARG, const xyze_float_t &cart_dist_mm) , const_feedRate_t fr_mm_s, const uint8_t extruder=active_extruder, const_float_t millimeters=0.0 ); public: /** * Add a new linear movement to the buffer. * The target is cartesian. It's translated to * delta/scara if needed. * * cart - target position in mm or degrees * fr_mm_s - (target) speed of the move (mm/s) * extruder - target extruder * millimeters - the length of the movement, if known * inv_duration - the reciprocal if the duration of the movement, if known (kinematic only if feeedrate scaling is enabled) */ static bool buffer_line(const xyze_pos_t &cart, const_feedRate_t fr_mm_s, const uint8_t extruder=active_extruder, const float millimeters=0.0 OPTARG(SCARA_FEEDRATE_SCALING, const_float_t inv_duration=0.0) ); #if ENABLED(DIRECT_STEPPING) static void buffer_page(const page_idx_t page_idx, const uint8_t extruder, const uint16_t num_steps); #endif /** * Set the planner.position and individual stepper positions. * Used by G92, G28, G29, and other procedures. * * 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. * * Multiplies by axis_steps_per_mm[] and does necessary conversion * for COREXY / COREXZ / COREYZ to set the corresponding stepper positions. * * Clears previous speed values. */ static void set_position_mm(const xyze_pos_t &xyze); #if HAS_EXTRUDERS static void set_e_position_mm(const_float_t e); #endif /** * Set the planner.position and individual stepper positions. * * The supplied position is in machine space, and no additional * conversions are applied. */ static void set_machine_position_mm(const abce_pos_t &abce); /** * 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); static abce_pos_t get_axis_positions_mm() { const abce_pos_t out = LOGICAL_AXIS_ARRAY( get_axis_position_mm(E_AXIS), get_axis_position_mm(A_AXIS), get_axis_position_mm(B_AXIS), get_axis_position_mm(C_AXIS), get_axis_position_mm(I_AXIS), get_axis_position_mm(J_AXIS), get_axis_position_mm(K_AXIS) ); return out; } // 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 // Called to force a quick stop of the machine (for example, when // a Full Shutdown is required, or when endstops are hit) static void quick_stop(); #if ENABLED(REALTIME_REPORTING_COMMANDS) // Force a quick pause of the machine (e.g., when a pause is required in the middle of move). // NOTE: Hard-stops will lose steps so encoders are highly recommended if using these! static void quick_pause(); static void quick_resume(); #endif // Called when an endstop is triggered. Causes the machine to stop immediately static void endstop_triggered(const AxisEnum axis); // Triggered position of an axis in mm (not core-savvy) static float triggered_position_mm(const AxisEnum axis); // Blocks are queued, or we're running out moves, or the closed loop controller is waiting static bool busy() { return (has_blocks_queued() || cleaning_buffer_counter || TERN0(EXTERNAL_CLOSED_LOOP_CONTROLLER, CLOSED_LOOP_WAITING()) ); } // Block until all buffered steps are executed / cleaned static void synchronize(); // Wait for moves to finish and disable all steppers static void finish_and_disable(); // Periodic handler to manage the cleaning buffer counter // Called from the Temperature ISR at ~1kHz static void isr() { if (cleaning_buffer_counter) --cleaning_buffer_counter; } /** * Does the buffer have any blocks queued? */ FORCE_INLINE static bool has_blocks_queued() { return (block_buffer_head != block_buffer_tail); } /** * 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. * * WARNING: Called from Stepper ISR context! */ static block_t* get_current_block(); /** * "Release" the current block so its slot can be reused. * Called when the current block is no longer needed. */ FORCE_INLINE static void release_current_block() { if (has_blocks_queued()) block_buffer_tail = next_block_index(block_buffer_tail); } #if HAS_WIRED_LCD static uint16_t block_buffer_runtime(); static void clear_block_buffer_runtime(); #endif #if ENABLED(AUTOTEMP) static celsius_t autotemp_min, autotemp_max; static float autotemp_factor; static bool autotemp_enabled; static void autotemp_update(); static void autotemp_M104_M109(); static void autotemp_task(); #endif #if HAS_LINEAR_E_JERK FORCE_INLINE static void recalculate_max_e_jerk() { const float prop = junction_deviation_mm * SQRT(0.5) / (1.0f - SQRT(0.5)); EXTRUDER_LOOP() max_e_jerk[E_INDEX_N(e)] = SQRT(prop * settings.max_acceleration_mm_per_s2[E_INDEX_N(e)]); } #endif private: #if ENABLED(AUTOTEMP) #if ENABLED(AUTOTEMP_PROPORTIONAL) static void _autotemp_update_from_hotend(); #else static void _autotemp_update_from_hotend() {} #endif #endif /** * Get the index of the next / previous block in the ring buffer */ 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); } /** * Calculate the distance (not time) it takes to accelerate * from initial_rate to target_rate using the given acceleration: */ static float estimate_acceleration_distance(const_float_t initial_rate, const_float_t target_rate, const_float_t accel) { if (accel == 0) return 0; // accel was 0, set acceleration distance to 0 return (sq(target_rate) - sq(initial_rate)) / (accel * 2); } /** * Return the point at which you must start braking (at the rate of -'accel') if * 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) */ static float intersection_distance(const_float_t initial_rate, const_float_t final_rate, const_float_t accel, const_float_t distance) { if (accel == 0) return 0; // accel was 0, set intersection distance to 0 return (accel * 2 * distance - sq(initial_rate) + sq(final_rate)) / (accel * 4); } /** * Calculate the maximum allowable speed squared at this point, in order * to reach 'target_velocity_sqr' using 'acceleration' within a given * 'distance'. */ static float max_allowable_speed_sqr(const_float_t accel, const_float_t target_velocity_sqr, const_float_t distance) { return target_velocity_sqr - 2 * accel * distance; } #if ENABLED(S_CURVE_ACCELERATION) /** * Calculate the speed reached given initial speed, acceleration and distance */ static float final_speed(const_float_t initial_velocity, const_float_t accel, const_float_t distance) { return SQRT(sq(initial_velocity) + 2 * accel * distance); } #endif static void calculate_trapezoid_for_block(block_t * const block, const_float_t entry_factor, const_float_t exit_factor); static void reverse_pass_kernel(block_t * const current, const block_t * const next); static void forward_pass_kernel(const block_t * const previous, block_t * const current, uint8_t block_index); static void reverse_pass(); static void forward_pass(); static void recalculate_trapezoids(); static void recalculate(); #if HAS_JUNCTION_DEVIATION FORCE_INLINE static void normalize_junction_vector(xyze_float_t &vector) { float magnitude_sq = 0; LOOP_LOGICAL_AXES(idx) if (vector[idx]) magnitude_sq += sq(vector[idx]); vector *= RSQRT(magnitude_sq); } FORCE_INLINE static float limit_value_by_axis_maximum(const_float_t max_value, xyze_float_t &unit_vec) { float limit_value = max_value; LOOP_LOGICAL_AXES(idx) { if (unit_vec[idx]) { if (limit_value * ABS(unit_vec[idx]) > settings.max_acceleration_mm_per_s2[idx]) limit_value = ABS(settings.max_acceleration_mm_per_s2[idx] / unit_vec[idx]); } } return limit_value; } #endif // HAS_JUNCTION_DEVIATION }; #define PLANNER_XY_FEEDRATE() _MIN(planner.settings.max_feedrate_mm_s[X_AXIS], planner.settings.max_feedrate_mm_s[Y_AXIS]) extern Planner planner;