planner.cpp 49.8 KB
Newer Older
MagoKimbra's avatar
MagoKimbra committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22
/**
 * MK4due 3D Printer Firmware
 *
 * Based on Marlin, Sprinter and grbl
 * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
 * Copyright (C) 2013 - 2016 Alberto Cotronei @MagoKimbra
 *
 * 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/>.
 *
 */

MagoKimbra's avatar
MagoKimbra committed
23 24 25
/**
 * planner.cpp - Buffer movement commands and manage the acceleration profile plan
 * Part of Grbl
MagoKimbra's avatar
MagoKimbra committed
26
 *
MagoKimbra's avatar
MagoKimbra committed
27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70
 * Copyright (c) 2009-2011 Simen Svale Skogsrud
 *
 * Grbl is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Grbl is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
 *
 *
 * The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis.
 *
 *
 * Reasoning behind the mathematics in this module (in the key of 'Mathematica'):
 *
 * s == speed, a == acceleration, t == time, d == distance
 *
 * Basic definitions:
 *   Speed[s_, a_, t_] := s + (a*t)
 *   Travel[s_, a_, t_] := Integrate[Speed[s, a, t], t]
 *
 * Distance to reach a specific speed with a constant acceleration:
 *   Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, d, t]
 *   d -> (m^2 - s^2)/(2 a) --> estimate_acceleration_distance()
 *
 * Speed after a given distance of travel with constant acceleration:
 *   Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, m, t]
 *   m -> Sqrt[2 a d + s^2]
 *
 * DestinationSpeed[s_, a_, d_] := Sqrt[2 a d + s^2]
 *
 * When to start braking (di) to reach a specified destination speed (s2) after accelerating
 * from initial speed s1 without ever stopping at a plateau:
 *   Solve[{DestinationSpeed[s1, a, di] == DestinationSpeed[s2, a, d - di]}, di]
 *   di -> (2 a d - s1^2 + s2^2)/(4 a) --> intersection_distance()
 *
 * IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a)
 *
MagoKimbra's avatar
MagoKimbra committed
71 72
 */

MagoKimbra's avatar
MagoKimbra committed
73
#include "../../base.h"
MagoKimbra's avatar
MagoKimbra committed
74 75 76 77 78 79
#include "planner.h"

//===========================================================================
//============================= public variables ============================
//===========================================================================

MagoKimbra's avatar
MagoKimbra committed
80
millis_t minsegmenttime;
MagoKimbra's avatar
MagoKimbra committed
81
float max_feedrate[3 + EXTRUDERS]; // Max speeds in mm per minute
MagoKimbra's avatar
MagoKimbra committed
82 83 84
float axis_steps_per_unit[3 + EXTRUDERS];
unsigned long max_acceleration_units_per_sq_second[3 + EXTRUDERS]; // Use M201 to override by software
float minimumfeedrate;
85 86 87 88
float acceleration;                     // Normal acceleration mm/s^2  DEFAULT ACCELERATION for all printing moves. M204 SXXXX
float retract_acceleration[EXTRUDERS];  // mm/s^2 filament pull-back and push-forward while standing still in the other axes M204 TXXXX
float travel_acceleration;              // Travel acceleration mm/s^2  DEFAULT ACCELERATION for all NON printing moves. M204 MXXXX
float max_xy_jerk;                      // The largest speed change requiring no acceleration
MagoKimbra's avatar
MagoKimbra committed
89
float max_z_jerk;
90
float max_e_jerk[EXTRUDERS];            // mm/s - initial speed for extruder retract moves
MagoKimbra's avatar
MagoKimbra committed
91 92
float mintravelfeedrate;
unsigned long axis_steps_per_sqr_second[3 + EXTRUDERS];
MagoKimbra's avatar
MagoKimbra committed
93
uint8_t last_extruder;
MagoKimbra's avatar
MagoKimbra committed
94

MagoKimbra's avatar
MagoKimbra committed
95
#if ENABLED(AUTO_BED_LEVELING_FEATURE)
MagoKimbra's avatar
MagoKimbra committed
96
  // Transform required to compensate for bed level
MagoKimbra's avatar
MagoKimbra committed
97 98 99 100 101
  matrix_3x3 plan_bed_level_matrix = {
    1.0, 0.0, 0.0,
    0.0, 1.0, 0.0,
    0.0, 0.0, 1.0
  };
MagoKimbra's avatar
MagoKimbra committed
102
#endif // AUTO_BED_LEVELING_FEATURE
MagoKimbra's avatar
MagoKimbra committed
103

MagoKimbra's avatar
MagoKimbra committed
104
#if ENABLED(AUTOTEMP)
MagoKimbra's avatar
MagoKimbra committed
105 106 107 108 109 110 111
  float autotemp_max = 250;
  float autotemp_min = 210;
  float autotemp_factor = 0.1;
  bool autotemp_enabled = false;
#endif

//===========================================================================
MagoKimbra's avatar
MagoKimbra committed
112
//============ semi-private variables, used in inline functions =============
MagoKimbra's avatar
MagoKimbra committed
113
//===========================================================================
MagoKimbra's avatar
MagoKimbra committed
114 115 116 117

block_t block_buffer[BLOCK_BUFFER_SIZE];            // A ring buffer for motion instfructions
volatile unsigned char block_buffer_head;           // Index of the next block to be pushed
volatile unsigned char block_buffer_tail;           // Index of the block to process now
MagoKimbra's avatar
MagoKimbra committed
118 119

//===========================================================================
MagoKimbra's avatar
MagoKimbra committed
120
//============================ private variables ============================
MagoKimbra's avatar
MagoKimbra committed
121
//===========================================================================
MagoKimbra's avatar
MagoKimbra committed
122 123 124 125 126 127

// The current position of the tool in absolute steps
long position[NUM_AXIS];               // Rescaled from extern when axis_steps_per_unit are changed by gcode
static float previous_speed[NUM_AXIS]; // Speed of previous path line segment
static float previous_nominal_speed;   // Nominal speed of previous path line segment

MagoKimbra's avatar
MagoKimbra committed
128
uint8_t g_uc_extruder_last_move[EXTRUDERS] = { 0 };
MagoKimbra's avatar
MagoKimbra committed
129

Simone Primarosa's avatar
Simone Primarosa committed
130
#if ENABLED(XY_FREQUENCY_LIMIT)
MagoKimbra's avatar
MagoKimbra committed
131 132 133 134
  // Used for the frequency limit
  #define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT)
  // Old direction bits. Used for speed calculations
  static unsigned char old_direction_bits = 0;
MagoKimbra's avatar
MagoKimbra committed
135
  // Segment times (in µs). Used for speed calculations
MagoKimbra's avatar
MagoKimbra committed
136
  static long axis_segment_time[2][3] = { {MAX_FREQ_TIME + 1, 0, 0}, {MAX_FREQ_TIME + 1, 0, 0} };
MagoKimbra's avatar
MagoKimbra committed
137 138
#endif

MagoKimbra's avatar
MagoKimbra committed
139
#if ENABLED(FILAMENT_SENSOR)
MagoKimbra's avatar
MagoKimbra committed
140
  static char meas_sample; // temporary variable to hold filament measurement sample
MagoKimbra's avatar
MagoKimbra committed
141 142
#endif

MagoKimbra's avatar
MagoKimbra committed
143 144 145 146
#if ENABLED(DUAL_X_CARRIAGE)
  extern bool extruder_duplication_enabled;
#endif

MagoKimbra's avatar
MagoKimbra committed
147 148 149 150
//===========================================================================
//================================ functions ================================
//===========================================================================

MagoKimbra's avatar
MagoKimbra committed
151 152 153 154 155
// Get the next / previous index of the next block in the ring buffer
// NOTE: Using & here (not %) because BLOCK_BUFFER_SIZE is always a power of 2
FORCE_INLINE int8_t next_block_index(int8_t block_index) { return BLOCK_MOD(block_index + 1); }
FORCE_INLINE int8_t prev_block_index(int8_t block_index) { return BLOCK_MOD(block_index - 1); }

MagoKimbra's avatar
MagoKimbra committed
156
// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
MagoKimbra's avatar
MagoKimbra committed
157 158 159 160 161 162
// given acceleration:
FORCE_INLINE float estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration) {
  if (acceleration == 0) return 0; // acceleration was 0, set acceleration distance to 0
  return (target_rate * target_rate - initial_rate * initial_rate) / (acceleration * 2);
}

MagoKimbra's avatar
MagoKimbra committed
163
// This function gives you the point at which you must start braking (at the rate of -acceleration) if
MagoKimbra's avatar
MagoKimbra committed
164 165 166 167 168 169 170 171 172 173 174
// you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
// a total travel of distance. This can be used to compute the intersection point between acceleration and
// deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)

FORCE_INLINE float intersection_distance(float initial_rate, float final_rate, float acceleration, float distance) {
  if (acceleration == 0) return 0; // acceleration was 0, set intersection distance to 0
  return (acceleration * 2 * distance - initial_rate * initial_rate + final_rate * final_rate) / (acceleration * 4);
}

// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.

MagoKimbra's avatar
MagoKimbra committed
175
void calculate_trapezoid_for_block(block_t* block, float entry_factor, float exit_factor) {
MagoKimbra's avatar
MagoKimbra committed
176 177
  unsigned long initial_rate = ceil(block->nominal_rate * entry_factor),
                final_rate = ceil(block->nominal_rate * exit_factor); // (steps per second)
MagoKimbra's avatar
MagoKimbra committed
178 179

  // Limit minimal step rate (Otherwise the timer will overflow.)
MagoKimbra's avatar
MagoKimbra committed
180 181
  NOLESS(initial_rate, 120);
  NOLESS(final_rate, 120);
MagoKimbra's avatar
MagoKimbra committed
182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199

  long acceleration = block->acceleration_st;
  int32_t accelerate_steps = ceil(estimate_acceleration_distance(initial_rate, block->nominal_rate, acceleration));
  int32_t decelerate_steps = floor(estimate_acceleration_distance(block->nominal_rate, final_rate, -acceleration));

  // Calculate the size of Plateau of Nominal Rate.
  int32_t plateau_steps = block->step_event_count - accelerate_steps - decelerate_steps;

  // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
  // have to use intersection_distance() to calculate when to abort acceleration and start braking
  // in order to reach the final_rate exactly at the end of this block.
  if (plateau_steps < 0) {
    accelerate_steps = ceil(intersection_distance(initial_rate, final_rate, acceleration, block->step_event_count));
    accelerate_steps = max(accelerate_steps, 0); // Check limits due to numerical round-off
    accelerate_steps = min((uint32_t)accelerate_steps, block->step_event_count);//(We can cast here to unsigned, because the above line ensures that we are above zero)
    plateau_steps = 0;
  }

MagoKimbra's avatar
MagoKimbra committed
200 201 202 203
  #if ENABLED(ADVANCE)
    volatile long initial_advance = block->advance * entry_factor * entry_factor;
    volatile long final_advance = block->advance * exit_factor * exit_factor;
  #endif // ADVANCE
MagoKimbra's avatar
MagoKimbra committed
204 205 206 207 208 209

  // block->accelerate_until = accelerate_steps;
  // block->decelerate_after = accelerate_steps+plateau_steps;
  CRITICAL_SECTION_START;  // Fill variables used by the stepper in a critical section
  if (!block->busy) { // Don't update variables if block is busy.
    block->accelerate_until = accelerate_steps;
MagoKimbra's avatar
MagoKimbra committed
210
    block->decelerate_after = accelerate_steps + plateau_steps;
MagoKimbra's avatar
MagoKimbra committed
211 212
    block->initial_rate = initial_rate;
    block->final_rate = final_rate;
MagoKimbra's avatar
MagoKimbra committed
213
    #if ENABLED(ADVANCE)
MagoKimbra's avatar
MagoKimbra committed
214 215
      block->initial_advance = initial_advance;
      block->final_advance = final_advance;
MagoKimbra's avatar
MagoKimbra committed
216
    #endif
MagoKimbra's avatar
MagoKimbra committed
217 218
  }
  CRITICAL_SECTION_END;
MagoKimbra's avatar
MagoKimbra committed
219
}
MagoKimbra's avatar
MagoKimbra committed
220

MagoKimbra's avatar
MagoKimbra committed
221
// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
MagoKimbra's avatar
MagoKimbra committed
222 223 224 225 226 227
// acceleration within the allotted distance.
FORCE_INLINE float max_allowable_speed(float acceleration, float target_velocity, float distance) {
  return sqrt(target_velocity * target_velocity - 2 * acceleration * distance);
}

// "Junction jerk" in this context is the immediate change in speed at the junction of two blocks.
MagoKimbra's avatar
MagoKimbra committed
228
// This method will calculate the junction jerk as the euclidean distance between the nominal
MagoKimbra's avatar
MagoKimbra committed
229
// velocities of the respective blocks.
MagoKimbra's avatar
MagoKimbra committed
230
// inline float junction_jerk(block_t *before, block_t *after) {
MagoKimbra's avatar
MagoKimbra committed
231 232 233 234 235
//  return sqrt(
//    pow((before->speed_x-after->speed_x), 2)+pow((before->speed_y-after->speed_y), 2));
//}

// The kernel called by planner_recalculate() when scanning the plan from last to first entry.
MagoKimbra's avatar
MagoKimbra committed
236
void planner_reverse_pass_kernel(block_t* previous, block_t* current, block_t* next) {
MagoKimbra's avatar
MagoKimbra committed
237
  if (!current) return;
MagoKimbra's avatar
MagoKimbra committed
238
  UNUSED(previous);
MagoKimbra's avatar
MagoKimbra committed
239 240 241 242 243

  if (next) {
    // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
    // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
    // check for maximum allowable speed reductions to ensure maximum possible planned speed.
MagoKimbra's avatar
MagoKimbra committed
244 245
    float max_entry_speed = current->max_entry_speed;
    if (current->entry_speed != max_entry_speed) {
MagoKimbra's avatar
MagoKimbra committed
246 247 248

      // If nominal length true, max junction speed is guaranteed to be reached. Only compute
      // for max allowable speed if block is decelerating and nominal length is false.
MagoKimbra's avatar
MagoKimbra committed
249 250 251
      if (!current->nominal_length_flag && max_entry_speed > next->entry_speed) {
        current->entry_speed = min(max_entry_speed,
                                   max_allowable_speed(-current->acceleration, next->entry_speed, current->millimeters));
MagoKimbra's avatar
MagoKimbra committed
252
      }
MagoKimbra's avatar
MagoKimbra committed
253
      else {
MagoKimbra's avatar
MagoKimbra committed
254
        current->entry_speed = max_entry_speed;
MagoKimbra's avatar
MagoKimbra committed
255 256 257 258 259 260 261
      }
      current->recalculate_flag = true;

    }
  } // Skip last block. Already initialized and set for recalculation.
}

MagoKimbra's avatar
MagoKimbra committed
262
// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
MagoKimbra's avatar
MagoKimbra committed
263 264 265
// implements the reverse pass.
void planner_reverse_pass() {
  uint8_t block_index = block_buffer_head;
MagoKimbra's avatar
MagoKimbra committed
266

MagoKimbra's avatar
MagoKimbra committed
267 268 269 270
  //Make a local copy of block_buffer_tail, because the interrupt can alter it
  CRITICAL_SECTION_START;
    unsigned char tail = block_buffer_tail;
  CRITICAL_SECTION_END
MagoKimbra's avatar
MagoKimbra committed
271

MagoKimbra's avatar
MagoKimbra committed
272 273
  if (BLOCK_MOD(block_buffer_head - tail + BLOCK_BUFFER_SIZE) > 3) { // moves queued
    block_index = BLOCK_MOD(block_buffer_head - 3);
MagoKimbra's avatar
MagoKimbra committed
274
    block_t* block[3] = { NULL, NULL, NULL };
MagoKimbra's avatar
MagoKimbra committed
275 276
    while (block_index != tail) {
      block_index = prev_block_index(block_index);
MagoKimbra's avatar
MagoKimbra committed
277 278
      block[2] = block[1];
      block[1] = block[0];
MagoKimbra's avatar
MagoKimbra committed
279 280 281 282 283 284 285
      block[0] = &block_buffer[block_index];
      planner_reverse_pass_kernel(block[0], block[1], block[2]);
    }
  }
}

// The kernel called by planner_recalculate() when scanning the plan from first to last entry.
MagoKimbra's avatar
MagoKimbra committed
286
void planner_forward_pass_kernel(block_t* previous, block_t* current, block_t* next) {
MagoKimbra's avatar
MagoKimbra committed
287
  if (!previous) return;
MagoKimbra's avatar
MagoKimbra committed
288
  UNUSED(next);
MagoKimbra's avatar
MagoKimbra committed
289 290 291 292 293 294 295 296

  // If the previous block is an acceleration block, but it is not long enough to complete the
  // full speed change within the block, we need to adjust the entry speed accordingly. Entry
  // speeds have already been reset, maximized, and reverse planned by reverse planner.
  // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
  if (!previous->nominal_length_flag) {
    if (previous->entry_speed < current->entry_speed) {
      double entry_speed = min(current->entry_speed,
MagoKimbra's avatar
MagoKimbra committed
297
                               max_allowable_speed(-previous->acceleration, previous->entry_speed, previous->millimeters));
MagoKimbra's avatar
MagoKimbra committed
298 299 300 301 302 303 304 305 306 307 308 309 310
      // Check for junction speed change
      if (current->entry_speed != entry_speed) {
        current->entry_speed = entry_speed;
        current->recalculate_flag = true;
      }
    }
  }
}

// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
// implements the forward pass.
void planner_forward_pass() {
  uint8_t block_index = block_buffer_tail;
MagoKimbra's avatar
MagoKimbra committed
311
  block_t* block[3] = { NULL, NULL, NULL };
MagoKimbra's avatar
MagoKimbra committed
312 313 314 315 316 317 318 319 320 321 322

  while (block_index != block_buffer_head) {
    block[0] = block[1];
    block[1] = block[2];
    block[2] = &block_buffer[block_index];
    planner_forward_pass_kernel(block[0], block[1], block[2]);
    block_index = next_block_index(block_index);
  }
  planner_forward_pass_kernel(block[1], block[2], NULL);
}

MagoKimbra's avatar
MagoKimbra committed
323 324
// Recalculates the trapezoid speed profiles for all blocks in the plan according to the
// entry_factor for each junction. Must be called by planner_recalculate() after
MagoKimbra's avatar
MagoKimbra committed
325 326 327
// updating the blocks.
void planner_recalculate_trapezoids() {
  int8_t block_index = block_buffer_tail;
MagoKimbra's avatar
MagoKimbra committed
328 329
  block_t* current;
  block_t* next = NULL;
MagoKimbra's avatar
MagoKimbra committed
330 331 332 333 334 335 336 337 338 339 340 341 342

  while (block_index != block_buffer_head) {
    current = next;
    next = &block_buffer[block_index];
    if (current) {
      // Recalculate if current block entry or exit junction speed has changed.
      if (current->recalculate_flag || next->recalculate_flag) {
        // NOTE: Entry and exit factors always > 0 by all previous logic operations.
        float nom = current->nominal_speed;
        calculate_trapezoid_for_block(current, current->entry_speed / nom, next->entry_speed / nom);
        current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
      }
    }
MagoKimbra's avatar
MagoKimbra committed
343
    block_index = next_block_index(block_index);
MagoKimbra's avatar
MagoKimbra committed
344 345 346 347
  }
  // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
  if (next) {
    float nom = next->nominal_speed;
MagoKimbra's avatar
MagoKimbra committed
348
    calculate_trapezoid_for_block(next, next->entry_speed / nom, (MINIMUM_PLANNER_SPEED) / nom);
MagoKimbra's avatar
MagoKimbra committed
349 350 351 352 353 354
    next->recalculate_flag = false;
  }
}

// Recalculates the motion plan according to the following algorithm:
//
MagoKimbra's avatar
MagoKimbra committed
355
//   1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_factor)
MagoKimbra's avatar
MagoKimbra committed
356 357
//      so that:
//     a. The junction jerk is within the set limit
MagoKimbra's avatar
MagoKimbra committed
358
//     b. No speed reduction within one block requires faster deceleration than the one, true constant
MagoKimbra's avatar
MagoKimbra committed
359
//        acceleration.
MagoKimbra's avatar
MagoKimbra committed
360 361
//   2. Go over every block in chronological order and dial down junction speed reduction values if
//     a. The speed increase within one block would require faster acceleration than the one, true
MagoKimbra's avatar
MagoKimbra committed
362 363
//        constant acceleration.
//
MagoKimbra's avatar
MagoKimbra committed
364 365
// When these stages are complete all blocks have an entry_factor that will allow all speed changes to
// be performed using only the one, true constant acceleration, and where no junction jerk is jerkier than
MagoKimbra's avatar
MagoKimbra committed
366 367 368 369
// the set limit. Finally it will:
//
//   3. Recalculate trapezoids for all blocks.

MagoKimbra's avatar
MagoKimbra committed
370
void planner_recalculate() {
MagoKimbra's avatar
MagoKimbra committed
371 372 373 374 375 376 377 378
  planner_reverse_pass();
  planner_forward_pass();
  planner_recalculate_trapezoids();
}

void plan_init() {
  block_buffer_head = block_buffer_tail = 0;
  memset(position, 0, sizeof(position)); // clear position
MagoKimbra's avatar
MagoKimbra committed
379
  for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = 0.0;
MagoKimbra's avatar
MagoKimbra committed
380 381 382
  previous_nominal_speed = 0.0;
}

MagoKimbra's avatar
MagoKimbra committed
383
#if ENABLED(AUTOTEMP)
MagoKimbra's avatar
MagoKimbra committed
384 385 386 387 388 389 390 391 392 393
  void getHighESpeed() {
    static float oldt = 0;

    if (!autotemp_enabled) return;
    if (degTargetHotend0() + 2 < autotemp_min) return; // probably temperature set to zero.

    float high = 0.0;
    uint8_t block_index = block_buffer_tail;

    while (block_index != block_buffer_head) {
MagoKimbra's avatar
MagoKimbra committed
394
      block_t* block = &block_buffer[block_index];
MagoKimbra's avatar
MagoKimbra committed
395 396
      if (block->steps[X_AXIS] || block->steps[Y_AXIS] || block->steps[Z_AXIS]) {
        float se = (float)block->steps[E_AXIS] / block->step_event_count * block->nominal_speed; // mm/sec;
MagoKimbra's avatar
MagoKimbra committed
397
        NOLESS(high, se);
MagoKimbra's avatar
MagoKimbra committed
398 399 400 401 402
      }
      block_index = next_block_index(block_index);
    }

    float t = autotemp_min + high * autotemp_factor;
MagoKimbra's avatar
MagoKimbra committed
403 404
    t = constrain(t, autotemp_min, autotemp_max);
    if (oldt > t) {
MagoKimbra's avatar
MagoKimbra committed
405 406
      t *= (1 - (AUTOTEMP_OLDWEIGHT));
      t += (AUTOTEMP_OLDWEIGHT) * oldt;
MagoKimbra's avatar
MagoKimbra committed
407
    }
MagoKimbra's avatar
MagoKimbra committed
408 409 410
    oldt = t;
    setTargetHotend0(t);
  }
MagoKimbra's avatar
MagoKimbra committed
411
#endif //AUTOTEMP
MagoKimbra's avatar
MagoKimbra committed
412 413

void check_axes_activity() {
MagoKimbra's avatar
MagoKimbra committed
414
  unsigned char axis_active[NUM_AXIS] = { 0 },
MagoKimbra's avatar
MagoKimbra committed
415
                tail_fan_speed = fanSpeed;
MagoKimbra's avatar
MagoKimbra committed
416
  #if ENABLED(BARICUDA)
MagoKimbra's avatar
MagoKimbra committed
417 418 419
    unsigned char tail_valve_pressure = ValvePressure,
                  tail_e_to_p_pressure = EtoPPressure;
  #endif
MagoKimbra's avatar
MagoKimbra committed
420
  #if ENABLED(LASERBEAM)
MagoKimbra's avatar
MagoKimbra committed
421 422
    unsigned char tail_laser_ttl_modulation = laser_ttl_modulation;
  #endif
MagoKimbra's avatar
MagoKimbra committed
423
  block_t* block;
MagoKimbra's avatar
MagoKimbra committed
424 425 426 427

  if (blocks_queued()) {
    uint8_t block_index = block_buffer_tail;
    tail_fan_speed = block_buffer[block_index].fan_speed;
MagoKimbra's avatar
MagoKimbra committed
428
    #if ENABLED(BARICUDA)
MagoKimbra's avatar
MagoKimbra committed
429 430 431 432
      block = &block_buffer[block_index];
      tail_valve_pressure = block->valve_pressure;
      tail_e_to_p_pressure = block->e_to_p_pressure;
    #endif
MagoKimbra's avatar
MagoKimbra committed
433
    #if ENABLED(LASERBEAM)
MagoKimbra's avatar
MagoKimbra committed
434 435 436 437 438
      tail_laser_ttl_modulation = block_buffer[block_index].laser_ttlmodulation;
    #endif

    while (block_index != block_buffer_head) {
      block = &block_buffer[block_index];
MagoKimbra's avatar
MagoKimbra committed
439
      for (int i = 0; i < NUM_AXIS; i++) if (block->steps[i]) axis_active[i]++;
MagoKimbra's avatar
MagoKimbra committed
440 441 442 443 444 445 446 447 448 449 450 451 452
      block_index = next_block_index(block_index);
    }
  }
  if (DISABLE_X && !axis_active[X_AXIS]) disable_x();
  if (DISABLE_Y && !axis_active[Y_AXIS]) disable_y();
  if (DISABLE_Z && !axis_active[Z_AXIS]) disable_z();
  if (DISABLE_E && !axis_active[E_AXIS]) {
    disable_e0();
    disable_e1();
    disable_e2();
    disable_e3();
  }

Simone Primarosa's avatar
Simone Primarosa committed
453
  #if HAS(FAN)
Simone Primarosa's avatar
Simone Primarosa committed
454
    #if ENABLED(FAN_KICKSTART_TIME)
MagoKimbra's avatar
MagoKimbra committed
455
      static millis_t fan_kick_end;
MagoKimbra's avatar
MagoKimbra committed
456
      if (tail_fan_speed) {
MagoKimbra's avatar
MagoKimbra committed
457
        millis_t ms = millis();
MagoKimbra's avatar
MagoKimbra committed
458 459
        if (fan_kick_end == 0) {
          // Just starting up fan - run at full power.
MagoKimbra's avatar
MagoKimbra committed
460
          fan_kick_end = ms + FAN_KICKSTART_TIME;
MagoKimbra's avatar
MagoKimbra committed
461
          tail_fan_speed = 255;
MagoKimbra's avatar
MagoKimbra committed
462 463
        }
        else if (fan_kick_end > ms)
MagoKimbra's avatar
MagoKimbra committed
464 465
          // Fan still spinning up.
          tail_fan_speed = 255;
MagoKimbra's avatar
MagoKimbra committed
466 467
        }
        else {
MagoKimbra's avatar
MagoKimbra committed
468 469
          fan_kick_end = 0;
        }
MagoKimbra's avatar
MagoKimbra committed
470
    #endif //FAN_KICKSTART_TIME
MagoKimbra's avatar
MagoKimbra committed
471
    #if ENABLED(FAN_MIN_PWM)
MagoKimbra's avatar
MagoKimbra committed
472
      #define CALC_FAN_SPEED (tail_fan_speed ? ( FAN_MIN_PWM + (tail_fan_speed * (255 - (FAN_MIN_PWM))) / 255 ) : 0)
MagoKimbra's avatar
MagoKimbra committed
473 474 475
    #else
      #define CALC_FAN_SPEED tail_fan_speed
    #endif // FAN_MIN_PWM
MagoKimbra's avatar
MagoKimbra committed
476
    #if ENABLED(FAN_SOFT_PWM)
MagoKimbra's avatar
MagoKimbra committed
477
      fanSpeedSoftPwm = CALC_FAN_SPEED;
MagoKimbra's avatar
MagoKimbra committed
478
    #else
MagoKimbra's avatar
MagoKimbra committed
479 480
      analogWrite(FAN_PIN, CALC_FAN_SPEED);
    #endif // FAN_SOFT_PWM
Simone Primarosa's avatar
Simone Primarosa committed
481
  #endif // HAS(FAN)
MagoKimbra's avatar
MagoKimbra committed
482

MagoKimbra's avatar
MagoKimbra committed
483
  #if ENABLED(AUTOTEMP)
MagoKimbra's avatar
MagoKimbra committed
484 485 486
    getHighESpeed();
  #endif

MagoKimbra's avatar
MagoKimbra committed
487
  #if ENABLED(BARICUDA)
Simone Primarosa's avatar
Simone Primarosa committed
488
    #if HAS(HEATER_1)
MagoKimbra's avatar
MagoKimbra committed
489
      analogWrite(HEATER_1_PIN, tail_valve_pressure);
MagoKimbra's avatar
MagoKimbra committed
490
    #endif
Simone Primarosa's avatar
Simone Primarosa committed
491
    #if HAS(HEATER_2)
MagoKimbra's avatar
MagoKimbra committed
492
      analogWrite(HEATER_2_PIN, tail_e_to_p_pressure);
MagoKimbra's avatar
MagoKimbra committed
493 494 495 496
    #endif
  #endif

  // add Laser TTL Modulation(PWM) Control
MagoKimbra's avatar
MagoKimbra committed
497
  #if ENABLED(LASERBEAM)
MagoKimbra's avatar
MagoKimbra committed
498 499 500 501 502
    analogWrite(LASER_TTL_PIN, tail_laser_ttl_modulation);
  #endif
}

float junction_deviation = 0.1;
MagoKimbra's avatar
MagoKimbra committed
503
// Add a new linear movement to the buffer. steps[X_AXIS], _y and _z is the absolute position in
MagoKimbra's avatar
MagoKimbra committed
504 505
// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
// calculation the caller must also provide the physical length of the line in millimeters.
MagoKimbra's avatar
MagoKimbra committed
506
#if ENABLED(AUTO_BED_LEVELING_FEATURE)
MagoKimbra's avatar
MagoKimbra committed
507
  void plan_buffer_line(float x, float y, float z, const float& e, float feed_rate, const uint8_t extruder, const uint8_t driver)
MagoKimbra's avatar
MagoKimbra committed
508
#else
MagoKimbra's avatar
MagoKimbra committed
509
  void plan_buffer_line(const float& x, const float& y, const float& z, const float& e, float feed_rate, const uint8_t extruder, const uint8_t driver)
MagoKimbra's avatar
MagoKimbra committed
510
#endif  // AUTO_BED_LEVELING_FEATURE
MagoKimbra's avatar
MagoKimbra committed
511
{
MagoKimbra's avatar
MagoKimbra committed
512 513 514 515 516 517 518 519 520 521

  #if ENABLED(ZWOBBLE)
    // Calculate ZWobble
    zwobble.InsertCorrection(z);
  #endif
  #if ENABLED(HYSTERESIS)
    // Calculate Hysteresis
    hysteresis.InsertCorrection(x, y, z, e);
  #endif

MagoKimbra's avatar
MagoKimbra committed
522 523 524
  // Calculate the buffer head after we push this byte
  int next_buffer_head = next_block_index(block_buffer_head);

MagoKimbra's avatar
MagoKimbra committed
525
  // If the buffer is full: good! That means we are well ahead of the robot.
MagoKimbra's avatar
MagoKimbra committed
526
  // Rest here until there is room in the buffer.
MagoKimbra's avatar
MagoKimbra committed
527
  while (block_buffer_tail == next_buffer_head) idle();
MagoKimbra's avatar
MagoKimbra committed
528

MagoKimbra's avatar
MagoKimbra committed
529
  #if ENABLED(AUTO_BED_LEVELING_FEATURE)
MagoKimbra's avatar
MagoKimbra committed
530 531 532 533 534
    apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
  #endif

  // The target position of the tool in absolute steps
  // Calculate target position in absolute steps
MagoKimbra's avatar
MagoKimbra committed
535 536 537 538 539 540 541
  // this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
  int32_t target[NUM_AXIS] = {
    lround(x * axis_steps_per_unit[X_AXIS]),
    lround(y * axis_steps_per_unit[Y_AXIS]),
    lround(z * axis_steps_per_unit[Z_AXIS]),
    lround(e * axis_steps_per_unit[E_AXIS + extruder])
  };
MagoKimbra's avatar
MagoKimbra committed
542

MagoKimbra's avatar
MagoKimbra committed
543 544 545 546 547 548 549 550 551 552 553
  // If changing extruder have to recalculate current position based on 
  // the steps-per-mm value for the new extruder.
  #if EXTRUDERS > 1
    if(last_extruder != extruder && axis_steps_per_unit[E_AXIS + extruder] != 
                                    axis_steps_per_unit[E_AXIS + last_extruder]) {
      float factor = float(axis_steps_per_unit[E_AXIS + extruder]) /
                     float(axis_steps_per_unit[E_AXIS + last_extruder]);
      position[E_AXIS] = lround(position[E_AXIS] * factor);
    }
  #endif

554 555 556 557
  int32_t dx = target[X_AXIS] - position[X_AXIS],
          dy = target[Y_AXIS] - position[Y_AXIS],
          dz = target[Z_AXIS] - position[Z_AXIS],
          de = target[E_AXIS] - position[E_AXIS];
Simone Primarosa's avatar
Simone Primarosa committed
558
  #if MECH(COREXY)
559 560 561 562 563
    int32_t da = dx + COREX_YZ_FACTOR * dy;
    int32_t db = dx - COREX_YZ_FACTOR * dy;
  #elif MECH(COREYX)
    int32_t da = dy + COREX_YZ_FACTOR * dx;
    int32_t db = dy - COREX_YZ_FACTOR * dx;
Simone Primarosa's avatar
Simone Primarosa committed
564
  #elif MECH(COREXZ)
565 566 567 568 569
    int32_t da = dx + COREX_YZ_FACTOR * dz;
    int32_t dc = dx - COREX_YZ_FACTOR * dz;
  #elif MECH(COREZX)
    int32_t da = dz + COREX_YZ_FACTOR * dx;
    int32_t dc = dz - COREX_YZ_FACTOR * dx;
MagoKimbra's avatar
MagoKimbra committed
570 571
  #endif

MagoKimbra's avatar
MagoKimbra committed
572
  #if ENABLED(PREVENT_DANGEROUS_EXTRUDE)
MagoKimbra's avatar
MagoKimbra committed
573
    if (de) {
MagoKimbra's avatar
MagoKimbra committed
574
      #if ENABLED(NPR2)
MagoKimbra's avatar
MagoKimbra committed
575
        if (extruder != 1)
MagoKimbra's avatar
MagoKimbra committed
576
      #endif
MagoKimbra's avatar
MagoKimbra committed
577
        {
MagoKimbra's avatar
MagoKimbra committed
578
          if (degHotend(extruder) < extrude_min_temp && !(DEBUGGING(DRYRUN))) {
MagoKimbra's avatar
MagoKimbra committed
579
            position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
MagoKimbra's avatar
MagoKimbra committed
580
            de = 0; // no difference
MagoKimbra's avatar
MagoKimbra committed
581
            ECHO_LM(ER, SERIAL_ERR_COLD_EXTRUDE_STOP);
MagoKimbra's avatar
MagoKimbra committed
582 583 584
          }
        }

MagoKimbra's avatar
MagoKimbra committed
585
      #if ENABLED(PREVENT_LENGTHY_EXTRUDE)
MagoKimbra's avatar
MagoKimbra committed
586
        if (labs(de) > axis_steps_per_unit[E_AXIS + extruder] * (EXTRUDE_MAXLENGTH)) {
Simone Primarosa's avatar
Simone Primarosa committed
587
          #if ENABLED(EASY_LOAD)
MagoKimbra's avatar
MagoKimbra committed
588 589 590
            if (!allow_lengthy_extrude_once) {
          #endif
          position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
MagoKimbra's avatar
MagoKimbra committed
591
          de = 0; // no difference
MagoKimbra's avatar
MagoKimbra committed
592
          ECHO_LM(ER, SERIAL_ERR_LONG_EXTRUDE_STOP);
MagoKimbra's avatar
MagoKimbra committed
593
          #if ENABLED(EASY_LOAD)
MagoKimbra's avatar
MagoKimbra committed
594 595 596 597 598 599 600 601 602
            }
            allow_lengthy_extrude_once = false;
          #endif
        }
      #endif // PREVENT_LENGTHY_EXTRUDE
    }
  #endif // PREVENT_DANGEROUS_EXTRUDE

  // Prepare to set up new block
MagoKimbra's avatar
MagoKimbra committed
603
  block_t* block = &block_buffer[block_buffer_head];
MagoKimbra's avatar
MagoKimbra committed
604 605 606 607 608

  // Mark block as not busy (Not executed by the stepper interrupt)
  block->busy = false;

  // Number of steps for each axis
609
  #if MECH(COREXY) || MECH(COREYX)
MagoKimbra's avatar
MagoKimbra committed
610
    // corexy planning
MagoKimbra's avatar
MagoKimbra committed
611 612
    block->steps[A_AXIS] = labs(da);
    block->steps[B_AXIS] = labs(db);
MagoKimbra's avatar
MagoKimbra committed
613
    block->steps[Z_AXIS] = labs(dz);
614
  #elif MECH(COREXZ) || MECH(COREZX)
MagoKimbra's avatar
MagoKimbra committed
615
    // corexz planning
MagoKimbra's avatar
MagoKimbra committed
616
    block->steps[A_AXIS] = labs(da);
MagoKimbra's avatar
MagoKimbra committed
617
    block->steps[Y_AXIS] = labs(dy);
MagoKimbra's avatar
MagoKimbra committed
618
    block->steps[C_AXIS] = labs(dc);
MagoKimbra's avatar
MagoKimbra committed
619 620 621 622
  #else
    // default non-h-bot planning
    block->steps[X_AXIS] = labs(dx);
    block->steps[Y_AXIS] = labs(dy);
MagoKimbra's avatar
MagoKimbra committed
623
    block->steps[Z_AXIS] = labs(dz);
MagoKimbra's avatar
MagoKimbra committed
624 625 626
  #endif

  block->steps[E_AXIS] = labs(de);
MagoKimbra's avatar
MagoKimbra committed
627
  block->steps[E_AXIS] *= volumetric_multiplier[extruder];
MagoKimbra's avatar
MagoKimbra committed
628
  block->steps[E_AXIS] *= extruder_multiplier[extruder];
MagoKimbra's avatar
MagoKimbra committed
629 630 631 632
  block->steps[E_AXIS] /= 100;
  block->step_event_count = max(block->steps[X_AXIS], max(block->steps[Y_AXIS], max(block->steps[Z_AXIS], block->steps[E_AXIS])));

  // Bail if this is a zero-length block
Simone Primarosa's avatar
Simone Primarosa committed
633
  if (block->step_event_count <= DROP_SEGMENTS) return;
MagoKimbra's avatar
MagoKimbra committed
634 635

  block->fan_speed = fanSpeed;
MagoKimbra's avatar
MagoKimbra committed
636

MagoKimbra's avatar
MagoKimbra committed
637
  #if ENABLED(BARICUDA)
MagoKimbra's avatar
MagoKimbra committed
638 639 640
    block->valve_pressure = ValvePressure;
    block->e_to_p_pressure = EtoPPressure;
  #endif
MagoKimbra's avatar
MagoKimbra committed
641

MagoKimbra's avatar
MagoKimbra committed
642 643
  // For a mixing extruder, get steps for each
  #if ENABLED(COLOR_MIXING_EXTRUDER)
MagoKimbra's avatar
MagoKimbra committed
644
    for (uint8_t i = 0; i < DRIVER_EXTRUDERS; i++)
MagoKimbra's avatar
MagoKimbra committed
645
      block->mix_event_count[i] = block->steps[E_AXIS] * mixing_factor[i];
MagoKimbra's avatar
MagoKimbra committed
646 647
  #endif

MagoKimbra's avatar
MagoKimbra committed
648
  // Add update block variables for LASER BEAM control 
MagoKimbra's avatar
MagoKimbra committed
649
  #if ENABLED(LASERBEAM)
MagoKimbra's avatar
MagoKimbra committed
650 651 652 653
    block->laser_ttlmodulation = laser_ttl_modulation;
  #endif

  // Compute direction bits for this block 
654
  uint8_t dirb = 0;
655
  #if MECH(COREXY) || MECH(COREYX)
MagoKimbra's avatar
MagoKimbra committed
656 657 658 659 660
    if (dx < 0) SBI(dirb, X_HEAD); // Save the real Extruder (head) direction in X Axis
    if (dy < 0) SBI(dirb, Y_HEAD); // ...and Y
    if (dz < 0) SBI(dirb, Z_AXIS);
    if (da < 0) SBI(dirb, A_AXIS); // Motor A direction
    if (db < 0) SBI(dirb, B_AXIS); // Motor B direction
661
  #elif MECH(COREXZ) || MECH(COREZX)
MagoKimbra's avatar
MagoKimbra committed
662 663 664 665 666
    if (dx < 0) SBI(dirb, X_HEAD); // Save the real Extruder (head) direction in X Axis
    if (dy < 0) SBI(dirb, Y_AXIS);
    if (dz < 0) SBI(dirb, Z_HEAD); // ...and Z
    if (da < 0) SBI(dirb, A_AXIS); // Motor A direction
    if (dc < 0) SBI(dirb, C_AXIS); // Motor B direction
MagoKimbra's avatar
MagoKimbra committed
667
  #else
MagoKimbra's avatar
MagoKimbra committed
668
    if (dx < 0) SBI(dirb, X_AXIS);
MagoKimbra's avatar
MagoKimbra committed
669
    if (dy < 0) SBI(dirb, Y_AXIS);
MagoKimbra's avatar
MagoKimbra committed
670
    if (dz < 0) SBI(dirb, Z_AXIS);
MagoKimbra's avatar
MagoKimbra committed
671
  #endif
MagoKimbra's avatar
MagoKimbra committed
672
  if (de < 0) SBI(dirb, E_AXIS);
673
  block->direction_bits = dirb;
MagoKimbra's avatar
MagoKimbra committed
674 675 676

  block->active_driver = driver;

677
  // Enable active axes
678
  #if MECH(COREXY) || MECH(COREYX)
MagoKimbra's avatar
MagoKimbra committed
679 680 681 682
    if (block->steps[A_AXIS] || block->steps[B_AXIS]) {
      enable_x();
      enable_y();
    }
MagoKimbra's avatar
MagoKimbra committed
683
    #if DISABLED(Z_LATE_ENABLE)
MagoKimbra's avatar
MagoKimbra committed
684 685
      if (block->steps[Z_AXIS]) enable_z();
    #endif
686
  #elif MECH(COREXZ) || MECH(COREZX)
MagoKimbra's avatar
MagoKimbra committed
687 688 689 690
    if (block->steps[A_AXIS] || block->steps[C_AXIS]) {
      enable_x();
      enable_z();
    }
MagoKimbra's avatar
MagoKimbra committed
691
    if (block->steps[Y_AXIS]) enable_y();
MagoKimbra's avatar
MagoKimbra committed
692 693 694
  #else
    if (block->steps[X_AXIS]) enable_x();
    if (block->steps[Y_AXIS]) enable_y();
MagoKimbra's avatar
MagoKimbra committed
695
    #if DISABLED(Z_LATE_ENABLE)
MagoKimbra's avatar
MagoKimbra committed
696 697
      if (block->steps[Z_AXIS]) enable_z();
    #endif
MagoKimbra's avatar
MagoKimbra committed
698 699 700 701
  #endif

  // Enable extruder(s)
  if (block->steps[E_AXIS]) {
MagoKimbra's avatar
MagoKimbra committed
702
    #if DISABLED(MKR4) && DISABLED(NPR2)
MagoKimbra's avatar
MagoKimbra committed
703 704
      if (DISABLE_INACTIVE_EXTRUDER) { //enable only selected extruder

MagoKimbra's avatar
MagoKimbra committed
705
        for (int i = 0; i < EXTRUDERS; i++)
MagoKimbra's avatar
MagoKimbra committed
706 707 708 709 710
          if (g_uc_extruder_last_move[i] > 0) g_uc_extruder_last_move[i]--;

        switch(extruder) {
          case 0:
            enable_e0();
MagoKimbra's avatar
MagoKimbra committed
711
            g_uc_extruder_last_move[0] = (BLOCK_BUFFER_SIZE) * 2;
MagoKimbra's avatar
MagoKimbra committed
712 713 714 715 716 717
            #if EXTRUDERS > 1
              if (g_uc_extruder_last_move[1] == 0) disable_e1();
              #if EXTRUDERS > 2
                if (g_uc_extruder_last_move[2] == 0) disable_e2();
                #if EXTRUDERS > 3
                  if (g_uc_extruder_last_move[3] == 0) disable_e3();
MagoKimbra's avatar
MagoKimbra committed
718 719 720 721 722 723
                  #if EXTRUDERS > 4
                    if (g_uc_extruder_last_move[4] == 0) disable_e4();
                    #if EXTRUDERS > 5
                      if (g_uc_extruder_last_move[5] == 0) disable_e5();
                    #endif
                  #endif
MagoKimbra's avatar
MagoKimbra committed
724 725 726 727 728 729 730
                #endif
              #endif
            #endif
          break;
          #if EXTRUDERS > 1
            case 1:
              enable_e1();
MagoKimbra's avatar
MagoKimbra committed
731
              g_uc_extruder_last_move[1] = (BLOCK_BUFFER_SIZE) * 2;
MagoKimbra's avatar
MagoKimbra committed
732 733 734 735 736
              if (g_uc_extruder_last_move[0] == 0) disable_e0();
              #if EXTRUDERS > 2
                if (g_uc_extruder_last_move[2] == 0) disable_e2();
                #if EXTRUDERS > 3
                  if (g_uc_extruder_last_move[3] == 0) disable_e3();
MagoKimbra's avatar
MagoKimbra committed
737 738 739 740 741 742
                  #if EXTRUDERS > 4
                    if (g_uc_extruder_last_move[4] == 0) disable_e4();
                    #if EXTRUDERS > 5
                      if (g_uc_extruder_last_move[5] == 0) disable_e5();
                    #endif
                  #endif
MagoKimbra's avatar
MagoKimbra committed
743 744 745 746 747 748
                #endif
              #endif
            break;
            #if EXTRUDERS > 2
              case 2:
                enable_e2();
MagoKimbra's avatar
MagoKimbra committed
749
                g_uc_extruder_last_move[2] = (BLOCK_BUFFER_SIZE) * 2;
MagoKimbra's avatar
MagoKimbra committed
750 751 752 753
                if (g_uc_extruder_last_move[0] == 0) disable_e0();
                if (g_uc_extruder_last_move[1] == 0) disable_e1();
                #if EXTRUDERS > 3
                  if (g_uc_extruder_last_move[3] == 0) disable_e3();
MagoKimbra's avatar
MagoKimbra committed
754 755 756 757 758 759
                  #if EXTRUDERS > 4
                    if (g_uc_extruder_last_move[4] == 0) disable_e4();
                    #if EXTRUDERS > 5
                      if (g_uc_extruder_last_move[5] == 0) disable_e5();
                    #endif
                  #endif
MagoKimbra's avatar
MagoKimbra committed
760 761 762 763 764
                #endif
              break;
              #if EXTRUDERS > 3
                case 3:
                  enable_e3();
MagoKimbra's avatar
MagoKimbra committed
765
                  g_uc_extruder_last_move[3] = (BLOCK_BUFFER_SIZE) * 2;
MagoKimbra's avatar
MagoKimbra committed
766 767 768
                  if (g_uc_extruder_last_move[0] == 0) disable_e0();
                  if (g_uc_extruder_last_move[1] == 0) disable_e1();
                  if (g_uc_extruder_last_move[2] == 0) disable_e2();
MagoKimbra's avatar
MagoKimbra committed
769 770 771 772 773 774
                  #if EXTRUDERS > 4
                    if (g_uc_extruder_last_move[4] == 0) disable_e4();
                    #if EXTRUDERS > 5
                      if (g_uc_extruder_last_move[5] == 0) disable_e5();
                    #endif
                  #endif
MagoKimbra's avatar
MagoKimbra committed
775
                break;
MagoKimbra's avatar
MagoKimbra committed
776 777 778 779 780 781 782 783 784 785 786 787 788 789 790 791 792 793 794 795 796 797 798 799
                #if EXTRUDERS > 4
                  case 4:
                    enable_e4();
                    g_uc_extruder_last_move[4] = (BLOCK_BUFFER_SIZE) * 2;
                    if (g_uc_extruder_last_move[0] == 0) disable_e0();
                    if (g_uc_extruder_last_move[1] == 0) disable_e1();
                    if (g_uc_extruder_last_move[2] == 0) disable_e2();
                    if (g_uc_extruder_last_move[3] == 0) disable_e3();
                    #if EXTRUDERS > 5
                      if (g_uc_extruder_last_move[5] == 0) disable_e5();
                    #endif
                  break;
                  #if EXTRUDERS > 5
                    case 4:
                      enable_e5();
                      g_uc_extruder_last_move[5] = (BLOCK_BUFFER_SIZE) * 2;
                      if (g_uc_extruder_last_move[0] == 0) disable_e0();
                      if (g_uc_extruder_last_move[1] == 0) disable_e1();
                      if (g_uc_extruder_last_move[2] == 0) disable_e2();
                      if (g_uc_extruder_last_move[3] == 0) disable_e3();
                      if (g_uc_extruder_last_move[4] == 0) disable_e4();
                    break;
                  #endif // EXTRUDERS > 5
                #endif // EXTRUDERS > 4
MagoKimbra's avatar
MagoKimbra committed
800 801 802 803 804 805 806 807 808 809 810
              #endif // EXTRUDERS > 3
            #endif // EXTRUDERS > 2
          #endif // EXTRUDERS > 1
        }
      }
      else //enable all
      {
        enable_e0();
        enable_e1();
        enable_e2();
        enable_e3();
MagoKimbra's avatar
MagoKimbra committed
811 812
        enable_e4();
        enable_e5();
MagoKimbra's avatar
MagoKimbra committed
813 814 815 816 817 818 819 820 821 822 823 824 825 826 827 828 829 830 831
      }
    #else //MKR4 or NPr2
      switch(extruder)
      {
        case 0:
          enable_e0();
        break;
        case 1:
          enable_e1();
        break;
        case 2:
          enable_e0();
        break;
        case 3:
          enable_e1();
        break;
      }
    #endif //!MKR4 && !NPR2
  }
MagoKimbra's avatar
MagoKimbra committed
832 833 834 835 836

  if (block->steps[E_AXIS])
    NOLESS(feed_rate, minimumfeedrate);
  else
    NOLESS(feed_rate, mintravelfeedrate);
MagoKimbra's avatar
MagoKimbra committed
837 838

  /**
MagoKimbra's avatar
MagoKimbra committed
839
   * This part of the code calculates the total length of the movement.
MagoKimbra's avatar
MagoKimbra committed
840 841 842
   * For cartesian bots, the X_AXIS is the real X movement and same for Y_AXIS.
   * But for corexy bots, that is not true. The "X_AXIS" and "Y_AXIS" motors (that should be named to A_AXIS
   * and B_AXIS) cannot be used for X and Y length, because A=X+Y and B=X-Y.
MagoKimbra's avatar
MagoKimbra committed
843
   * So we need to create other 2 "AXIS", named X_HEAD and Y_HEAD, meaning the real displacement of the Head.
MagoKimbra's avatar
MagoKimbra committed
844
   * Having the real displacement of the head, we can calculate the total movement length and apply the desired speed.
MagoKimbra's avatar
MagoKimbra committed
845
   */
846
  #if MECH(COREXY) || MECH(COREYX)
MagoKimbra's avatar
MagoKimbra committed
847 848 849
    float delta_mm[6];
    delta_mm[X_HEAD] = dx / axis_steps_per_unit[A_AXIS];
    delta_mm[Y_HEAD] = dy / axis_steps_per_unit[B_AXIS];
MagoKimbra's avatar
MagoKimbra committed
850
    delta_mm[Z_AXIS] = dz / axis_steps_per_unit[Z_AXIS];
MagoKimbra's avatar
MagoKimbra committed
851 852
    delta_mm[A_AXIS] = da / axis_steps_per_unit[A_AXIS];
    delta_mm[B_AXIS] = db / axis_steps_per_unit[B_AXIS];
853
  #elif MECH(COREXZ) || MECH(COREZX)
MagoKimbra's avatar
MagoKimbra committed
854 855 856 857
    float delta_mm[6];
    delta_mm[X_HEAD] = dx / axis_steps_per_unit[A_AXIS];
    delta_mm[Y_AXIS] = dy / axis_steps_per_unit[Y_AXIS];
    delta_mm[Z_HEAD] = dz / axis_steps_per_unit[C_AXIS];
MagoKimbra's avatar
MagoKimbra committed
858 859
    delta_mm[A_AXIS] = da / axis_steps_per_unit[A_AXIS];
    delta_mm[C_AXIS] = dc / axis_steps_per_unit[C_AXIS];
MagoKimbra's avatar
MagoKimbra committed
860 861 862 863
  #else
    float delta_mm[4];
    delta_mm[X_AXIS] = dx / axis_steps_per_unit[X_AXIS];
    delta_mm[Y_AXIS] = dy / axis_steps_per_unit[Y_AXIS];
MagoKimbra's avatar
MagoKimbra committed
864
    delta_mm[Z_AXIS] = dz / axis_steps_per_unit[Z_AXIS];
MagoKimbra's avatar
MagoKimbra committed
865
  #endif
MagoKimbra's avatar
MagoKimbra committed
866
  delta_mm[E_AXIS] = (de / axis_steps_per_unit[E_AXIS + extruder]) * volumetric_multiplier[extruder] * extruder_multiplier[extruder] / 100.0;
MagoKimbra's avatar
MagoKimbra committed
867

Simone Primarosa's avatar
Simone Primarosa committed
868
  if (block->steps[X_AXIS] <= DROP_SEGMENTS && block->steps[Y_AXIS] <= DROP_SEGMENTS && block->steps[Z_AXIS] <= DROP_SEGMENTS) {
MagoKimbra's avatar
MagoKimbra committed
869 870 871 872
    block->millimeters = fabs(delta_mm[E_AXIS]);
  }
  else {
    block->millimeters = sqrt(
873
      #if MECH(COREXY) || MECH(COREYX)
MagoKimbra's avatar
MagoKimbra committed
874
        square(delta_mm[X_HEAD]) + square(delta_mm[Y_HEAD]) + square(delta_mm[Z_AXIS])
875
      #elif MECH(COREXZ) || MECH(COREZX)
MagoKimbra's avatar
MagoKimbra committed
876
        square(delta_mm[X_HEAD]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_HEAD])
MagoKimbra's avatar
MagoKimbra committed
877
      #else
MagoKimbra's avatar
MagoKimbra committed
878
        square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS])
MagoKimbra's avatar
MagoKimbra committed
879 880 881
      #endif
    );
  }
MagoKimbra's avatar
MagoKimbra committed
882
  float inverse_millimeters = 1.0 / block->millimeters;  // Inverse millimeters to remove multiple divides
MagoKimbra's avatar
MagoKimbra committed
883 884 885 886 887 888 889

  // Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
  float inverse_second = feed_rate * inverse_millimeters;

  int moves_queued = movesplanned();

  // Slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
MagoKimbra's avatar
MagoKimbra committed
890
  #if ENABLED(OLD_SLOWDOWN) || ENABLED(SLOWDOWN)
MagoKimbra's avatar
MagoKimbra committed
891
    bool mq = moves_queued > 1 && moves_queued < (BLOCK_BUFFER_SIZE) / 2;
MagoKimbra's avatar
MagoKimbra committed
892
    #if ENABLED(OLD_SLOWDOWN)
MagoKimbra's avatar
MagoKimbra committed
893
      if (mq) feed_rate *= 2.0 * moves_queued / (BLOCK_BUFFER_SIZE);
MagoKimbra's avatar
MagoKimbra committed
894
    #endif
MagoKimbra's avatar
MagoKimbra committed
895
    #if ENABLED(SLOWDOWN)
MagoKimbra's avatar
MagoKimbra committed
896
      //  segment time im micro seconds
MagoKimbra's avatar
MagoKimbra committed
897
      unsigned long segment_time = lround(1000000.0 / inverse_second);
MagoKimbra's avatar
MagoKimbra committed
898 899 900 901
      if (mq) {
        if (segment_time < minsegmenttime) {
          // buffer is draining, add extra time.  The amount of time added increases if the buffer is still emptied more.
          inverse_second = 1000000.0 / (segment_time + lround(2 * (minsegmenttime - segment_time) / moves_queued));
Simone Primarosa's avatar
Simone Primarosa committed
902
          #if ENABLED(XY_FREQUENCY_LIMIT)
MagoKimbra's avatar
MagoKimbra committed
903 904 905 906 907 908 909 910 911 912
            segment_time = lround(1000000.0 / inverse_second);
          #endif
        }
      }
    #endif
  #endif

  block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0
  block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0

MagoKimbra's avatar
MagoKimbra committed
913
  #if ENABLED(FILAMENT_SENSOR)
MagoKimbra's avatar
MagoKimbra committed
914 915 916 917 918 919 920 921 922 923 924 925 926 927 928 929 930 931 932 933 934 935 936 937 938 939 940 941
    //FMM update ring buffer used for delay with filament measurements

    if (extruder == FILAMENT_SENSOR_EXTRUDER_NUM && delay_index2 > -1) {  //only for extruder with filament sensor and if ring buffer is initialized

      const int MMD = MAX_MEASUREMENT_DELAY + 1, MMD10 = MMD * 10;

      delay_dist += delta_mm[E_AXIS];  // increment counter with next move in e axis
      while (delay_dist >= MMD10) delay_dist -= MMD10; // loop around the buffer
      while (delay_dist < 0) delay_dist += MMD10;

      delay_index1 = delay_dist / 10.0;  // calculate index
      delay_index1 = constrain(delay_index1, 0, MAX_MEASUREMENT_DELAY); // (already constrained above)

      if (delay_index1 != delay_index2) { // moved index
        meas_sample = widthFil_to_size_ratio() - 100;  // Subtract 100 to reduce magnitude - to store in a signed char
        while (delay_index1 != delay_index2) {
          // Increment and loop around buffer
          if (++delay_index2 >= MMD) delay_index2 -= MMD;
          delay_index2 = constrain(delay_index2, 0, MAX_MEASUREMENT_DELAY);
          measurement_delay[delay_index2] = meas_sample;
        }
      }
    }
  #endif

  // Calculate and limit speed in mm/sec for each axis
  float current_speed[NUM_AXIS];
  float speed_factor = 1.0; //factor <=1 do decrease speed
MagoKimbra's avatar
MagoKimbra committed
942
  for (int i = 0; i < NUM_AXIS; i++) {
MagoKimbra's avatar
MagoKimbra committed
943 944 945 946 947 948
    current_speed[i] = delta_mm[i] * inverse_second;
    float cs = fabs(current_speed[i]), mf = max_feedrate[i];
    if (cs > mf) speed_factor = min(speed_factor, mf / cs);
  }

  // Max segement time in us.
Simone Primarosa's avatar
Simone Primarosa committed
949
  #if ENABLED(XY_FREQUENCY_LIMIT)
MagoKimbra's avatar
MagoKimbra committed
950 951 952 953 954

    // Check and limit the xy direction change frequency
    unsigned char direction_change = block->direction_bits ^ old_direction_bits;
    old_direction_bits = block->direction_bits;
    segment_time = lround((float)segment_time / speed_factor);
MagoKimbra's avatar
MagoKimbra committed
955

MagoKimbra's avatar
MagoKimbra committed
956 957 958 959 960 961 962
    long xs0 = axis_segment_time[X_AXIS][0],
         xs1 = axis_segment_time[X_AXIS][1],
         xs2 = axis_segment_time[X_AXIS][2],
         ys0 = axis_segment_time[Y_AXIS][0],
         ys1 = axis_segment_time[Y_AXIS][1],
         ys2 = axis_segment_time[Y_AXIS][2];

MagoKimbra's avatar
MagoKimbra committed
963
    if (TEST(direction_change, X_AXIS)) {
MagoKimbra's avatar
MagoKimbra committed
964 965 966 967 968 969
      xs2 = axis_segment_time[X_AXIS][2] = xs1;
      xs1 = axis_segment_time[X_AXIS][1] = xs0;
      xs0 = 0;
    }
    xs0 = axis_segment_time[X_AXIS][0] = xs0 + segment_time;

MagoKimbra's avatar
MagoKimbra committed
970
    if (TEST(direction_change, Y_AXIS)) {
MagoKimbra's avatar
MagoKimbra committed
971 972 973 974 975 976 977 978 979 980
      ys2 = axis_segment_time[Y_AXIS][2] = axis_segment_time[Y_AXIS][1];
      ys1 = axis_segment_time[Y_AXIS][1] = axis_segment_time[Y_AXIS][0];
      ys0 = 0;
    }
    ys0 = axis_segment_time[Y_AXIS][0] = ys0 + segment_time;

    long max_x_segment_time = max(xs0, max(xs1, xs2)),
         max_y_segment_time = max(ys0, max(ys1, ys2)),
         min_xy_segment_time = min(max_x_segment_time, max_y_segment_time);
    if (min_xy_segment_time < MAX_FREQ_TIME) {
MagoKimbra's avatar
MagoKimbra committed
981
      float low_sf = speed_factor * min_xy_segment_time / (MAX_FREQ_TIME);
MagoKimbra's avatar
MagoKimbra committed
982 983 984 985
      speed_factor = min(speed_factor, low_sf);
    }
  #endif // XY_FREQUENCY_LIMIT

MagoKimbra's avatar
MagoKimbra committed
986
  // Correct the speed
MagoKimbra's avatar
MagoKimbra committed
987 988 989 990 991 992 993 994
  if (speed_factor < 1.0) {
    for (unsigned char i = 0; i < NUM_AXIS; i++) current_speed[i] *= speed_factor;
    block->nominal_speed *= speed_factor;
    block->nominal_rate *= speed_factor;
  }

  // Compute and limit the acceleration rate for the trapezoid generator.
  float steps_per_mm = block->step_event_count / block->millimeters;
MagoKimbra's avatar
MagoKimbra committed
995
  long bsx = block->steps[X_AXIS], bsy = block->steps[Y_AXIS], bsz = block->steps[Z_AXIS], bse = block->steps[E_AXIS];
MagoKimbra's avatar
MagoKimbra committed
996
  if (bsx == 0 && bsy == 0 && bsz == 0) {
997
    block->acceleration_st = ceil(retract_acceleration[extruder] * steps_per_mm); // convert to: acceleration steps/sec^2
MagoKimbra's avatar
MagoKimbra committed
998 999 1000 1001 1002 1003 1004 1005 1006 1007 1008 1009
  }
  else if (bse == 0) {
    block->acceleration_st = ceil(travel_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
  }
  else {
    block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
  }
  // Limit acceleration per axis
  unsigned long acc_st = block->acceleration_st,
                xsteps = axis_steps_per_sqr_second[X_AXIS],
                ysteps = axis_steps_per_sqr_second[Y_AXIS],
                zsteps = axis_steps_per_sqr_second[Z_AXIS],
MagoKimbra's avatar
MagoKimbra committed
1010 1011 1012 1013 1014 1015
                esteps = axis_steps_per_sqr_second[E_AXIS + extruder],
                allsteps = block->step_event_count;
  if (xsteps < (acc_st * bsx) / allsteps) acc_st = (xsteps * allsteps) / bsx;
  if (ysteps < (acc_st * bsy) / allsteps) acc_st = (ysteps * allsteps) / bsy;
  if (zsteps < (acc_st * bsz) / allsteps) acc_st = (zsteps * allsteps) / bsz;
  if (esteps < (acc_st * bse) / allsteps) acc_st = (esteps * allsteps) / bse;
MagoKimbra's avatar
MagoKimbra committed
1016

MagoKimbra's avatar
MagoKimbra committed
1017 1018 1019
  block->acceleration_st = acc_st;
  block->acceleration = acc_st / steps_per_mm;

MagoKimbra's avatar
MagoKimbra committed
1020
  #ifdef __SAM3X8E__
MagoKimbra's avatar
MagoKimbra committed
1021
    block->acceleration_rate = (long)(acc_st * (4294967296.0 / (HAL_TIMER_RATE)));
MagoKimbra's avatar
MagoKimbra committed
1022 1023 1024 1025 1026 1027 1028 1029
  #else
    block->acceleration_rate = (long)(acc_st * 16777216.0 / (F_CPU / 8.0));
  #endif

  #if 0  // Use old jerk for now
    // Compute path unit vector
    double unit_vec[3];

MagoKimbra's avatar
MagoKimbra committed
1030 1031 1032
    unit_vec[X_AXIS] = delta_mm[X_AXIS] * inverse_millimeters;
    unit_vec[Y_AXIS] = delta_mm[Y_AXIS] * inverse_millimeters;
    unit_vec[Z_AXIS] = delta_mm[Z_AXIS] * inverse_millimeters;
MagoKimbra's avatar
MagoKimbra committed
1033 1034 1035 1036

    // Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
    // Let a circle be tangent to both previous and current path line segments, where the junction
    // deviation is defined as the distance from the junction to the closest edge of the circle,
MagoKimbra's avatar
MagoKimbra committed
1037
    // collinear with the circle center. The circular segment joining the two paths represents the
MagoKimbra's avatar
MagoKimbra committed
1038 1039 1040 1041 1042 1043 1044 1045 1046 1047 1048 1049
    // path of centripetal acceleration. Solve for max velocity based on max acceleration about the
    // radius of the circle, defined indirectly by junction deviation. This may be also viewed as
    // path width or max_jerk in the previous grbl version. This approach does not actually deviate
    // from path, but used as a robust way to compute cornering speeds, as it takes into account the
    // nonlinearities of both the junction angle and junction velocity.
    double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed

    // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
    if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
      // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
      // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
      double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
MagoKimbra's avatar
MagoKimbra committed
1050 1051
                         - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
                         - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
MagoKimbra's avatar
MagoKimbra committed
1052 1053
      // Skip and use default max junction speed for 0 degree acute junction.
      if (cos_theta < 0.95) {
MagoKimbra's avatar
MagoKimbra committed
1054
        vmax_junction = min(previous_nominal_speed, block->nominal_speed);
MagoKimbra's avatar
MagoKimbra committed
1055 1056 1057
        // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
        if (cos_theta > -0.95) {
          // Compute maximum junction velocity based on maximum acceleration and junction deviation
MagoKimbra's avatar
MagoKimbra committed
1058
          double sin_theta_d2 = sqrt(0.5 * (1.0 - cos_theta)); // Trig half angle identity. Always positive.
MagoKimbra's avatar
MagoKimbra committed
1059
          vmax_junction = min(vmax_junction,
MagoKimbra's avatar
MagoKimbra committed
1060
                              sqrt(block->acceleration * junction_deviation * sin_theta_d2 / (1.0 - sin_theta_d2)));
MagoKimbra's avatar
MagoKimbra committed
1061 1062 1063 1064 1065 1066 1067
        }
      }
    }
  #endif

  // Start with a safe speed
  float vmax_junction = max_xy_jerk / 2;
MagoKimbra's avatar
MagoKimbra committed
1068
  float vmax_junction_factor = 1.0;
1069
  float mz2 = max_z_jerk / 2, me2 = max_e_jerk[extruder] / 2;
MagoKimbra's avatar
MagoKimbra committed
1070 1071 1072 1073 1074 1075 1076
  float csz = current_speed[Z_AXIS], cse = current_speed[E_AXIS];
  if (fabs(csz) > mz2) vmax_junction = min(vmax_junction, mz2);
  if (fabs(cse) > me2) vmax_junction = min(vmax_junction, me2);
  vmax_junction = min(vmax_junction, block->nominal_speed);
  float safe_speed = vmax_junction;

  if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
MagoKimbra's avatar
MagoKimbra committed
1077 1078 1079 1080 1081
    float dsx = current_speed[X_AXIS] - previous_speed[X_AXIS],
          dsy = current_speed[Y_AXIS] - previous_speed[Y_AXIS],
          dsz = fabs(csz - previous_speed[Z_AXIS]),
          dse = fabs(cse - previous_speed[E_AXIS]),
          jerk = sqrt(dsx * dsx + dsy * dsy);
MagoKimbra's avatar
MagoKimbra committed
1082 1083 1084 1085 1086

    //    if ((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
    vmax_junction = block->nominal_speed;
    //    }
    if (jerk > max_xy_jerk) vmax_junction_factor = max_xy_jerk / jerk;
MagoKimbra's avatar
MagoKimbra committed
1087 1088
    if (dsz > max_z_jerk) vmax_junction_factor = min(vmax_junction_factor, max_z_jerk / dsz);
    if (dse > max_e_jerk[extruder]) vmax_junction_factor = min(vmax_junction_factor, max_e_jerk[extruder] / dse);
MagoKimbra's avatar
MagoKimbra committed
1089 1090 1091 1092 1093 1094 1095 1096 1097 1098 1099 1100 1101 1102 1103 1104 1105

    vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
  }
  block->max_entry_speed = vmax_junction;

  // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
  double v_allowable = max_allowable_speed(-block->acceleration, MINIMUM_PLANNER_SPEED, block->millimeters);
  block->entry_speed = min(vmax_junction, v_allowable);

  // Initialize planner efficiency flags
  // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
  // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
  // the current block and next block junction speeds are guaranteed to always be at their maximum
  // junction speeds in deceleration and acceleration, respectively. This is due to how the current
  // block nominal speed limits both the current and next maximum junction speeds. Hence, in both
  // the reverse and forward planners, the corresponding block junction speed will always be at the
  // the maximum junction speed and may always be ignored for any speed reduction checks.
MagoKimbra's avatar
MagoKimbra committed
1106
  block->nominal_length_flag = (block->nominal_speed <= v_allowable);
MagoKimbra's avatar
MagoKimbra committed
1107 1108 1109
  block->recalculate_flag = true; // Always calculate trapezoid for new block

  // Update previous path unit_vector and nominal speed
MagoKimbra's avatar
MagoKimbra committed
1110
  memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[]
MagoKimbra's avatar
MagoKimbra committed
1111 1112
  previous_nominal_speed = block->nominal_speed;

MagoKimbra's avatar
MagoKimbra committed
1113
  #if ENABLED(ADVANCE)
MagoKimbra's avatar
MagoKimbra committed
1114 1115 1116 1117 1118 1119 1120
    // Calculate advance rate
    if (!bse || (!bsx && !bsy && !bsz)) {
      block->advance_rate = 0;
      block->advance = 0;
    }
    else {
      long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);
MagoKimbra's avatar
MagoKimbra committed
1121
      float advance = ((STEPS_PER_CUBIC_MM_E) * (EXTRUDER_ADVANCE_K)) * (cse * cse * (EXTRUSION_AREA) * (EXTRUSION_AREA)) * 256;
MagoKimbra's avatar
MagoKimbra committed
1122 1123 1124 1125
      block->advance = advance;
      block->advance_rate = acc_dist ? advance / (float)acc_dist : 0;
    }
    /*
MagoKimbra's avatar
MagoKimbra committed
1126 1127
    ECHO_SMV(OK, "advance :", block->advance/256);
    ECHO_EMV("advance rate :", block->advance_rate/256);
MagoKimbra's avatar
MagoKimbra committed
1128
    */
MagoKimbra's avatar
MagoKimbra committed
1129 1130 1131 1132 1133 1134 1135 1136 1137 1138 1139 1140 1141 1142
  #elif ENABLED(ADVANCE_LPC) // ADVANCE_LPC
    // bse == allsteps: A problem occurs when there's a very tiny move before a retract.
    // In this case, the retract and the move will be executed together.
    // This leads to an enormous number of advance steps due to a huge e_acceleration.
    // The math is correct, but you don't want a retract move done with advance!
    // So this situation is filtered out here.
    if (!bse || (!bsx && !bsy && !bsz) || extruder_advance_k == 0 || bse == allsteps) {
      block->use_advance_lead = false;
    }
    else {
      block->use_advance_lead = true;
      block->e_speed_multiplier8 = (block->steps[E_AXIS] << 8) / block->step_event_count;
    }
  #endif
MagoKimbra's avatar
MagoKimbra committed
1143

MagoKimbra's avatar
MagoKimbra committed
1144 1145
  calculate_trapezoid_for_block(block, block->entry_speed / block->nominal_speed, safe_speed / block->nominal_speed);

MagoKimbra's avatar
MagoKimbra committed
1146 1147 1148 1149
  // Move buffer head
  block_buffer_head = next_buffer_head;

  // Update position
MagoKimbra's avatar
MagoKimbra committed
1150
  memcpy(position, target, sizeof(target)); // position[] = target[]
MagoKimbra's avatar
MagoKimbra committed
1151 1152 1153 1154 1155 1156 1157

  planner_recalculate();

  st_wake_up();

} // plan_buffer_line()

MagoKimbra's avatar
MagoKimbra committed
1158
#if ENABLED(AUTO_BED_LEVELING_FEATURE)
MagoKimbra's avatar
MagoKimbra committed
1159 1160 1161 1162 1163
  /**
   * Get the XYZ position of the steppers as a vector_3.
   *
   * On CORE machines XYZ is derived from ABC.
   */
MagoKimbra's avatar
MagoKimbra committed
1164
  vector_3 plan_get_position() {
MagoKimbra's avatar
MagoKimbra committed
1165
    vector_3 position = vector_3(st_get_axis_position_mm(X_AXIS), st_get_axis_position_mm(Y_AXIS), st_get_axis_position_mm(Z_AXIS));
MagoKimbra's avatar
MagoKimbra committed
1166 1167

    //position.debug("in plan_get position");
MagoKimbra's avatar
MagoKimbra committed
1168
    //plan_bed_level_matrix.debug("in plan_get_position");
MagoKimbra's avatar
MagoKimbra committed
1169 1170 1171 1172 1173 1174 1175
    matrix_3x3 inverse = matrix_3x3::transpose(plan_bed_level_matrix);
    //inverse.debug("in plan_get inverse");
    position.apply_rotation(inverse);
    //position.debug("after rotation");

    return position;
  }
MagoKimbra's avatar
MagoKimbra committed
1176
#endif // AUTO_BED_LEVELING_FEATURE
MagoKimbra's avatar
MagoKimbra committed
1177

MagoKimbra's avatar
MagoKimbra committed
1178 1179 1180 1181 1182
/**
 * Directly set the planner XYZ position (hence the stepper positions).
 *
 * On CORE machines stepper ABC will be translated from the given XYZ.
 */
MagoKimbra's avatar
MagoKimbra committed
1183
#if ENABLED(AUTO_BED_LEVELING_FEATURE)
MagoKimbra's avatar
MagoKimbra committed
1184
  void plan_set_position(float x, float y, float z, const float& e)
MagoKimbra's avatar
MagoKimbra committed
1185
#else
MagoKimbra's avatar
MagoKimbra committed
1186
  void plan_set_position(const float& x, const float& y, const float& z, const float& e)
MagoKimbra's avatar
MagoKimbra committed
1187
#endif // AUTO_BED_LEVELING_FEATURE
MagoKimbra's avatar
MagoKimbra committed
1188 1189 1190 1191
{
  #if ENABLED(AUTO_BED_LEVELING_FEATURE)
    apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
  #endif
MagoKimbra's avatar
MagoKimbra committed
1192

MagoKimbra's avatar
MagoKimbra committed
1193 1194 1195 1196 1197 1198 1199
  long  nx = position[X_AXIS] = lround(x * axis_steps_per_unit[X_AXIS]),
        ny = position[Y_AXIS] = lround(y * axis_steps_per_unit[Y_AXIS]),
        nz = position[Z_AXIS] = lround(z * axis_steps_per_unit[Z_AXIS]),
        ne = position[E_AXIS] = lround(e * axis_steps_per_unit[E_AXIS + active_extruder]);
  last_extruder = active_extruder;
  st_set_position(nx, ny, nz, ne);
  previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
MagoKimbra's avatar
MagoKimbra committed
1200

MagoKimbra's avatar
MagoKimbra committed
1201 1202
  for (uint8_t i = 0; i < NUM_AXIS; i++) previous_speed[i] = 0.0;
}
MagoKimbra's avatar
MagoKimbra committed
1203

MagoKimbra's avatar
MagoKimbra committed
1204
void plan_set_e_position(const float& e) {
MagoKimbra's avatar
MagoKimbra committed
1205
  position[E_AXIS] = lround(e * axis_steps_per_unit[E_AXIS + active_extruder]);
MagoKimbra's avatar
MagoKimbra committed
1206
  last_extruder = active_extruder;
MagoKimbra's avatar
MagoKimbra committed
1207 1208 1209 1210 1211
  st_set_e_position(position[E_AXIS]);
}

// Calculate the steps/s^2 acceleration rates, based on the mm/s^s
void reset_acceleration_rates() {
MagoKimbra's avatar
MagoKimbra committed
1212
  for (uint8_t i = 0; i < 3 + EXTRUDERS; i++)
MagoKimbra's avatar
MagoKimbra committed
1213 1214
    axis_steps_per_sqr_second[i] = max_acceleration_units_per_sq_second[i] * axis_steps_per_unit[i];
}