|
@@ -182,7 +182,10 @@ float Planner::previous_speed[NUM_AXIS],
|
182
|
182
|
|
183
|
183
|
#if ENABLED(LIN_ADVANCE)
|
184
|
184
|
float Planner::extruder_advance_k, // Initialized by settings.load()
|
185
|
|
- Planner::advance_ed_ratio; // Initialized by settings.load()
|
|
185
|
+ Planner::advance_ed_ratio, // Initialized by settings.load()
|
|
186
|
+ Planner::position_float[XYZE], // Needed for accurate maths. Steps cannot be used!
|
|
187
|
+ Planner::lin_dist_xy,
|
|
188
|
+ Planner::lin_dist_e;
|
186
|
189
|
#endif
|
187
|
190
|
|
188
|
191
|
#if ENABLED(ULTRA_LCD)
|
|
@@ -198,6 +201,9 @@ Planner::Planner() { init(); }
|
198
|
201
|
void Planner::init() {
|
199
|
202
|
block_buffer_head = block_buffer_tail = 0;
|
200
|
203
|
ZERO(position);
|
|
204
|
+ #if ENABLED(LIN_ADVANCE)
|
|
205
|
+ ZERO(position_float);
|
|
206
|
+ #endif
|
201
|
207
|
ZERO(previous_speed);
|
202
|
208
|
previous_nominal_speed = 0.0;
|
203
|
209
|
#if ABL_PLANAR
|
|
@@ -742,7 +748,9 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
|
742
|
748
|
SERIAL_ECHOLNPGM(" steps)");
|
743
|
749
|
//*/
|
744
|
750
|
|
745
|
|
- #if ENABLED(PREVENT_COLD_EXTRUSION) || ENABLED(PREVENT_LENGTHY_EXTRUDE)
|
|
751
|
+ // If LIN_ADVANCE is disabled then do E move prevention with integers
|
|
752
|
+ // Otherwise it's done in _buffer_segment.
|
|
753
|
+ #if DISABLED(LIN_ADVANCE) && (ENABLED(PREVENT_COLD_EXTRUSION) || ENABLED(PREVENT_LENGTHY_EXTRUDE))
|
746
|
754
|
if (de) {
|
747
|
755
|
#if ENABLED(PREVENT_COLD_EXTRUSION)
|
748
|
756
|
if (thermalManager.tooColdToExtrude(extruder)) {
|
|
@@ -761,7 +769,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
|
761
|
769
|
}
|
762
|
770
|
#endif // PREVENT_LENGTHY_EXTRUDE
|
763
|
771
|
}
|
764
|
|
- #endif // PREVENT_COLD_EXTRUSION || PREVENT_LENGTHY_EXTRUDE
|
|
772
|
+ #endif // !LIN_ADVANCE && (PREVENT_COLD_EXTRUSION || PREVENT_LENGTHY_EXTRUDE)
|
765
|
773
|
|
766
|
774
|
// Compute direction bit-mask for this block
|
767
|
775
|
uint8_t dm = 0;
|
|
@@ -1102,14 +1110,10 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
|
1102
|
1110
|
}
|
1103
|
1111
|
#endif
|
1104
|
1112
|
|
1105
|
|
- // Calculate and limit speed in mm/sec for each axis, calculate minimum acceleration ratio
|
|
1113
|
+ // Calculate and limit speed in mm/sec for each axis
|
1106
|
1114
|
float current_speed[NUM_AXIS], speed_factor = 1.0; // factor <1 decreases speed
|
1107
|
|
- float max_stepper_speed = 0, min_axis_accel_ratio = 1; // ratio < 1 means acceleration ramp needed
|
1108
|
1115
|
LOOP_XYZE(i) {
|
1109
|
1116
|
const float cs = FABS((current_speed[i] = delta_mm[i] * inverse_secs));
|
1110
|
|
- if (cs > max_jerk[i])
|
1111
|
|
- NOMORE(min_axis_accel_ratio, max_jerk[i] / cs);
|
1112
|
|
- NOLESS(max_stepper_speed, cs);
|
1113
|
1117
|
#if ENABLED(DISTINCT_E_FACTORS)
|
1114
|
1118
|
if (i == E_AXIS) i += extruder;
|
1115
|
1119
|
#endif
|
|
@@ -1154,9 +1158,6 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
|
1154
|
1158
|
}
|
1155
|
1159
|
#endif // XY_FREQUENCY_LIMIT
|
1156
|
1160
|
|
1157
|
|
- block->nominal_speed = max_stepper_speed; // (mm/sec) Always > 0
|
1158
|
|
- block->nominal_rate = CEIL(block->step_event_count * inverse_secs); // (step/sec) Always > 0
|
1159
|
|
-
|
1160
|
1161
|
// Correct the speed
|
1161
|
1162
|
if (speed_factor < 1.0) {
|
1162
|
1163
|
LOOP_XYZE(i) current_speed[i] *= speed_factor;
|
|
@@ -1164,9 +1165,6 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
|
1164
|
1165
|
block->nominal_rate *= speed_factor;
|
1165
|
1166
|
}
|
1166
|
1167
|
|
1167
|
|
- float safe_speed = block->nominal_speed * min_axis_accel_ratio;
|
1168
|
|
- static float previous_safe_speed;
|
1169
|
|
-
|
1170
|
1168
|
// Compute and limit the acceleration rate for the trapezoid generator.
|
1171
|
1169
|
const float steps_per_mm = block->step_event_count * inverse_millimeters;
|
1172
|
1170
|
uint32_t accel;
|
|
@@ -1268,6 +1266,32 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
|
1268
|
1266
|
}
|
1269
|
1267
|
#endif
|
1270
|
1268
|
|
|
1269
|
+ /**
|
|
1270
|
+ * Adapted from Průša MKS firmware
|
|
1271
|
+ * https://github.com/prusa3d/Prusa-Firmware
|
|
1272
|
+ *
|
|
1273
|
+ * Start with a safe speed (from which the machine may halt to stop immediately).
|
|
1274
|
+ */
|
|
1275
|
+
|
|
1276
|
+ // Exit speed limited by a jerk to full halt of a previous last segment
|
|
1277
|
+ static float previous_safe_speed;
|
|
1278
|
+
|
|
1279
|
+ float safe_speed = block->nominal_speed;
|
|
1280
|
+ uint8_t limited = 0;
|
|
1281
|
+ LOOP_XYZE(i) {
|
|
1282
|
+ const float jerk = FABS(current_speed[i]), maxj = max_jerk[i];
|
|
1283
|
+ if (jerk > maxj) {
|
|
1284
|
+ if (limited) {
|
|
1285
|
+ const float mjerk = maxj * block->nominal_speed;
|
|
1286
|
+ if (jerk * safe_speed > mjerk) safe_speed = mjerk / jerk;
|
|
1287
|
+ }
|
|
1288
|
+ else {
|
|
1289
|
+ ++limited;
|
|
1290
|
+ safe_speed = maxj;
|
|
1291
|
+ }
|
|
1292
|
+ }
|
|
1293
|
+ }
|
|
1294
|
+
|
1271
|
1295
|
if (moves_queued && !UNEAR_ZERO(previous_nominal_speed)) {
|
1272
|
1296
|
// Estimate a maximum velocity allowed at a joint of two successive segments.
|
1273
|
1297
|
// If this maximum velocity allowed is lower than the minimum of the entry / exit safe velocities,
|
|
@@ -1279,7 +1303,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
|
1279
|
1303
|
|
1280
|
1304
|
// Factor to multiply the previous / current nominal velocities to get componentwise limited velocities.
|
1281
|
1305
|
float v_factor = 1;
|
1282
|
|
- uint8_t limited = 0;
|
|
1306
|
+ limited = 0;
|
1283
|
1307
|
|
1284
|
1308
|
// Now limit the jerk in all axes.
|
1285
|
1309
|
const float smaller_speed_factor = vmax_junction / previous_nominal_speed;
|
|
@@ -1355,16 +1379,16 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
|
1355
|
1379
|
* In that case, the retract and move will be executed together.
|
1356
|
1380
|
* This leads to too many advance steps due to a huge e_acceleration.
|
1357
|
1381
|
* The math is good, but we must avoid retract moves with advance!
|
1358
|
|
- * de > 0 : Extruder is running forward (e.g., for "Wipe while retracting" (Slic3r) or "Combing" (Cura) moves)
|
|
1382
|
+ * lin_dist_e > 0 : Extruder is running forward (e.g., for "Wipe while retracting" (Slic3r) or "Combing" (Cura) moves)
|
1359
|
1383
|
*/
|
1360
|
1384
|
block->use_advance_lead = esteps && (block->steps[X_AXIS] || block->steps[Y_AXIS])
|
1361
|
1385
|
&& extruder_advance_k
|
1362
|
1386
|
&& (uint32_t)esteps != block->step_event_count
|
1363
|
|
- && de > 0;
|
|
1387
|
+ && lin_dist_e > 0;
|
1364
|
1388
|
if (block->use_advance_lead)
|
1365
|
1389
|
block->abs_adv_steps_multiplier8 = LROUND(
|
1366
|
1390
|
extruder_advance_k
|
1367
|
|
- * (UNEAR_ZERO(advance_ed_ratio) ? de * steps_to_mm[E_AXIS_N] / HYPOT(da * steps_to_mm[X_AXIS], db * steps_to_mm[Y_AXIS]) : advance_ed_ratio) // Use the fixed ratio, if set
|
|
1391
|
+ * (UNEAR_ZERO(advance_ed_ratio) ? lin_dist_e / lin_dist_xy : advance_ed_ratio) // Use the fixed ratio, if set
|
1368
|
1392
|
* (block->nominal_speed / (float)block->nominal_rate)
|
1369
|
1393
|
* axis_steps_per_mm[E_AXIS_N] * 256.0
|
1370
|
1394
|
);
|
|
@@ -1442,16 +1466,69 @@ void Planner::buffer_segment(const float &a, const float &b, const float &c, con
|
1442
|
1466
|
SERIAL_ECHOLNPGM(")");
|
1443
|
1467
|
//*/
|
1444
|
1468
|
|
1445
|
|
- // DRYRUN ignores all temperature constraints and assures that the extruder is instantly satisfied
|
1446
|
|
- if (DEBUGGING(DRYRUN))
|
|
1469
|
+ // DRYRUN prevents E moves from taking place
|
|
1470
|
+ if (DEBUGGING(DRYRUN)) {
|
1447
|
1471
|
position[E_AXIS] = target[E_AXIS];
|
|
1472
|
+ #if ENABLED(LIN_ADVANCE)
|
|
1473
|
+ position_float[E_AXIS] = e;
|
|
1474
|
+ #endif
|
|
1475
|
+ }
|
|
1476
|
+
|
|
1477
|
+ #if ENABLED(LIN_ADVANCE)
|
|
1478
|
+ lin_dist_e = e - position_float[E_AXIS];
|
|
1479
|
+ #endif
|
|
1480
|
+
|
|
1481
|
+ // If LIN_ADVANCE is enabled then do E move prevention with floats
|
|
1482
|
+ // Otherwise it's done in _buffer_steps.
|
|
1483
|
+ #if ENABLED(LIN_ADVANCE) && (ENABLED(PREVENT_COLD_EXTRUSION) || ENABLED(PREVENT_LENGTHY_EXTRUDE))
|
|
1484
|
+ if (lin_dist_e) {
|
|
1485
|
+ #if ENABLED(PREVENT_COLD_EXTRUSION)
|
|
1486
|
+ if (thermalManager.tooColdToExtrude(extruder)) {
|
|
1487
|
+ position_float[E_AXIS] = e; // Behave as if the move really took place, but ignore E part
|
|
1488
|
+ position[E_AXIS] = target[E_AXIS];
|
|
1489
|
+ lin_dist_e = 0;
|
|
1490
|
+ SERIAL_ECHO_START();
|
|
1491
|
+ SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
|
|
1492
|
+ }
|
|
1493
|
+ #endif // PREVENT_COLD_EXTRUSION
|
|
1494
|
+ #if ENABLED(PREVENT_LENGTHY_EXTRUDE)
|
|
1495
|
+ if (lin_dist_e * e_factor[extruder] > (EXTRUDE_MAXLENGTH)) {
|
|
1496
|
+ position_float[E_AXIS] = e; // Behave as if the move really took place, but ignore E part
|
|
1497
|
+ position[E_AXIS] = target[E_AXIS];
|
|
1498
|
+ lin_dist_e = 0;
|
|
1499
|
+ SERIAL_ECHO_START();
|
|
1500
|
+ SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
|
|
1501
|
+ }
|
|
1502
|
+ #endif // PREVENT_LENGTHY_EXTRUDE
|
|
1503
|
+ }
|
|
1504
|
+ #endif // LIN_ADVANCE && (PREVENT_COLD_EXTRUSION || PREVENT_LENGTHY_EXTRUDE)
|
|
1505
|
+
|
|
1506
|
+ #if ENABLED(LIN_ADVANCE)
|
|
1507
|
+ if (lin_dist_e > 0)
|
|
1508
|
+ lin_dist_xy = HYPOT(a - position_float[X_AXIS], b - position_float[Y_AXIS]);
|
|
1509
|
+ #endif
|
1448
|
1510
|
|
1449
|
1511
|
// Always split the first move into two (if not homing or probing)
|
1450
|
1512
|
if (!blocks_queued()) {
|
|
1513
|
+
|
1451
|
1514
|
#define _BETWEEN(A) (position[A##_AXIS] + target[A##_AXIS]) >> 1
|
1452
|
1515
|
const int32_t between[XYZE] = { _BETWEEN(X), _BETWEEN(Y), _BETWEEN(Z), _BETWEEN(E) };
|
1453
|
1516
|
DISABLE_STEPPER_DRIVER_INTERRUPT();
|
|
1517
|
+
|
|
1518
|
+ #if ENABLED(LIN_ADVANCE)
|
|
1519
|
+ lin_dist_xy *= 0.5;
|
|
1520
|
+ lin_dist_e *= 0.5;
|
|
1521
|
+ #endif
|
|
1522
|
+
|
1454
|
1523
|
_buffer_steps(between, fr_mm_s, extruder);
|
|
1524
|
+
|
|
1525
|
+ #if ENABLED(LIN_ADVANCE)
|
|
1526
|
+ position_float[X_AXIS] = (position_float[X_AXIS] + a) * 0.5;
|
|
1527
|
+ position_float[Y_AXIS] = (position_float[Y_AXIS] + b) * 0.5;
|
|
1528
|
+ //position_float[Z_AXIS] = (position_float[Z_AXIS] + c) * 0.5;
|
|
1529
|
+ position_float[E_AXIS] = (position_float[E_AXIS] + e) * 0.5;
|
|
1530
|
+ #endif
|
|
1531
|
+
|
1455
|
1532
|
const uint8_t next = block_buffer_head;
|
1456
|
1533
|
_buffer_steps(target, fr_mm_s, extruder);
|
1457
|
1534
|
SBI(block_buffer[next].flag, BLOCK_BIT_CONTINUED);
|
|
@@ -1462,6 +1539,12 @@ void Planner::buffer_segment(const float &a, const float &b, const float &c, con
|
1462
|
1539
|
|
1463
|
1540
|
stepper.wake_up();
|
1464
|
1541
|
|
|
1542
|
+ #if ENABLED(LIN_ADVANCE)
|
|
1543
|
+ position_float[X_AXIS] = a;
|
|
1544
|
+ position_float[Y_AXIS] = b;
|
|
1545
|
+ //position_float[Z_AXIS] = c;
|
|
1546
|
+ position_float[E_AXIS] = e;
|
|
1547
|
+ #endif
|
1465
|
1548
|
} // buffer_segment()
|
1466
|
1549
|
|
1467
|
1550
|
/**
|
|
@@ -1482,6 +1565,12 @@ void Planner::_set_position_mm(const float &a, const float &b, const float &c, c
|
1482
|
1565
|
nb = position[Y_AXIS] = LROUND(b * axis_steps_per_mm[Y_AXIS]),
|
1483
|
1566
|
nc = position[Z_AXIS] = LROUND(c * axis_steps_per_mm[Z_AXIS]),
|
1484
|
1567
|
ne = position[E_AXIS] = LROUND(e * axis_steps_per_mm[_EINDEX]);
|
|
1568
|
+ #if ENABLED(LIN_ADVANCE)
|
|
1569
|
+ position_float[X_AXIS] = a;
|
|
1570
|
+ position_float[Y_AXIS] = b;
|
|
1571
|
+ //position_float[Z_AXIS] = c;
|
|
1572
|
+ position_float[E_AXIS] = e;
|
|
1573
|
+ #endif
|
1485
|
1574
|
stepper.set_position(na, nb, nc, ne);
|
1486
|
1575
|
previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
|
1487
|
1576
|
ZERO(previous_speed);
|
|
@@ -1506,8 +1595,16 @@ void Planner::set_position_mm_kinematic(const float (&cart)[XYZE]) {
|
1506
|
1595
|
* Sync from the stepper positions. (e.g., after an interrupted move)
|
1507
|
1596
|
*/
|
1508
|
1597
|
void Planner::sync_from_steppers() {
|
1509
|
|
- LOOP_XYZE(i)
|
|
1598
|
+ LOOP_XYZE(i) {
|
1510
|
1599
|
position[i] = stepper.position((AxisEnum)i);
|
|
1600
|
+ #if ENABLED(LIN_ADVANCE)
|
|
1601
|
+ position_float[i] = position[i] * steps_to_mm[i
|
|
1602
|
+ #if ENABLED(DISTINCT_E_FACTORS)
|
|
1603
|
+ + (i == E_AXIS ? active_extruder : 0)
|
|
1604
|
+ #endif
|
|
1605
|
+ ];
|
|
1606
|
+ #endif
|
|
1607
|
+ }
|
1511
|
1608
|
}
|
1512
|
1609
|
|
1513
|
1610
|
/**
|
|
@@ -1521,6 +1618,9 @@ void Planner::set_position_mm(const AxisEnum axis, const float &v) {
|
1521
|
1618
|
const uint8_t axis_index = axis;
|
1522
|
1619
|
#endif
|
1523
|
1620
|
position[axis] = LROUND(v * axis_steps_per_mm[axis_index]);
|
|
1621
|
+ #if ENABLED(LIN_ADVANCE)
|
|
1622
|
+ position_float[axis] = v;
|
|
1623
|
+ #endif
|
1524
|
1624
|
stepper.set_position(axis, v);
|
1525
|
1625
|
previous_speed[axis] = 0.0;
|
1526
|
1626
|
}
|