Firmware2/Marlin/src/module/planner.h

543 lines
19 KiB
C
Raw Normal View History

/**
2016-03-24 19:01:20 +01:00
* Marlin 3D Printer Firmware
* Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
*
* Based on Sprinter and grbl.
* Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
*/
/**
* planner.h
*
* Buffer movement commands and manage the acceleration profile plan
*
* Derived from Grbl
* Copyright (c) 2009-2011 Simen Svale Skogsrud
*/
#ifndef PLANNER_H
#define PLANNER_H
2011-12-22 12:38:50 +01:00
2017-09-06 13:28:32 +02:00
#include "../Marlin.h"
2017-09-08 22:35:25 +02:00
#include "motion.h"
#if ENABLED(DELTA)
#include "delta.h"
#endif
#if HAS_ABL
2017-09-06 13:28:32 +02:00
#include "../libs/vector_3.h"
2016-04-28 03:06:32 +02:00
#endif
enum BlockFlagBit {
// 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,
// Start from a halt at the start of this block, respecting the maximum allowed jerk.
BLOCK_BIT_START_FROM_FULL_HALT,
// The block is busy
BLOCK_BIT_BUSY
};
enum BlockFlag {
BLOCK_FLAG_RECALCULATE = _BV(BLOCK_BIT_RECALCULATE),
BLOCK_FLAG_NOMINAL_LENGTH = _BV(BLOCK_BIT_NOMINAL_LENGTH),
BLOCK_FLAG_START_FROM_FULL_HALT = _BV(BLOCK_BIT_START_FROM_FULL_HALT),
BLOCK_FLAG_BUSY = _BV(BLOCK_BIT_BUSY)
};
2016-04-28 03:06:32 +02: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.
*/
typedef struct {
2016-04-28 03:06:32 +02:00
2016-10-30 22:08:46 +01:00
uint8_t flag; // Block flags (See BlockFlag enum above)
2016-04-28 03:06:32 +02:00
unsigned char active_extruder; // The extruder to move (if E move)
2016-10-30 22:08:46 +01:00
// Fields used by the Bresenham algorithm for tracing the line
int32_t steps[NUM_AXIS]; // Step count along each axis
uint32_t step_event_count; // The number of step events required to complete this block
2016-04-28 03:06:32 +02:00
#if ENABLED(MIXING_EXTRUDER)
2016-10-30 22:08:46 +01:00
uint32_t mix_event_count[MIXING_STEPPERS]; // Scaled step_event_count for the mixing steppers
#endif
2016-10-30 22:08:46 +01:00
int32_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
acceleration_rate; // The acceleration rate used for acceleration calculation
2016-04-28 03:06:32 +02:00
2016-10-30 22:08:46 +01:00
uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
2016-04-28 03:06:32 +02:00
// Advance extrusion
#if ENABLED(LIN_ADVANCE)
bool use_advance_lead;
uint32_t abs_adv_steps_multiplier8; // Factorised by 2^8 to avoid float
#endif
// Fields used by the motion planner to manage acceleration
2016-10-30 22:08:46 +01:00
float nominal_speed, // The nominal speed for this block in mm/sec
entry_speed, // Entry speed at previous-current junction in mm/sec
max_entry_speed, // Maximum allowable junction entry speed in mm/sec
millimeters, // The total travel of this block in mm
acceleration; // acceleration mm/sec^2
// Settings for the trapezoid generator
2016-10-30 22:08:46 +01: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 03:27:45 +01:00
#if FAN_COUNT > 0
uint16_t fan_speed[FAN_COUNT];
2016-03-06 03:27:45 +01:00
#endif
#if ENABLED(BARICUDA)
uint8_t valve_pressure, e_to_p_pressure;
#endif
2016-12-15 16:21:32 +01:00
uint32_t segment_time;
2016-03-06 03:27:45 +01:00
} block_t;
#define BLOCK_MOD(n) ((n)&(BLOCK_BUFFER_SIZE-1))
2016-04-28 03:06:32 +02:00
class Planner {
2016-04-28 03:06:32 +02:00
public:
2016-04-28 03:06:32 +02:00
/**
* A ring buffer of moves described in steps
*/
2016-05-26 20:22:51 +02:00
static block_t block_buffer[BLOCK_BUFFER_SIZE];
2016-12-04 05:03:46 +01:00
static volatile uint8_t block_buffer_head, // Index of the next block to be pushed
block_buffer_tail;
2016-05-26 20:22:51 +02:00
2016-12-04 05:02:27 +01:00
#if ENABLED(DISTINCT_E_FACTORS)
static uint8_t last_extruder; // Respond to extruder change
#endif
2017-09-09 06:50:46 +02:00
static int16_t flow_percentage[EXTRUDERS]; // Extrusion factor for each extruder
2017-09-18 12:51:45 +02:00
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_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
2016-12-04 05:02:27 +01:00
static float max_feedrate_mm_s[XYZE_N], // Max speeds in mm per second
axis_steps_per_mm[XYZE_N],
steps_to_mm[XYZE_N];
static uint32_t max_acceleration_steps_per_s2[XYZE_N],
max_acceleration_mm_per_s2[XYZE_N]; // Use M201 to override by software
2016-05-26 20:22:51 +02:00
static millis_t min_segment_time;
2016-12-04 05:03:46 +01:00
static float min_feedrate_mm_s,
acceleration, // Normal acceleration mm/s^2 DEFAULT ACCELERATION for all printing moves. M204 SXXXX
retract_acceleration, // Retract acceleration mm/s^2 filament pull-back and push-forward while standing still in the other axes M204 TXXXX
travel_acceleration, // Travel acceleration mm/s^2 DEFAULT ACCELERATION for all NON printing moves. M204 MXXXX
max_jerk[XYZE], // The largest speed change requiring no acceleration
min_travel_feedrate_mm_s;
2016-04-28 03:06:32 +02:00
#if HAS_LEVELING
static bool leveling_active; // Flag that bed leveling is enabled
2017-05-21 02:23:39 +02:00
#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
2016-11-26 06:32:47 +01:00
#endif
#if ENABLED(LIN_ADVANCE)
static float extruder_advance_k, advance_ed_ratio;
#endif
2016-04-28 03:06:32 +02:00
private:
2016-04-28 03:06:32 +02:00
/**
* The current position of the tool in absolute steps
* Recalculated if any axis_steps_per_mm are changed by gcode
2016-04-28 03:06:32 +02:00
*/
2016-05-26 20:22:51 +02:00
static long position[NUM_AXIS];
2016-04-28 03:06:32 +02:00
/**
* Speed of previous path line segment
*/
2016-05-26 20:22:51 +02:00
static float previous_speed[NUM_AXIS];
2016-04-28 03:06:32 +02:00
/**
* Nominal speed of previous path line segment
*/
2016-05-26 20:22:51 +02:00
static float previous_nominal_speed;
2016-12-15 16:21:32 +01:00
2016-11-26 06:32:47 +01:00
/**
* Limit where 64bit math is necessary for acceleration calculation
*/
static uint32_t cutoff_long;
2016-04-28 03:06:32 +02:00
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
static float last_raw_lz;
#endif
2016-04-28 03:06:32 +02:00
#if ENABLED(DISABLE_INACTIVE_EXTRUDER)
/**
* Counters to manage disabling inactive extruders
*/
2016-05-26 20:22:51 +02:00
static uint8_t g_uc_extruder_last_move[EXTRUDERS];
2016-04-28 03:06:32 +02:00
#endif // DISABLE_INACTIVE_EXTRUDER
#ifdef XY_FREQUENCY_LIMIT
// Used for the frequency limit
2016-05-26 20:22:51 +02:00
#define MAX_FREQ_TIME long(1000000.0/XY_FREQUENCY_LIMIT)
2016-04-28 03:06:32 +02:00
// Old direction bits. Used for speed calculations
2016-05-26 20:22:51 +02:00
static unsigned char old_direction_bits;
2016-04-28 03:06:32 +02:00
// Segment times (in µs). Used for speed calculations
2016-05-26 20:22:51 +02:00
static long axis_segment_time[2][3];
2016-04-28 03:06:32 +02:00
#endif
2016-12-15 16:21:32 +01:00
#if ENABLED(LIN_ADVANCE)
static float position_float[NUM_AXIS];
#endif
2016-04-28 03:06:32 +02:00
#if ENABLED(ULTRA_LCD)
volatile static uint32_t block_buffer_runtime_us; //Theoretical block buffer runtime in µs
#endif
2016-04-28 03:06:32 +02:00
public:
2016-05-26 20:22:51 +02:00
/**
* Instance Methods
*/
2016-04-28 03:06:32 +02:00
Planner();
void init();
2016-05-26 20:22:51 +02:00
/**
* Static (class) Methods
*/
static void reset_acceleration_rates();
static void refresh_positioning();
2016-04-28 03:06:32 +02:00
// Manage fans, paste pressure, etc.
2016-05-26 20:22:51 +02:00
static void check_axes_activity();
/**
2016-04-28 03:06:32 +02:00
* Number of moves currently in the planner
*/
static uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail + BLOCK_BUFFER_SIZE); }
2016-04-28 03:06:32 +02:00
2016-04-04 03:06:17 +02:00
static bool is_full() { return (block_buffer_tail == BLOCK_MOD(block_buffer_head + 1)); }
2017-09-18 12:51:45 +02:00
// Update multipliers based on new diameter measurements
static void calculate_volumetric_multipliers();
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
for (uint8_t i = 0; i < COUNT(filament_size); i++)
if (!filament_size[i]) filament_size[i] = DEFAULT_NOMINAL_FILAMENT_DIA;
}
#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'.
*/
inline static float fade_scaling_factor_for_z(const float &lz) {
static float z_fade_factor = 1.0;
if (z_fade_height) {
const float raw_lz = RAW_Z_POSITION(lz);
if (raw_lz >= z_fade_height) return 0.0;
if (last_raw_lz != raw_lz) {
last_raw_lz = raw_lz;
z_fade_factor = 1.0 - raw_lz * inverse_z_fade_height;
}
return z_fade_factor;
}
return 1.0;
}
FORCE_INLINE static void force_fade_recalc() { last_raw_lz = -999.999; }
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 &lz) {
return !z_fade_height || RAW_Z_POSITION(lz) < z_fade_height;
}
#else
FORCE_INLINE static float fade_scaling_factor_for_z(const float &lz) {
UNUSED(lz);
return 1.0;
}
FORCE_INLINE static bool leveling_active_at_z(const float &lz) { return true; }
#endif
2017-05-01 23:13:09 +02:00
#if PLANNER_LEVELING
2016-10-07 20:27:00 +02:00
#define ARG_X float lx
#define ARG_Y float ly
#define ARG_Z float lz
2016-04-28 03:06:32 +02:00
/**
* Apply leveling to transform a cartesian position
* as it will be given to the planner and steppers.
2016-04-28 03:06:32 +02:00
*/
static void apply_leveling(float &lx, float &ly, float &lz);
static void apply_leveling(float logical[XYZ]) { apply_leveling(logical[X_AXIS], logical[Y_AXIS], logical[Z_AXIS]); }
2016-09-23 10:01:27 +02:00
static void unapply_leveling(float logical[XYZ]);
2016-04-28 03:06:32 +02:00
2016-10-07 20:27:00 +02:00
#else
#define ARG_X const float &lx
#define ARG_Y const float &ly
#define ARG_Z const float &lz
#endif
2016-12-15 16:21:32 +01:00
/**
* Planner::_buffer_line
*
2016-10-09 20:25:25 +02:00
* Add a new direct linear movement to the buffer.
*
* Leveling and kinematics should be applied ahead of this.
*
2016-10-09 20:25:25 +02:00
* a,b,c,e - target position in mm or degrees
* fr_mm_s - (target) speed of the move (mm/s)
* extruder - target extruder
*/
2016-10-09 20:25:25 +02:00
static void _buffer_line(const float &a, const float &b, const float &c, const float &e, float fr_mm_s, const uint8_t extruder);
2016-10-09 20:25:25 +02:00
static void _set_position_mm(const float &a, const float &b, const float &c, const float &e);
/**
* Add a new linear movement to the buffer.
* The target is NOT translated to delta/scara
*
2016-10-09 20:25:25 +02:00
* Leveling will be applied to input on cartesians.
* Kinematic machines should call buffer_line_kinematic (for leveled moves).
* (Cartesians may also call buffer_line_kinematic.)
*
* lx,ly,lz,e - target position in mm or degrees
* fr_mm_s - (target) speed of the move (mm/s)
* extruder - target extruder
*/
2016-10-27 12:53:24 +02:00
static FORCE_INLINE void buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, const float &fr_mm_s, const uint8_t extruder) {
2017-05-01 23:13:09 +02:00
#if PLANNER_LEVELING && IS_CARTESIAN
apply_leveling(lx, ly, lz);
#endif
_buffer_line(lx, ly, lz, e, fr_mm_s, extruder);
}
/**
* Add a new linear movement to the buffer.
* The target is cartesian, it's translated to delta/scara if
* needed.
*
* ltarget - x,y,z,e CARTESIAN target in mm
* fr_mm_s - (target) speed of the move (mm/s)
* extruder - target extruder
*/
static FORCE_INLINE void buffer_line_kinematic(const float ltarget[XYZE], const float &fr_mm_s, const uint8_t extruder) {
2017-05-01 23:13:09 +02:00
#if PLANNER_LEVELING
float lpos[XYZ] = { ltarget[X_AXIS], ltarget[Y_AXIS], ltarget[Z_AXIS] };
apply_leveling(lpos);
#else
const float * const lpos = ltarget;
#endif
#if IS_KINEMATIC
inverse_kinematics(lpos);
_buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], ltarget[E_AXIS], fr_mm_s, extruder);
#else
_buffer_line(lpos[X_AXIS], lpos[Y_AXIS], lpos[Z_AXIS], ltarget[E_AXIS], fr_mm_s, extruder);
#endif
}
2016-04-28 03:06:32 +02:00
/**
* Set the planner.position and individual stepper positions.
* Used by G92, G28, G29, and other procedures.
*
* 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 FORCE_INLINE void set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) {
2017-05-01 23:13:09 +02:00
#if PLANNER_LEVELING && IS_CARTESIAN
apply_leveling(lx, ly, lz);
#endif
_set_position_mm(lx, ly, lz, e);
}
static void set_position_mm_kinematic(const float position[NUM_AXIS]);
2016-10-27 12:53:24 +02:00
static void set_position_mm(const AxisEnum axis, const float &v);
static FORCE_INLINE void set_z_position_mm(const float &z) { set_position_mm(Z_AXIS, z); }
static FORCE_INLINE void set_e_position_mm(const float &e) { set_position_mm(AxisEnum(E_AXIS), e); }
2016-04-28 03:06:32 +02:00
/**
* Sync from the stepper positions. (e.g., after an interrupted move)
*/
static void sync_from_steppers();
2016-04-28 03:06:32 +02:00
/**
* Does the buffer have any blocks queued?
*/
static bool blocks_queued() { return (block_buffer_head != block_buffer_tail); }
2016-04-28 03:06:32 +02:00
/**
* "Discards" the block and "releases" the memory.
* Called when the current block is no longer needed.
*/
static void discard_current_block() {
2016-04-28 03:06:32 +02:00
if (blocks_queued())
block_buffer_tail = BLOCK_MOD(block_buffer_tail + 1);
}
/**
* The current block. NULL if the buffer is empty.
* This also marks the block as busy.
*/
static block_t* get_current_block() {
2016-04-28 03:06:32 +02:00
if (blocks_queued()) {
block_t* block = &block_buffer[block_buffer_tail];
#if ENABLED(ULTRA_LCD)
block_buffer_runtime_us -= block->segment_time; //We can't be sure how long an active block will take, so don't count it.
#endif
SBI(block->flag, BLOCK_BIT_BUSY);
2016-04-28 03:06:32 +02:00
return block;
}
else {
#if ENABLED(ULTRA_LCD)
clear_block_buffer_runtime(); // paranoia. Buffer is empty now - so reset accumulated time to zero.
#endif
2016-04-28 03:06:32 +02:00
return NULL;
}
2016-04-28 03:06:32 +02:00
}
#if ENABLED(ULTRA_LCD)
static uint16_t block_buffer_runtime() {
CRITICAL_SECTION_START
millis_t bbru = block_buffer_runtime_us;
CRITICAL_SECTION_END
// To translate µs to ms a division by 1000 would be required.
// We introduce 2.4% error here by dividing by 1024.
// Doesn't matter because block_buffer_runtime_us is already too small an estimation.
bbru >>= 10;
// limit to about a minute.
2017-04-22 04:42:27 +02:00
NOMORE(bbru, 0xFFFFul);
return bbru;
}
static void clear_block_buffer_runtime(){
CRITICAL_SECTION_START
block_buffer_runtime_us = 0;
CRITICAL_SECTION_END
}
#endif
#if ENABLED(AUTOTEMP)
2016-11-12 01:11:04 +01:00
static float autotemp_min, autotemp_max, autotemp_factor;
2016-05-26 20:22:51 +02:00
static bool autotemp_enabled;
static void getHighESpeed();
static void autotemp_M104_M109();
#endif
private:
2016-04-28 03:06:32 +02:00
/**
* Get the index of the next / previous block in the ring buffer
*/
static int8_t next_block_index(int8_t block_index) { return BLOCK_MOD(block_index + 1); }
static int8_t prev_block_index(int8_t block_index) { return BLOCK_MOD(block_index - 1); }
2016-04-28 03:06:32 +02:00
/**
* Calculate the distance (not time) it takes to accelerate
* from initial_rate to target_rate using the given acceleration:
*/
2016-10-27 12:53:24 +02:00
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
2016-07-16 03:50:25 +02:00
return (sq(target_rate) - sq(initial_rate)) / (accel * 2);
2016-04-28 03:06:32 +02:00
}
/**
* Return the point at which you must start braking (at the rate of -'acceleration') 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)
*/
2016-10-27 12:53:24 +02:00
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
2016-07-16 03:50:25 +02:00
return (accel * 2 * distance - sq(initial_rate) + sq(final_rate)) / (accel * 4);
2016-04-28 03:06:32 +02:00
}
/**
* Calculate the maximum allowable speed at this point, in order
* to reach 'target_velocity' using 'acceleration' within a given
* 'distance'.
*/
2016-10-27 12:53:24 +02:00
static float max_allowable_speed(const float &accel, const float &target_velocity, const float &distance) {
return SQRT(sq(target_velocity) - 2 * accel * distance);
2016-04-28 03:06:32 +02:00
}
static void calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor);
2016-04-28 03:06:32 +02:00
static void reverse_pass_kernel(block_t* const current, const block_t *next);
static void forward_pass_kernel(const block_t *previous, block_t* const current);
2016-04-28 03:06:32 +02:00
2016-05-26 20:22:51 +02:00
static void reverse_pass();
static void forward_pass();
2016-04-28 03:06:32 +02:00
2016-05-26 20:22:51 +02:00
static void recalculate_trapezoids();
2016-04-28 03:06:32 +02:00
2016-05-26 20:22:51 +02:00
static void recalculate();
2011-11-19 15:37:10 +01:00
2016-04-28 03:06:32 +02:00
};
#define PLANNER_XY_FEEDRATE() (min(planner.max_feedrate_mm_s[X_AXIS], planner.max_feedrate_mm_s[Y_AXIS]))
2016-10-10 22:34:35 +02:00
extern Planner planner;
#endif // PLANNER_H