Marlin/planner.cpp

   1 /*
   2   planner.c - buffers movement commands and manages the acceleration profile plan
   3  Part of Grbl
   4
   5  Copyright (c) 2009-2011 Simen Svale Skogsrud
   6
   7  Grbl is free software: you can redistribute it and/or modify
   8  it under the terms of the GNU General Public License as published by
   9  the Free Software Foundation, either version 3 of the License, or
  10  (at your option) any later version.
  11
  12  Grbl is distributed in the hope that it will be useful,
  13  but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  GNU General Public License for more details.
  16
  17  You should have received a copy of the GNU General Public License
  18  along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 /* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */
  22
  23 /*
  24  Reasoning behind the mathematics in this module (in the key of 'Mathematica'):
  25
  26  s == speed, a == acceleration, t == time, d == distance
  27
  28  Basic definitions:
  29
  30  Speed[s_, a_, t_] := s + (a*t)
  31  Travel[s_, a_, t_] := Integrate[Speed[s, a, t], t]
  32
  33  Distance to reach a specific speed with a constant acceleration:
  34
  35  Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, d, t]
  36  d -> (m^2 - s^2)/(2 a) --> estimate_acceleration_distance()
  37
  38  Speed after a given distance of travel with constant acceleration:
  39
  40  Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, m, t]
  41  m -> Sqrt[2 a d + s^2]
  42
  43  DestinationSpeed[s_, a_, d_] := Sqrt[2 a d + s^2]
  44
  45  When to start braking (di) to reach a specified destionation speed (s2) after accelerating
  46  from initial speed s1 without ever stopping at a plateau:
  47
  48  Solve[{DestinationSpeed[s1, a, di] == DestinationSpeed[s2, a, d - di]}, di]
  49  di -> (2 a d - s1^2 + s2^2)/(4 a) --> intersection_distance()
  50
  51  IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a)
  52  */
  53
  54 #include "Marlin.h"
  55 #include "planner.h"
  56 #include "stepper.h"
  57 #include "temperature.h"
  58 #include "ultralcd.h"
  59 #include "language.h"
  60
  61 //===========================================================================
  62 //=============================public variables ============================
  63 //===========================================================================
  64
  65 unsigned long minsegmenttime;
  66 float max_feedrate[4]; // set the max speeds
  67 float axis_steps_per_unit[4];
  68 unsigned long max_acceleration_units_per_sq_second[4]; // Use M201 to override by software
  69 float minimumfeedrate;
  70 float acceleration;         // Normal acceleration mm/s^2  THIS IS THE DEFAULT ACCELERATION for all moves. M204 SXXXX
  71 float retract_acceleration; //  mm/s^2   filament pull-pack and push-forward  while standing still in the other axis M204 TXXXX
  72 float max_xy_jerk; //speed than can be stopped at once, if i understand correctly.
  73 float max_z_jerk;
  74 float max_e_jerk;
  75 float mintravelfeedrate;
  76 unsigned long axis_steps_per_sqr_second[NUM_AXIS];
  77
  78 // The current position of the tool in absolute steps
  79 long position[4];   //rescaled from extern when axis_steps_per_unit are changed by gcode
  80 static float previous_speed[4]; // Speed of previous path line segment
  81 static float previous_nominal_speed; // Nominal speed of previous path line segment
  82
  83 extern volatile int extrudemultiply; // Sets extrude multiply factor (in percent)
  84
  85 #ifdef AUTOTEMP
  86 float autotemp_max=250;
  87 float autotemp_min=210;
  88 float autotemp_factor=0.1;
  89 bool autotemp_enabled=false;
  90 #endif
  91
  92 //===========================================================================
  93 //=================semi-private variables, used in inline  functions    =====
  94 //===========================================================================
  95 block_t block_buffer[BLOCK_BUFFER_SIZE];            // A ring buffer for motion instfructions
  96 volatile unsigned char block_buffer_head;           // Index of the next block to be pushed
  97 volatile unsigned char block_buffer_tail;           // Index of the block to process now
  98
  99 //===========================================================================
 100 //=============================private variables ============================
 101 //===========================================================================
 102 #ifdef PREVENT_DANGEROUS_EXTRUDE
 103 bool allow_cold_extrude=false;
 104 #endif
 105 #ifdef XY_FREQUENCY_LIMIT
 106 // Used for the frequency limit
 107 static unsigned char old_direction_bits = 0;               // Old direction bits. Used for speed calculations
 108 static long x_segment_time[3]={
 109   0,0,0};                     // Segment times (in us). Used for speed calculations
 110 static long y_segment_time[3]={
 111   0,0,0};
 112 #endif
 113
 114 // Returns the index of the next block in the ring buffer
 115 // NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication.
 116 static int8_t next_block_index(int8_t block_index) {
 117   block_index++;
 118   if (block_index == BLOCK_BUFFER_SIZE) {
 119     block_index = 0;
 120   }
 121   return(block_index);
 122 }
 123
 124
 125 // Returns the index of the previous block in the ring buffer
 126 static int8_t prev_block_index(int8_t block_index) {
 127   if (block_index == 0) {
 128     block_index = BLOCK_BUFFER_SIZE;
 129   }
 130   block_index--;
 131   return(block_index);
 132 }
 133
 134 //===========================================================================
 135 //=============================functions         ============================
 136 //===========================================================================
 137
 138 // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
 139 // given acceleration:
 140 FORCE_INLINE float estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration)
 141 {
 142   if (acceleration!=0) {
 143     return((target_rate*target_rate-initial_rate*initial_rate)/
 144       (2.0*acceleration));
 145   }
 146   else {
 147     return 0.0;  // acceleration was 0, set acceleration distance to 0
 148   }
 149 }
 150
 151 // This function gives you the point at which you must start braking (at the rate of -acceleration) if
 152 // you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
 153 // a total travel of distance. This can be used to compute the intersection point between acceleration and
 154 // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
 155
 156 FORCE_INLINE float intersection_distance(float initial_rate, float final_rate, float acceleration, float distance)
 157 {
 158   if (acceleration!=0) {
 159     return((2.0*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/
 160       (4.0*acceleration) );
 161   }
 162   else {
 163     return 0.0;  // acceleration was 0, set intersection distance to 0
 164   }
 165 }
 166
 167 // Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
 168
 169 void calculate_trapezoid_for_block(block_t *block, float entry_factor, float exit_factor) {
 170   unsigned long initial_rate = ceil(block->nominal_rate*entry_factor); // (step/min)
 171   unsigned long final_rate = ceil(block->nominal_rate*exit_factor); // (step/min)
 172
 173   // Limit minimal step rate (Otherwise the timer will overflow.)
 174   if(initial_rate <120) {
 175     initial_rate=120;
 176   }
 177   if(final_rate < 120) {
 178     final_rate=120;
 179   }
 180
 181   long acceleration = block->acceleration_st;
 182   int32_t accelerate_steps =
 183     ceil(estimate_acceleration_distance(block->initial_rate, block->nominal_rate, acceleration));
 184   int32_t decelerate_steps =
 185     floor(estimate_acceleration_distance(block->nominal_rate, block->final_rate, -acceleration));
 186
 187   // Calculate the size of Plateau of Nominal Rate.
 188   int32_t plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps;
 189
 190   // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
 191   // have to use intersection_distance() to calculate when to abort acceleration and start braking
 192   // in order to reach the final_rate exactly at the end of this block.
 193   if (plateau_steps < 0) {
 194     accelerate_steps = ceil(
 195     intersection_distance(block->initial_rate, block->final_rate, acceleration, block->step_event_count));
 196     accelerate_steps = max(accelerate_steps,0); // Check limits due to numerical round-off
 197     accelerate_steps = min(accelerate_steps,block->step_event_count);
 198     plateau_steps = 0;
 199   }
 200
 201 #ifdef ADVANCE
 202   volatile long initial_advance = block->advance*entry_factor*entry_factor;
 203   volatile long final_advance = block->advance*exit_factor*exit_factor;
 204 #endif // ADVANCE
 205
 206   // block->accelerate_until = accelerate_steps;
 207   // block->decelerate_after = accelerate_steps+plateau_steps;
 208   CRITICAL_SECTION_START;  // Fill variables used by the stepper in a critical section
 209   if(block->busy == false) { // Don't update variables if block is busy.
 210     block->accelerate_until = accelerate_steps;
 211     block->decelerate_after = accelerate_steps+plateau_steps;
 212     block->initial_rate = initial_rate;
 213     block->final_rate = final_rate;
 214 #ifdef ADVANCE
 215     block->initial_advance = initial_advance;
 216     block->final_advance = final_advance;
 217 #endif //ADVANCE
 218   }
 219   CRITICAL_SECTION_END;
 220 }
 221
 222 // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
 223 // acceleration within the allotted distance.
 224 FORCE_INLINE float max_allowable_speed(float acceleration, float target_velocity, float distance) {
 225   return  sqrt(target_velocity*target_velocity-2*acceleration*distance);
 226 }
 227
 228 // "Junction jerk" in this context is the immediate change in speed at the junction of two blocks.
 229 // This method will calculate the junction jerk as the euclidean distance between the nominal
 230 // velocities of the respective blocks.
 231 //inline float junction_jerk(block_t *before, block_t *after) {
 232 //  return sqrt(
 233 //    pow((before->speed_x-after->speed_x), 2)+pow((before->speed_y-after->speed_y), 2));
 234 //}
 235
 236
 237 // The kernel called by planner_recalculate() when scanning the plan from last to first entry.
 238 void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) {
 239   if(!current) {
 240     return;
 241   }
 242
 243   if (next) {
 244     // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
 245     // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
 246     // check for maximum allowable speed reductions to ensure maximum possible planned speed.
 247     if (current->entry_speed != current->max_entry_speed) {
 248
 249       // If nominal length true, max junction speed is guaranteed to be reached. Only compute
 250       // for max allowable speed if block is decelerating and nominal length is false.
 251       if ((!current->nominal_length_flag) && (current->max_entry_speed > next->entry_speed)) {
 252         current->entry_speed = min( current->max_entry_speed,
 253         max_allowable_speed(-current->acceleration,next->entry_speed,current->millimeters));
 254       }
 255       else {
 256         current->entry_speed = current->max_entry_speed;
 257       }
 258       current->recalculate_flag = true;
 259
 260     }
 261   } // Skip last block. Already initialized and set for recalculation.
 262 }
 263
 264 // planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
 265 // implements the reverse pass.
 266 void planner_reverse_pass() {
 267   uint8_t block_index = block_buffer_head;
 268
 269   //Make a local copy of block_buffer_tail, because the interrupt can alter it
 270   CRITICAL_SECTION_START;
 271   unsigned char tail = block_buffer_tail;
 272   CRITICAL_SECTION_END
 273
 274   if(((block_buffer_head-tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1)) > 3) {
 275     block_index = (block_buffer_head - 3) & (BLOCK_BUFFER_SIZE - 1);
 276     block_t *block[3] = {
 277       NULL, NULL, NULL         };
 278     while(block_index != tail) {
 279       block_index = prev_block_index(block_index);
 280       block[2]= block[1];
 281       block[1]= block[0];
 282       block[0] = &block_buffer[block_index];
 283       planner_reverse_pass_kernel(block[0], block[1], block[2]);
 284     }
 285   }
 286 }
 287
 288 // The kernel called by planner_recalculate() when scanning the plan from first to last entry.
 289 void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) {
 290   if(!previous) {
 291     return;
 292   }
 293
 294   // If the previous block is an acceleration block, but it is not long enough to complete the
 295   // full speed change within the block, we need to adjust the entry speed accordingly. Entry
 296   // speeds have already been reset, maximized, and reverse planned by reverse planner.
 297   // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
 298   if (!previous->nominal_length_flag) {
 299     if (previous->entry_speed < current->entry_speed) {
 300       double entry_speed = min( current->entry_speed,
 301       max_allowable_speed(-previous->acceleration,previous->entry_speed,previous->millimeters) );
 302
 303       // Check for junction speed change
 304       if (current->entry_speed != entry_speed) {
 305         current->entry_speed = entry_speed;
 306         current->recalculate_flag = true;
 307       }
 308     }
 309   }
 310 }
 311
 312 // planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
 313 // implements the forward pass.
 314 void planner_forward_pass() {
 315   uint8_t block_index = block_buffer_tail;
 316   block_t *block[3] = {
 317     NULL, NULL, NULL   };
 318
 319   while(block_index != block_buffer_head) {
 320     block[0] = block[1];
 321     block[1] = block[2];
 322     block[2] = &block_buffer[block_index];
 323     planner_forward_pass_kernel(block[0],block[1],block[2]);
 324     block_index = next_block_index(block_index);
 325   }
 326   planner_forward_pass_kernel(block[1], block[2], NULL);
 327 }
 328
 329 // Recalculates the trapezoid speed profiles for all blocks in the plan according to the
 330 // entry_factor for each junction. Must be called by planner_recalculate() after
 331 // updating the blocks.
 332 void planner_recalculate_trapezoids() {
 333   int8_t block_index = block_buffer_tail;
 334   block_t *current;
 335   block_t *next = NULL;
 336
 337   while(block_index != block_buffer_head) {
 338     current = next;
 339     next = &block_buffer[block_index];
 340     if (current) {
 341       // Recalculate if current block entry or exit junction speed has changed.
 342       if (current->recalculate_flag || next->recalculate_flag) {
 343         // NOTE: Entry and exit factors always > 0 by all previous logic operations.
 344         calculate_trapezoid_for_block(current, current->entry_speed/current->nominal_speed,
 345         next->entry_speed/current->nominal_speed);
 346         current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
 347       }
 348     }
 349     block_index = next_block_index( block_index );
 350   }
 351   // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
 352   if(next != NULL) {
 353     calculate_trapezoid_for_block(next, next->entry_speed/next->nominal_speed,
 354     MINIMUM_PLANNER_SPEED/next->nominal_speed);
 355     next->recalculate_flag = false;
 356   }
 357 }
 358
 359 // Recalculates the motion plan according to the following algorithm:
 360 //
 361 //   1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_factor)
 362 //      so that:
 363 //     a. The junction jerk is within the set limit
 364 //     b. No speed reduction within one block requires faster deceleration than the one, true constant
 365 //        acceleration.
 366 //   2. Go over every block in chronological order and dial down junction speed reduction values if
 367 //     a. The speed increase within one block would require faster accelleration than the one, true
 368 //        constant acceleration.
 369 //
 370 // When these stages are complete all blocks have an entry_factor that will allow all speed changes to
 371 // be performed using only the one, true constant acceleration, and where no junction jerk is jerkier than
 372 // the set limit. Finally it will:
 373 //
 374 //   3. Recalculate trapezoids for all blocks.
 375
 376 void planner_recalculate() {
 377   planner_reverse_pass();
 378   planner_forward_pass();
 379   planner_recalculate_trapezoids();
 380 }
 381
 382 void plan_init() {
 383   block_buffer_head = 0;
 384   block_buffer_tail = 0;
 385   memset(position, 0, sizeof(position)); // clear position
 386   previous_speed[0] = 0.0;
 387   previous_speed[1] = 0.0;
 388   previous_speed[2] = 0.0;
 389   previous_speed[3] = 0.0;
 390   previous_nominal_speed = 0.0;
 391 }
 392
 393
 394
 395
 396 #ifdef AUTOTEMP
 397 void getHighESpeed()
 398 {
 399   static float oldt=0;
 400   if(!autotemp_enabled){
 401     return;
 402   }
 403   if(degTargetHotend0()+2<autotemp_min) {  //probably temperature set to zero.
 404     return; //do nothing
 405   }
 406
 407   float high=0.0;
 408   uint8_t block_index = block_buffer_tail;
 409
 410   while(block_index != block_buffer_head) {
 411     if((block_buffer[block_index].steps_x != 0) ||
 412       (block_buffer[block_index].steps_y != 0) ||
 413       (block_buffer[block_index].steps_z != 0)) {
 414       float se=(float(block_buffer[block_index].steps_e)/float(block_buffer[block_index].step_event_count))*block_buffer[block_index].nominal_speed;
 415       //se; mm/sec;
 416       if(se>high)
 417       {
 418         high=se;
 419       }
 420     }
 421     block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1);
 422   }
 423
 424   float g=autotemp_min+high*autotemp_factor;
 425   float t=g;
 426   if(t<autotemp_min)
 427     t=autotemp_min;
 428   if(t>autotemp_max)
 429     t=autotemp_max;
 430   if(oldt>t)
 431   {
 432     t=AUTOTEMP_OLDWEIGHT*oldt+(1-AUTOTEMP_OLDWEIGHT)*t;
 433   }
 434   oldt=t;
 435   setTargetHotend0(t);
 436 }
 437 #endif
 438
 439 void check_axes_activity() {
 440   unsigned char x_active = 0;
 441   unsigned char y_active = 0;
 442   unsigned char z_active = 0;
 443   unsigned char e_active = 0;
 444   unsigned char fan_speed = 0;
 445   unsigned char tail_fan_speed = 0;
 446   block_t *block;
 447
 448   if(block_buffer_tail != block_buffer_head) {
 449     uint8_t block_index = block_buffer_tail;
 450     tail_fan_speed = block_buffer[block_index].fan_speed;
 451     while(block_index != block_buffer_head) {
 452       block = &block_buffer[block_index];
 453       if(block->steps_x != 0) x_active++;
 454       if(block->steps_y != 0) y_active++;
 455       if(block->steps_z != 0) z_active++;
 456       if(block->steps_e != 0) e_active++;
 457       if(block->fan_speed != 0) fan_speed++;
 458       block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1);
 459     }
 460   }
 461   else {
 462 #if FAN_PIN > -1
 463     if (FanSpeed != 0){
 464       analogWrite(FAN_PIN,FanSpeed); // If buffer is empty use current fan speed
 465     }
 466 #endif
 467   }
 468   if((DISABLE_X) && (x_active == 0)) disable_x();
 469   if((DISABLE_Y) && (y_active == 0)) disable_y();
 470   if((DISABLE_Z) && (z_active == 0)) disable_z();
 471   if((DISABLE_E) && (e_active == 0)) {
 472     disable_e0();
 473     disable_e1();
 474     disable_e2();
 475   }
 476 #if FAN_PIN > -1
 477   if((FanSpeed == 0) && (fan_speed ==0)) {
 478     analogWrite(FAN_PIN, 0);
 479   }
 480
 481   if (FanSpeed != 0 && tail_fan_speed !=0) {
 482     analogWrite(FAN_PIN,tail_fan_speed);
 483   }
 484 #endif
 485 #ifdef AUTOTEMP
 486   getHighESpeed();
 487 #endif
 488 }
 489
 490
 491 float junction_deviation = 0.1;
 492 // Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in
 493 // mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
 494 // calculation the caller must also provide the physical length of the line in millimeters.
 495 void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder)
 496 {
 497   // Calculate the buffer head after we push this byte
 498   int next_buffer_head = next_block_index(block_buffer_head);
 499
 500   // If the buffer is full: good! That means we are well ahead of the robot.
 501   // Rest here until there is room in the buffer.
 502   while(block_buffer_tail == next_buffer_head) {
 503     manage_heater();
 504     manage_inactivity(1);
 505     LCD_STATUS;
 506   }
 507
 508   // The target position of the tool in absolute steps
 509   // Calculate target position in absolute steps
 510   //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
 511   long target[4];
 512   target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
 513   target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
 514   target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
 515   target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);
 516
 517 #ifdef PREVENT_DANGEROUS_EXTRUDE
 518   if(target[E_AXIS]!=position[E_AXIS])
 519     if(degHotend(active_extruder)<EXTRUDE_MINTEMP && !allow_cold_extrude)
 520     {
 521       position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
 522       SERIAL_ECHO_START;
 523       SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
 524     }
 525 #ifdef PREVENT_LENGTHY_EXTRUDE
 526   if(labs(target[E_AXIS]-position[E_AXIS])>axis_steps_per_unit[E_AXIS]*EXTRUDE_MAXLENGTH)
 527   {
 528     position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
 529     SERIAL_ECHO_START;
 530     SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
 531   }
 532 #endif
 533 #endif
 534
 535   // Prepare to set up new block
 536   block_t *block = &block_buffer[block_buffer_head];
 537
 538   // Mark block as not busy (Not executed by the stepper interrupt)
 539   block->busy = false;
 540
 541   // Number of steps for each axis
 542   block->steps_x = labs(target[X_AXIS]-position[X_AXIS]);
 543   block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]);
 544   block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]);
 545   block->steps_e = labs(target[E_AXIS]-position[E_AXIS]);
 546   block->steps_e *= extrudemultiply;
 547   block->steps_e /= 100;
 548   block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e)));
 549
 550   // Bail if this is a zero-length block
 551   if (block->step_event_count <= dropsegments) {
 552     return;
 553   };
 554
 555   block->fan_speed = FanSpeed;
 556
 557   // Compute direction bits for this block
 558   block->direction_bits = 0;
 559   if (target[X_AXIS] < position[X_AXIS]) {
 560     block->direction_bits |= (1<<X_AXIS);
 561   }
 562   if (target[Y_AXIS] < position[Y_AXIS]) {
 563     block->direction_bits |= (1<<Y_AXIS);
 564   }
 565   if (target[Z_AXIS] < position[Z_AXIS]) {
 566     block->direction_bits |= (1<<Z_AXIS);
 567   }
 568   if (target[E_AXIS] < position[E_AXIS]) {
 569     block->direction_bits |= (1<<E_AXIS);
 570   }
 571
 572   block->active_extruder = extruder;
 573
 574   //enable active axes
 575   if(block->steps_x != 0) enable_x();
 576   if(block->steps_y != 0) enable_y();
 577 #ifndef Z_LATE_ENABLE
 578   if(block->steps_z != 0) enable_z();
 579 #endif
 580
 581   // Enable all
 582   if(block->steps_e != 0) {
 583     enable_e0();
 584     enable_e1();
 585     enable_e2();
 586   }
 587
 588   if (block->steps_e == 0) {
 589     if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate;
 590   }
 591   else {
 592     if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate;
 593   }
 594
 595   float delta_mm[4];
 596   delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
 597   delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
 598   delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS];
 599   delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS])*extrudemultiply/100.0;
 600   if ( block->steps_x <=dropsegments && block->steps_y <=dropsegments && block->steps_z <=dropsegments ) {
 601     block->millimeters = fabs(delta_mm[E_AXIS]);
 602   }
 603   else {
 604     block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]));
 605   }
 606   float inverse_millimeters = 1.0/block->millimeters;  // Inverse millimeters to remove multiple divides
 607
 608     // Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
 609   float inverse_second = feed_rate * inverse_millimeters;
 610
 611   int moves_queued=(block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1);
 612
 613   // slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill
 614 #ifdef OLD_SLOWDOWN
 615   if(moves_queued < (BLOCK_BUFFER_SIZE * 0.5) && moves_queued > 1) feed_rate = feed_rate*moves_queued / (BLOCK_BUFFER_SIZE * 0.5);
 616 #endif
 617
 618 #ifdef SLOWDOWN
 619   //  segment time im micro seconds
 620   unsigned long segment_time = lround(1000000.0/inverse_second);
 621   if ((moves_queued > 1) && (moves_queued < (BLOCK_BUFFER_SIZE * 0.5))) {
 622     if (segment_time < minsegmenttime)  { // buffer is draining, add extra time.  The amount of time added increases if the buffer is still emptied more.
 623       inverse_second=1000000.0/(segment_time+lround(2*(minsegmenttime-segment_time)/moves_queued));
 624     }
 625   }
 626 #endif
 627   //  END OF SLOW DOWN SECTION
 628
 629
 630   block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0
 631   block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0
 632
 633   // Calculate and limit speed in mm/sec for each axis
 634   float current_speed[4];
 635   float speed_factor = 1.0; //factor <=1 do decrease speed
 636   for(int i=0; i < 4; i++) {
 637     current_speed[i] = delta_mm[i] * inverse_second;
 638     if(fabs(current_speed[i]) > max_feedrate[i])
 639       speed_factor = min(speed_factor, max_feedrate[i] / fabs(current_speed[i]));
 640   }
 641
 642   // Max segement time in us.
 643 #ifdef XY_FREQUENCY_LIMIT
 644 #define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT)
 645
 646   // Check and limit the xy direction change frequency
 647   unsigned char direction_change = block->direction_bits ^ old_direction_bits;
 648   old_direction_bits = block->direction_bits;
 649
 650   if((direction_change & (1<<X_AXIS)) == 0) {
 651     x_segment_time[0] += segment_time;
 652   }
 653   else {
 654     x_segment_time[2] = x_segment_time[1];
 655     x_segment_time[1] = x_segment_time[0];
 656     x_segment_time[0] = segment_time;
 657   }
 658   if((direction_change & (1<<Y_AXIS)) == 0) {
 659     y_segment_time[0] += segment_time;
 660   }
 661   else {
 662     y_segment_time[2] = y_segment_time[1];
 663     y_segment_time[1] = y_segment_time[0];
 664     y_segment_time[0] = segment_time;
 665   }
 666   long max_x_segment_time = max(x_segment_time[0], max(x_segment_time[1], x_segment_time[2]));
 667   long max_y_segment_time = max(y_segment_time[0], max(y_segment_time[1], y_segment_time[2]));
 668   long min_xy_segment_time =min(max_x_segment_time, max_y_segment_time);
 669   if(min_xy_segment_time < MAX_FREQ_TIME) speed_factor = min(speed_factor, speed_factor * (float)min_xy_segment_time / (float)MAX_FREQ_TIME);
 670 #endif
 671
 672   // Correct the speed
 673   if( speed_factor < 1.0) {
 674     for(unsigned char i=0; i < 4; i++) {
 675       current_speed[i] *= speed_factor;
 676     }
 677     block->nominal_speed *= speed_factor;
 678     block->nominal_rate *= speed_factor;
 679   }
 680
 681   // Compute and limit the acceleration rate for the trapezoid generator.
 682   float steps_per_mm = block->step_event_count/block->millimeters;
 683   if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0) {
 684     block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
 685   }
 686   else {
 687     block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
 688     // Limit acceleration per axis
 689     if(((float)block->acceleration_st * (float)block->steps_x / (float)block->step_event_count) > axis_steps_per_sqr_second[X_AXIS])
 690       block->acceleration_st = axis_steps_per_sqr_second[X_AXIS];
 691     if(((float)block->acceleration_st * (float)block->steps_y / (float)block->step_event_count) > axis_steps_per_sqr_second[Y_AXIS])
 692       block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS];
 693     if(((float)block->acceleration_st * (float)block->steps_e / (float)block->step_event_count) > axis_steps_per_sqr_second[E_AXIS])
 694       block->acceleration_st = axis_steps_per_sqr_second[E_AXIS];
 695     if(((float)block->acceleration_st * (float)block->steps_z / (float)block->step_event_count ) > axis_steps_per_sqr_second[Z_AXIS])
 696       block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS];
 697   }
 698   block->acceleration = block->acceleration_st / steps_per_mm;
 699   block->acceleration_rate = (long)((float)block->acceleration_st * 8.388608);
 700
 701 #if 0  // Use old jerk for now
 702   // Compute path unit vector
 703   double unit_vec[3];
 704
 705   unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
 706   unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
 707   unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;
 708
 709   // Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
 710   // Let a circle be tangent to both previous and current path line segments, where the junction
 711   // deviation is defined as the distance from the junction to the closest edge of the circle,
 712   // colinear with the circle center. The circular segment joining the two paths represents the
 713   // path of centripetal acceleration. Solve for max velocity based on max acceleration about the
 714   // radius of the circle, defined indirectly by junction deviation. This may be also viewed as
 715   // path width or max_jerk in the previous grbl version. This approach does not actually deviate
 716   // from path, but used as a robust way to compute cornering speeds, as it takes into account the
 717   // nonlinearities of both the junction angle and junction velocity.
 718   double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
 719
 720   // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
 721   if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
 722     // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
 723     // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
 724     double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
 725       - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
 726       - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
 727
 728     // Skip and use default max junction speed for 0 degree acute junction.
 729     if (cos_theta < 0.95) {
 730       vmax_junction = min(previous_nominal_speed,block->nominal_speed);
 731       // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
 732       if (cos_theta > -0.95) {
 733         // Compute maximum junction velocity based on maximum acceleration and junction deviation
 734         double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
 735         vmax_junction = min(vmax_junction,
 736         sqrt(block->acceleration * junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
 737       }
 738     }
 739   }
 740 #endif
 741   // Start with a safe speed
 742   float vmax_junction = max_xy_jerk/2;
 743   float vmax_junction_factor = 1.0;
 744   if(fabs(current_speed[Z_AXIS]) > max_z_jerk/2)
 745     vmax_junction = min(vmax_junction, max_z_jerk/2);
 746   if(fabs(current_speed[E_AXIS]) > max_e_jerk/2)
 747     vmax_junction = min(vmax_junction, max_e_jerk/2);
 748   vmax_junction = min(vmax_junction, block->nominal_speed);
 749   float safe_speed = vmax_junction;
 750
 751   if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
 752     float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2));
 753     //    if((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
 754     vmax_junction = block->nominal_speed;
 755     //    }
 756     if (jerk > max_xy_jerk) {
 757       vmax_junction_factor = (max_xy_jerk/jerk);
 758     }
 759     if(fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk) {
 760       vmax_junction_factor= min(vmax_junction_factor, (max_z_jerk/fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS])));
 761     }
 762     if(fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]) > max_e_jerk) {
 763       vmax_junction_factor = min(vmax_junction_factor, (max_e_jerk/fabs(current_speed[E_AXIS] - previous_speed[E_AXIS])));
 764     }
 765     vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
 766   }
 767   block->max_entry_speed = vmax_junction;
 768
 769   // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
 770   double v_allowable = max_allowable_speed(-block->acceleration,MINIMUM_PLANNER_SPEED,block->millimeters);
 771   block->entry_speed = min(vmax_junction, v_allowable);
 772
 773   // Initialize planner efficiency flags
 774   // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
 775   // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
 776   // the current block and next block junction speeds are guaranteed to always be at their maximum
 777   // junction speeds in deceleration and acceleration, respectively. This is due to how the current
 778   // block nominal speed limits both the current and next maximum junction speeds. Hence, in both
 779   // the reverse and forward planners, the corresponding block junction speed will always be at the
 780   // the maximum junction speed and may always be ignored for any speed reduction checks.
 781   if (block->nominal_speed <= v_allowable) {
 782     block->nominal_length_flag = true;
 783   }
 784   else {
 785     block->nominal_length_flag = false;
 786   }
 787   block->recalculate_flag = true; // Always calculate trapezoid for new block
 788
 789   // Update previous path unit_vector and nominal speed
 790   memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[]
 791   previous_nominal_speed = block->nominal_speed;
 792
 793
 794 #ifdef ADVANCE
 795   // Calculate advance rate
 796   if((block->steps_e == 0) || (block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)) {
 797     block->advance_rate = 0;
 798     block->advance = 0;
 799   }
 800   else {
 801     long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);
 802     float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) *
 803       (current_speed[E_AXIS] * current_speed[E_AXIS] * EXTRUTION_AREA * EXTRUTION_AREA)*256;
 804     block->advance = advance;
 805     if(acc_dist == 0) {
 806       block->advance_rate = 0;
 807     }
 808     else {
 809       block->advance_rate = advance / (float)acc_dist;
 810     }
 811   }
 812   /*
 813     SERIAL_ECHO_START;
 814    SERIAL_ECHOPGM("advance :");
 815    SERIAL_ECHO(block->advance/256.0);
 816    SERIAL_ECHOPGM("advance rate :");
 817    SERIAL_ECHOLN(block->advance_rate/256.0);
 818    */
 819 #endif // ADVANCE
 820
 821   calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed,
 822   safe_speed/block->nominal_speed);
 823
 824   // Move buffer head
 825   block_buffer_head = next_buffer_head;
 826
 827   // Update position
 828   memcpy(position, target, sizeof(target)); // position[] = target[]
 829
 830   planner_recalculate();
 831
 832   st_wake_up();
 833 }
 834
 835 void plan_set_position(const float &x, const float &y, const float &z, const float &e)
 836 {
 837   position[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
 838   position[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
 839   position[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
 840   position[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);
 841   st_set_position(position[X_AXIS], position[Y_AXIS], position[Z_AXIS], position[E_AXIS]);
 842   previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
 843   previous_speed[0] = 0.0;
 844   previous_speed[1] = 0.0;
 845   previous_speed[2] = 0.0;
 846   previous_speed[3] = 0.0;
 847 }
 848
 849 void plan_set_e_position(const float &e)
 850 {
 851   position[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);
 852   st_set_e_position(position[E_AXIS]);
 853 }
 854
 855 uint8_t movesplanned()
 856 {
 857   return (block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1);
 858 }
 859
 860 void allow_cold_extrudes(bool allow)
 861 {
 862 #ifdef PREVENT_DANGEROUS_EXTRUDE
 863   allow_cold_extrude=allow;
 864 #endif
 865 }
 866