123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979 |
- /**
- * 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 <https://www.gnu.org/licenses/>.
- *
- */
- #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"
- #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
-
- // Feedrate for manual moves
- #ifdef MANUAL_FEEDRATE
- 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 };
- #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
- };
-
- 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_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
-
- TERN_(MIXING_EXTRUDER, MIXER_BLOCK_FIELD); // Normalized color for the mixing steppers
-
- // 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
-
- uint8_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;
- #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)
- #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[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
- } planner_settings_t;
-
- #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;
-
- 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 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[XYZE_N]; // (steps/s^2) Derived from mm_per_s2
- static float steps_to_mm[XYZE_N]; // 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 inline 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 inline void set_min_speed_factor_u8(const uint8_t v255) {
- xy_freq_min_speed_factor = float(ui8_to_percent(v255)) / 100;
- }
- static inline 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 cutoff_long;
-
- #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
- static float last_fade_z;
- #endif
-
- #if ENABLED(DISABLE_INACTIVE_EXTRUDER)
- // Counters to manage disabling inactive extruders
- static uint8_t g_uc_extruder_last_move[EXTRUDERS];
- #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
- */
-
- static void reset_acceleration_rates();
- static void refresh_positioning();
- 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);
-
-
- #if 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 inline 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();
-
- #if ENABLED(FILAMENT_WIDTH_SENSOR)
- 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]
- );
- }
- #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 &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 &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 inline float fade_scaling_factor_for_z(const float &rz) {
- static float z_fade_factor = 1;
- 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;
- }
- 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 &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 &rz) {
- return !z_fade_height || rz < z_fade_height;
- }
-
- #else
-
- FORCE_INLINE static float fade_scaling_factor_for_z(const float&) { return 1; }
-
- FORCE_INLINE static bool leveling_active_at_z(const float&) { return true; }
-
- #endif
-
- #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)) {
- 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 (WITHIN(sx, X_MIN_POS, X_MAX_POS) && WITHIN(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 &cz) {
- if (WITHIN(cx, X_MIN_POS, X_MAX_POS) && WITHIN(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 (WITHIN(sx, X_MIN_POS, X_MAX_POS) && WITHIN(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
- #if HAS_POSITION_FLOAT
- , const xyze_pos_t &target_float
- #endif
- #if HAS_DIST_MM_ARG
- , const xyze_float_t &cart_dist_mm
- #endif
- , feedRate_t fr_mm_s, const uint8_t extruder, const float &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
- #if HAS_POSITION_FLOAT
- , const xyze_pos_t &target_float
- #endif
- #if HAS_DIST_MM_ARG
- , const xyze_float_t &cart_dist_mm
- #endif
- , feedRate_t fr_mm_s, const uint8_t extruder, const float &millimeters=0.0
- );
-
- /**
- * Planner::buffer_sync_block
- * Add a block to the buffer that just updates the position
- */
- static void buffer_sync_block();
-
- #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);
- #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 float &a, const float &b, const float &c, const float &e
- #if HAS_DIST_MM_ARG
- , const xyze_float_t &cart_dist_mm
- #endif
- , const feedRate_t &fr_mm_s, const uint8_t extruder, const float &millimeters=0.0
- );
-
- FORCE_INLINE static bool buffer_segment(abce_pos_t &abce
- #if HAS_DIST_MM_ARG
- , const xyze_float_t &cart_dist_mm
- #endif
- , const feedRate_t &fr_mm_s, const uint8_t extruder, const float &millimeters=0.0
- ) {
- return buffer_segment(abce.a, abce.b, abce.c, abce.e
- #if HAS_DIST_MM_ARG
- , cart_dist_mm
- #endif
- , fr_mm_s, extruder, millimeters);
- }
-
- public:
-
- /**
- * Add a new linear movement to the buffer.
- * The target is cartesian. It's translated to
- * delta/scara if needed.
- *
- * rx,ry,rz,e - 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 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
- #if ENABLED(SCARA_FEEDRATE_SCALING)
- , const float &inv_duration=0.0
- #endif
- );
-
- 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
- #if ENABLED(SCARA_FEEDRATE_SCALING)
- , const float &inv_duration=0.0
- #endif
- ) {
- return buffer_line(cart.x, cart.y, cart.z, cart.e, fr_mm_s, extruder, millimeters
- #if ENABLED(SCARA_FEEDRATE_SCALING)
- , inv_duration
- #endif
- );
- }
-
- #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 float &rx, const float &ry, const float &rz, const float &e);
- FORCE_INLINE static void set_position_mm(const xyze_pos_t &cart) { set_position_mm(cart.x, cart.y, cart.z, cart.e); }
- static void set_e_position_mm(const float &e);
-
- /**
- * 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 float &a, const float &b, const float &c, const float &e);
- 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); }
-
- /**
- * 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 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;
- }
-
- // 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();
-
- // Called when an endstop is triggered. Causes the machine to stop inmediately
- 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);
-
- // 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 tick to handle cleaning timeouts
- // Called from the Temperature ISR at ~1kHz
- static void tick() {
- 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 float autotemp_min, autotemp_max, autotemp_factor;
- static bool autotemp_enabled;
- static void getHighESpeed();
- static void autotemp_M104_M109();
- static void autotemp_update();
- #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));
- LOOP_L_N(i, EXTRUDERS)
- max_e_jerk[E_INDEX_N(i)] = SQRT(prop * settings.max_acceleration_mm_per_s2[E_INDEX_N(i)]);
- }
- #endif
-
- private:
-
- /**
- * 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 &initial_rate, const float &target_rate, const float &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 &initial_rate, const float &final_rate, const float &accel, const float &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 &accel, const float &target_velocity_sqr, const float &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 &initial_velocity, const float &accel, const float &distance) {
- return SQRT(sq(initial_velocity) + 2 * accel * distance);
- }
- #endif
-
- static void calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &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_XYZE(idx) if (vector[idx]) magnitude_sq += sq(vector[idx]);
- vector *= RSQRT(magnitude_sq);
- }
-
- FORCE_INLINE static float limit_value_by_axis_maximum(const float &max_value, xyze_float_t &unit_vec) {
- float limit_value = max_value;
- LOOP_XYZE(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 // !CLASSIC_JERK
- };
-
- #define PLANNER_XY_FEEDRATE() (_MIN(planner.settings.max_feedrate_mm_s[X_AXIS], planner.settings.max_feedrate_mm_s[Y_AXIS]))
-
- extern Planner planner;
|