|
@@ -1,7 +1,7 @@
|
1
|
1
|
/**
|
2
|
2
|
* planner.cpp - Buffer movement commands and manage the acceleration profile plan
|
3
|
3
|
* Part of Grbl
|
4
|
|
- *
|
|
4
|
+ *
|
5
|
5
|
* Copyright (c) 2009-2011 Simen Svale Skogsrud
|
6
|
6
|
*
|
7
|
7
|
* Grbl is free software: you can redistribute it and/or modify
|
|
@@ -134,14 +134,14 @@ unsigned char g_uc_extruder_last_move[4] = {0,0,0,0};
|
134
|
134
|
FORCE_INLINE int8_t next_block_index(int8_t block_index) { return BLOCK_MOD(block_index + 1); }
|
135
|
135
|
FORCE_INLINE int8_t prev_block_index(int8_t block_index) { return BLOCK_MOD(block_index - 1); }
|
136
|
136
|
|
137
|
|
-// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
|
|
137
|
+// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
|
138
|
138
|
// given acceleration:
|
139
|
139
|
FORCE_INLINE float estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration) {
|
140
|
140
|
if (acceleration == 0) return 0; // acceleration was 0, set acceleration distance to 0
|
141
|
141
|
return (target_rate * target_rate - initial_rate * initial_rate) / (acceleration * 2);
|
142
|
142
|
}
|
143
|
143
|
|
144
|
|
-// This function gives you the point at which you must start braking (at the rate of -acceleration) if
|
|
144
|
+// This function gives you the point at which you must start braking (at the rate of -acceleration) if
|
145
|
145
|
// you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
|
146
|
146
|
// a total travel of distance. This can be used to compute the intersection point between acceleration and
|
147
|
147
|
// deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
|
|
@@ -179,7 +179,7 @@ void calculate_trapezoid_for_block(block_t *block, float entry_factor, float exi
|
179
|
179
|
}
|
180
|
180
|
|
181
|
181
|
#if ENABLED(ADVANCE)
|
182
|
|
- volatile long initial_advance = block->advance * entry_factor * entry_factor;
|
|
182
|
+ volatile long initial_advance = block->advance * entry_factor * entry_factor;
|
183
|
183
|
volatile long final_advance = block->advance * exit_factor * exit_factor;
|
184
|
184
|
#endif // ADVANCE
|
185
|
185
|
|
|
@@ -197,16 +197,16 @@ void calculate_trapezoid_for_block(block_t *block, float entry_factor, float exi
|
197
|
197
|
#endif
|
198
|
198
|
}
|
199
|
199
|
CRITICAL_SECTION_END;
|
200
|
|
-}
|
|
200
|
+}
|
201
|
201
|
|
202
|
|
-// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
|
|
202
|
+// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
|
203
|
203
|
// acceleration within the allotted distance.
|
204
|
204
|
FORCE_INLINE float max_allowable_speed(float acceleration, float target_velocity, float distance) {
|
205
|
205
|
return sqrt(target_velocity * target_velocity - 2 * acceleration * distance);
|
206
|
206
|
}
|
207
|
207
|
|
208
|
208
|
// "Junction jerk" in this context is the immediate change in speed at the junction of two blocks.
|
209
|
|
-// This method will calculate the junction jerk as the euclidean distance between the nominal
|
|
209
|
+// This method will calculate the junction jerk as the euclidean distance between the nominal
|
210
|
210
|
// velocities of the respective blocks.
|
211
|
211
|
//inline float junction_jerk(block_t *before, block_t *after) {
|
212
|
212
|
// return sqrt(
|
|
@@ -217,6 +217,7 @@ FORCE_INLINE float max_allowable_speed(float acceleration, float target_velocity
|
217
|
217
|
// The kernel called by planner_recalculate() when scanning the plan from last to first entry.
|
218
|
218
|
void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) {
|
219
|
219
|
if (!current) return;
|
|
220
|
+ UNUSED(previous);
|
220
|
221
|
|
221
|
222
|
if (next) {
|
222
|
223
|
// If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
|
|
@@ -229,7 +230,7 @@ void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *n
|
229
|
230
|
if (!current->nominal_length_flag && current->max_entry_speed > next->entry_speed) {
|
230
|
231
|
current->entry_speed = min(current->max_entry_speed,
|
231
|
232
|
max_allowable_speed(-current->acceleration, next->entry_speed, current->millimeters));
|
232
|
|
- }
|
|
233
|
+ }
|
233
|
234
|
else {
|
234
|
235
|
current->entry_speed = current->max_entry_speed;
|
235
|
236
|
}
|
|
@@ -239,16 +240,16 @@ void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *n
|
239
|
240
|
} // Skip last block. Already initialized and set for recalculation.
|
240
|
241
|
}
|
241
|
242
|
|
242
|
|
-// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
|
|
243
|
+// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
|
243
|
244
|
// implements the reverse pass.
|
244
|
245
|
void planner_reverse_pass() {
|
245
|
246
|
uint8_t block_index = block_buffer_head;
|
246
|
|
-
|
|
247
|
+
|
247
|
248
|
//Make a local copy of block_buffer_tail, because the interrupt can alter it
|
248
|
249
|
CRITICAL_SECTION_START;
|
249
|
250
|
unsigned char tail = block_buffer_tail;
|
250
|
251
|
CRITICAL_SECTION_END
|
251
|
|
-
|
|
252
|
+
|
252
|
253
|
if (BLOCK_MOD(block_buffer_head - tail + BLOCK_BUFFER_SIZE) > 3) { // moves queued
|
253
|
254
|
block_index = BLOCK_MOD(block_buffer_head - 3);
|
254
|
255
|
block_t *block[3] = { NULL, NULL, NULL };
|
|
@@ -265,6 +266,7 @@ void planner_reverse_pass() {
|
265
|
266
|
// The kernel called by planner_recalculate() when scanning the plan from first to last entry.
|
266
|
267
|
void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) {
|
267
|
268
|
if (!previous) return;
|
|
269
|
+ UNUSED(next);
|
268
|
270
|
|
269
|
271
|
// If the previous block is an acceleration block, but it is not long enough to complete the
|
270
|
272
|
// full speed change within the block, we need to adjust the entry speed accordingly. Entry
|
|
@@ -300,8 +302,8 @@ void planner_forward_pass() {
|
300
|
302
|
planner_forward_pass_kernel(block[1], block[2], NULL);
|
301
|
303
|
}
|
302
|
304
|
|
303
|
|
-// Recalculates the trapezoid speed profiles for all blocks in the plan according to the
|
304
|
|
-// entry_factor for each junction. Must be called by planner_recalculate() after
|
|
305
|
+// Recalculates the trapezoid speed profiles for all blocks in the plan according to the
|
|
306
|
+// entry_factor for each junction. Must be called by planner_recalculate() after
|
305
|
307
|
// updating the blocks.
|
306
|
308
|
void planner_recalculate_trapezoids() {
|
307
|
309
|
int8_t block_index = block_buffer_tail;
|
|
@@ -332,22 +334,22 @@ void planner_recalculate_trapezoids() {
|
332
|
334
|
|
333
|
335
|
// Recalculates the motion plan according to the following algorithm:
|
334
|
336
|
//
|
335
|
|
-// 1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_factor)
|
|
337
|
+// 1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_factor)
|
336
|
338
|
// so that:
|
337
|
339
|
// a. The junction jerk is within the set limit
|
338
|
|
-// b. No speed reduction within one block requires faster deceleration than the one, true constant
|
|
340
|
+// b. No speed reduction within one block requires faster deceleration than the one, true constant
|
339
|
341
|
// acceleration.
|
340
|
|
-// 2. Go over every block in chronological order and dial down junction speed reduction values if
|
341
|
|
-// a. The speed increase within one block would require faster acceleration than the one, true
|
|
342
|
+// 2. Go over every block in chronological order and dial down junction speed reduction values if
|
|
343
|
+// a. The speed increase within one block would require faster acceleration than the one, true
|
342
|
344
|
// constant acceleration.
|
343
|
345
|
//
|
344
|
|
-// When these stages are complete all blocks have an entry_factor that will allow all speed changes to
|
345
|
|
-// be performed using only the one, true constant acceleration, and where no junction jerk is jerkier than
|
|
346
|
+// When these stages are complete all blocks have an entry_factor that will allow all speed changes to
|
|
347
|
+// be performed using only the one, true constant acceleration, and where no junction jerk is jerkier than
|
346
|
348
|
// the set limit. Finally it will:
|
347
|
349
|
//
|
348
|
350
|
// 3. Recalculate trapezoids for all blocks.
|
349
|
351
|
|
350
|
|
-void planner_recalculate() {
|
|
352
|
+void planner_recalculate() {
|
351
|
353
|
planner_reverse_pass();
|
352
|
354
|
planner_forward_pass();
|
353
|
355
|
planner_recalculate_trapezoids();
|
|
@@ -356,7 +358,7 @@ void planner_recalculate() {
|
356
|
358
|
void plan_init() {
|
357
|
359
|
block_buffer_head = block_buffer_tail = 0;
|
358
|
360
|
memset(position, 0, sizeof(position)); // clear position
|
359
|
|
- for (int i=0; i<NUM_AXIS; i++) previous_speed[i] = 0.0;
|
|
361
|
+ for (int i=0; i<NUM_AXIS; i++) previous_speed[i] = 0.0;
|
360
|
362
|
previous_nominal_speed = 0.0;
|
361
|
363
|
}
|
362
|
364
|
|
|
@@ -469,7 +471,7 @@ void check_axes_activity() {
|
469
|
471
|
|
470
|
472
|
|
471
|
473
|
float junction_deviation = 0.1;
|
472
|
|
-// Add a new linear movement to the buffer. steps[X_AXIS], _y and _z is the absolute position in
|
|
474
|
+// Add a new linear movement to the buffer. steps[X_AXIS], _y and _z is the absolute position in
|
473
|
475
|
// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
|
474
|
476
|
// calculation the caller must also provide the physical length of the line in millimeters.
|
475
|
477
|
#if ENABLED(ENABLE_AUTO_BED_LEVELING) || ENABLED(MESH_BED_LEVELING)
|
|
@@ -481,7 +483,7 @@ float junction_deviation = 0.1;
|
481
|
483
|
// Calculate the buffer head after we push this byte
|
482
|
484
|
int next_buffer_head = next_block_index(block_buffer_head);
|
483
|
485
|
|
484
|
|
- // If the buffer is full: good! That means we are well ahead of the robot.
|
|
486
|
+ // If the buffer is full: good! That means we are well ahead of the robot.
|
485
|
487
|
// Rest here until there is room in the buffer.
|
486
|
488
|
while (block_buffer_tail == next_buffer_head) idle();
|
487
|
489
|
|
|
@@ -497,7 +499,7 @@ float junction_deviation = 0.1;
|
497
|
499
|
long target[NUM_AXIS];
|
498
|
500
|
target[X_AXIS] = lround(x * axis_steps_per_unit[X_AXIS]);
|
499
|
501
|
target[Y_AXIS] = lround(y * axis_steps_per_unit[Y_AXIS]);
|
500
|
|
- target[Z_AXIS] = lround(z * axis_steps_per_unit[Z_AXIS]);
|
|
502
|
+ target[Z_AXIS] = lround(z * axis_steps_per_unit[Z_AXIS]);
|
501
|
503
|
target[E_AXIS] = lround(e * axis_steps_per_unit[E_AXIS]);
|
502
|
504
|
|
503
|
505
|
float dx = target[X_AXIS] - position[X_AXIS],
|
|
@@ -569,7 +571,7 @@ float junction_deviation = 0.1;
|
569
|
571
|
block->e_to_p_pressure = EtoPPressure;
|
570
|
572
|
#endif
|
571
|
573
|
|
572
|
|
- // Compute direction bits for this block
|
|
574
|
+ // Compute direction bits for this block
|
573
|
575
|
uint8_t db = 0;
|
574
|
576
|
#if ENABLED(COREXY)
|
575
|
577
|
if (dx < 0) db |= BIT(X_HEAD); // Save the real Extruder (head) direction in X Axis
|
|
@@ -585,10 +587,10 @@ float junction_deviation = 0.1;
|
585
|
587
|
if (dx - dz < 0) db |= BIT(C_AXIS); // Motor B direction
|
586
|
588
|
#else
|
587
|
589
|
if (dx < 0) db |= BIT(X_AXIS);
|
588
|
|
- if (dy < 0) db |= BIT(Y_AXIS);
|
|
590
|
+ if (dy < 0) db |= BIT(Y_AXIS);
|
589
|
591
|
if (dz < 0) db |= BIT(Z_AXIS);
|
590
|
592
|
#endif
|
591
|
|
- if (de < 0) db |= BIT(E_AXIS);
|
|
593
|
+ if (de < 0) db |= BIT(E_AXIS);
|
592
|
594
|
block->direction_bits = db;
|
593
|
595
|
|
594
|
596
|
block->active_extruder = extruder;
|
|
@@ -622,7 +624,7 @@ float junction_deviation = 0.1;
|
622
|
624
|
|
623
|
625
|
for (int i=0; i<EXTRUDERS; i++)
|
624
|
626
|
if (g_uc_extruder_last_move[i] > 0) g_uc_extruder_last_move[i]--;
|
625
|
|
-
|
|
627
|
+
|
626
|
628
|
switch(extruder) {
|
627
|
629
|
case 0:
|
628
|
630
|
enable_e0();
|
|
@@ -686,13 +688,13 @@ float junction_deviation = 0.1;
|
686
|
688
|
NOLESS(feed_rate, mintravelfeedrate);
|
687
|
689
|
|
688
|
690
|
/**
|
689
|
|
- * This part of the code calculates the total length of the movement.
|
|
691
|
+ * This part of the code calculates the total length of the movement.
|
690
|
692
|
* For cartesian bots, the X_AXIS is the real X movement and same for Y_AXIS.
|
691
|
693
|
* But for corexy bots, that is not true. The "X_AXIS" and "Y_AXIS" motors (that should be named to A_AXIS
|
692
|
694
|
* and B_AXIS) cannot be used for X and Y length, because A=X+Y and B=X-Y.
|
693
|
|
- * So we need to create other 2 "AXIS", named X_HEAD and Y_HEAD, meaning the real displacement of the Head.
|
|
695
|
+ * So we need to create other 2 "AXIS", named X_HEAD and Y_HEAD, meaning the real displacement of the Head.
|
694
|
696
|
* Having the real displacement of the head, we can calculate the total movement length and apply the desired speed.
|
695
|
|
- */
|
|
697
|
+ */
|
696
|
698
|
#if ENABLED(COREXY)
|
697
|
699
|
float delta_mm[6];
|
698
|
700
|
delta_mm[X_HEAD] = dx / axis_steps_per_unit[A_AXIS];
|
|
@@ -717,7 +719,7 @@ float junction_deviation = 0.1;
|
717
|
719
|
|
718
|
720
|
if (block->steps[X_AXIS] <= dropsegments && block->steps[Y_AXIS] <= dropsegments && block->steps[Z_AXIS] <= dropsegments) {
|
719
|
721
|
block->millimeters = fabs(delta_mm[E_AXIS]);
|
720
|
|
- }
|
|
722
|
+ }
|
721
|
723
|
else {
|
722
|
724
|
block->millimeters = sqrt(
|
723
|
725
|
#if ENABLED(COREXY)
|
|
@@ -729,7 +731,7 @@ float junction_deviation = 0.1;
|
729
|
731
|
#endif
|
730
|
732
|
);
|
731
|
733
|
}
|
732
|
|
- float inverse_millimeters = 1.0 / block->millimeters; // Inverse millimeters to remove multiple divides
|
|
734
|
+ float inverse_millimeters = 1.0 / block->millimeters; // Inverse millimeters to remove multiple divides
|
733
|
735
|
|
734
|
736
|
// Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
|
735
|
737
|
float inverse_second = feed_rate * inverse_millimeters;
|
|
@@ -762,7 +764,7 @@ float junction_deviation = 0.1;
|
762
|
764
|
|
763
|
765
|
#if ENABLED(FILAMENT_SENSOR)
|
764
|
766
|
//FMM update ring buffer used for delay with filament measurements
|
765
|
|
-
|
|
767
|
+
|
766
|
768
|
if (extruder == FILAMENT_SENSOR_EXTRUDER_NUM && delay_index2 > -1) { //only for extruder with filament sensor and if ring buffer is initialized
|
767
|
769
|
|
768
|
770
|
const int MMD = MAX_MEASUREMENT_DELAY + 1, MMD10 = MMD * 10;
|
|
@@ -803,7 +805,7 @@ float junction_deviation = 0.1;
|
803
|
805
|
unsigned char direction_change = block->direction_bits ^ old_direction_bits;
|
804
|
806
|
old_direction_bits = block->direction_bits;
|
805
|
807
|
segment_time = lround((float)segment_time / speed_factor);
|
806
|
|
-
|
|
808
|
+
|
807
|
809
|
long xs0 = axis_segment_time[X_AXIS][0],
|
808
|
810
|
xs1 = axis_segment_time[X_AXIS][1],
|
809
|
811
|
xs2 = axis_segment_time[X_AXIS][2],
|
|
@@ -834,14 +836,14 @@ float junction_deviation = 0.1;
|
834
|
836
|
}
|
835
|
837
|
#endif // XY_FREQUENCY_LIMIT
|
836
|
838
|
|
837
|
|
- // Correct the speed
|
|
839
|
+ // Correct the speed
|
838
|
840
|
if (speed_factor < 1.0) {
|
839
|
841
|
for (unsigned char i = 0; i < NUM_AXIS; i++) current_speed[i] *= speed_factor;
|
840
|
842
|
block->nominal_speed *= speed_factor;
|
841
|
843
|
block->nominal_rate *= speed_factor;
|
842
|
844
|
}
|
843
|
845
|
|
844
|
|
- // Compute and limit the acceleration rate for the trapezoid generator.
|
|
846
|
+ // Compute and limit the acceleration rate for the trapezoid generator.
|
845
|
847
|
float steps_per_mm = block->step_event_count / block->millimeters;
|
846
|
848
|
long bsx = block->steps[X_AXIS], bsy = block->steps[Y_AXIS], bsz = block->steps[Z_AXIS], bse = block->steps[E_AXIS];
|
847
|
849
|
if (bsx == 0 && bsy == 0 && bsz == 0) {
|
|
@@ -863,7 +865,7 @@ float junction_deviation = 0.1;
|
863
|
865
|
if ((float)acc_st * bsy / block->step_event_count > ysteps) acc_st = ysteps;
|
864
|
866
|
if ((float)acc_st * bsz / block->step_event_count > zsteps) acc_st = zsteps;
|
865
|
867
|
if ((float)acc_st * bse / block->step_event_count > esteps) acc_st = esteps;
|
866
|
|
-
|
|
868
|
+
|
867
|
869
|
block->acceleration_st = acc_st;
|
868
|
870
|
block->acceleration = acc_st / steps_per_mm;
|
869
|
871
|
block->acceleration_rate = (long)(acc_st * 16777216.0 / (F_CPU / 8.0));
|
|
@@ -911,7 +913,7 @@ float junction_deviation = 0.1;
|
911
|
913
|
|
912
|
914
|
// Start with a safe speed
|
913
|
915
|
float vmax_junction = max_xy_jerk / 2;
|
914
|
|
- float vmax_junction_factor = 1.0;
|
|
916
|
+ float vmax_junction_factor = 1.0;
|
915
|
917
|
float mz2 = max_z_jerk / 2, me2 = max_e_jerk / 2;
|
916
|
918
|
float csz = current_speed[Z_AXIS], cse = current_speed[E_AXIS];
|
917
|
919
|
if (fabs(csz) > mz2) vmax_junction = min(vmax_junction, mz2);
|
|
@@ -949,7 +951,7 @@ float junction_deviation = 0.1;
|
949
|
951
|
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
|
950
|
952
|
// the reverse and forward planners, the corresponding block junction speed will always be at the
|
951
|
953
|
// the maximum junction speed and may always be ignored for any speed reduction checks.
|
952
|
|
- block->nominal_length_flag = (block->nominal_speed <= v_allowable);
|
|
954
|
+ block->nominal_length_flag = (block->nominal_speed <= v_allowable);
|
953
|
955
|
block->recalculate_flag = true; // Always calculate trapezoid for new block
|
954
|
956
|
|
955
|
957
|
// Update previous path unit_vector and nominal speed
|
|
@@ -1029,7 +1031,7 @@ float junction_deviation = 0.1;
|
1029
|
1031
|
}
|
1030
|
1032
|
|
1031
|
1033
|
void plan_set_e_position(const float &e) {
|
1032
|
|
- position[E_AXIS] = lround(e * axis_steps_per_unit[E_AXIS]);
|
|
1034
|
+ position[E_AXIS] = lround(e * axis_steps_per_unit[E_AXIS]);
|
1033
|
1035
|
st_set_e_position(position[E_AXIS]);
|
1034
|
1036
|
}
|
1035
|
1037
|
|