|
@@ -1453,26 +1453,23 @@ inline float get_homing_bump_feedrate(AxisEnum axis) {
|
1453
|
1453
|
return homing_feedrate_mm_s[axis] / hbd;
|
1454
|
1454
|
}
|
1455
|
1455
|
|
1456
|
|
-#if !IS_KINEMATIC
|
1457
|
|
- //
|
1458
|
|
- // line_to_current_position
|
1459
|
|
- // Move the planner to the current position from wherever it last moved
|
1460
|
|
- // (or from wherever it has been told it is located).
|
1461
|
|
- //
|
1462
|
|
- inline void line_to_current_position() {
|
1463
|
|
- planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate_mm_s, active_extruder);
|
1464
|
|
- }
|
1465
|
|
-
|
1466
|
|
- //
|
1467
|
|
- // line_to_destination
|
1468
|
|
- // Move the planner, not necessarily synced with current_position
|
1469
|
|
- //
|
1470
|
|
- inline void line_to_destination(float fr_mm_s) {
|
1471
|
|
- planner.buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], fr_mm_s, active_extruder);
|
1472
|
|
- }
|
1473
|
|
- inline void line_to_destination() { line_to_destination(feedrate_mm_s); }
|
|
1456
|
+//
|
|
1457
|
+// line_to_current_position
|
|
1458
|
+// Move the planner to the current position from wherever it last moved
|
|
1459
|
+// (or from wherever it has been told it is located).
|
|
1460
|
+//
|
|
1461
|
+inline void line_to_current_position() {
|
|
1462
|
+ planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate_mm_s, active_extruder);
|
|
1463
|
+}
|
1474
|
1464
|
|
1475
|
|
-#endif // !IS_KINEMATIC
|
|
1465
|
+//
|
|
1466
|
+// line_to_destination
|
|
1467
|
+// Move the planner, not necessarily synced with current_position
|
|
1468
|
+//
|
|
1469
|
+inline void line_to_destination(float fr_mm_s) {
|
|
1470
|
+ planner.buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], fr_mm_s, active_extruder);
|
|
1471
|
+}
|
|
1472
|
+inline void line_to_destination() { line_to_destination(feedrate_mm_s); }
|
1476
|
1473
|
|
1477
|
1474
|
inline void set_current_to_destination() { memcpy(current_position, destination, sizeof(current_position)); }
|
1478
|
1475
|
inline void set_destination_to_current() { memcpy(destination, current_position, sizeof(destination)); }
|
|
@@ -1485,12 +1482,19 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
|
1485
|
1482
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
1486
|
1483
|
if (DEBUGGING(LEVELING)) DEBUG_POS("prepare_uninterpolated_move_to_destination", destination);
|
1487
|
1484
|
#endif
|
|
1485
|
+
|
|
1486
|
+ if ( current_position[X_AXIS] == destination[X_AXIS]
|
|
1487
|
+ && current_position[Y_AXIS] == destination[Y_AXIS]
|
|
1488
|
+ && current_position[Z_AXIS] == destination[Z_AXIS]
|
|
1489
|
+ && current_position[E_AXIS] == destination[E_AXIS]
|
|
1490
|
+ ) return;
|
|
1491
|
+
|
1488
|
1492
|
refresh_cmd_timeout();
|
1489
|
1493
|
inverse_kinematics(destination);
|
1490
|
1494
|
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], destination[E_AXIS], MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s), active_extruder);
|
1491
|
1495
|
set_current_to_destination();
|
1492
|
1496
|
}
|
1493
|
|
-#endif
|
|
1497
|
+#endif // IS_KINEMATIC
|
1494
|
1498
|
|
1495
|
1499
|
/**
|
1496
|
1500
|
* Plan a move to (X, Y, Z) and set the current_position
|
|
@@ -1557,16 +1561,12 @@ void do_blocking_move_to(const float &x, const float &y, const float &z, const f
|
1557
|
1561
|
#endif
|
1558
|
1562
|
}
|
1559
|
1563
|
|
1560
|
|
- #if ENABLED(DEBUG_LEVELING_FEATURE)
|
1561
|
|
- if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("<<< do_blocking_move_to");
|
1562
|
|
- #endif
|
1563
|
|
-
|
1564
|
1564
|
#elif IS_SCARA
|
1565
|
1565
|
|
1566
|
1566
|
set_destination_to_current();
|
1567
|
1567
|
|
1568
|
1568
|
// If Z needs to raise, do it before moving XY
|
1569
|
|
- if (current_position[Z_AXIS] < z) {
|
|
1569
|
+ if (destination[Z_AXIS] < z) {
|
1570
|
1570
|
destination[Z_AXIS] = z;
|
1571
|
1571
|
prepare_uninterpolated_move_to_destination(fr_mm_s ? fr_mm_s : homing_feedrate_mm_s[Z_AXIS]);
|
1572
|
1572
|
}
|
|
@@ -1576,7 +1576,7 @@ void do_blocking_move_to(const float &x, const float &y, const float &z, const f
|
1576
|
1576
|
prepare_uninterpolated_move_to_destination(fr_mm_s ? fr_mm_s : XY_PROBE_FEEDRATE_MM_S);
|
1577
|
1577
|
|
1578
|
1578
|
// If Z needs to lower, do it after moving XY
|
1579
|
|
- if (current_position[Z_AXIS] > z) {
|
|
1579
|
+ if (destination[Z_AXIS] > z) {
|
1580
|
1580
|
destination[Z_AXIS] = z;
|
1581
|
1581
|
prepare_uninterpolated_move_to_destination(fr_mm_s ? fr_mm_s : homing_feedrate_mm_s[Z_AXIS]);
|
1582
|
1582
|
}
|
|
@@ -1607,6 +1607,10 @@ void do_blocking_move_to(const float &x, const float &y, const float &z, const f
|
1607
|
1607
|
stepper.synchronize();
|
1608
|
1608
|
|
1609
|
1609
|
feedrate_mm_s = old_feedrate_mm_s;
|
|
1610
|
+
|
|
1611
|
+ #if ENABLED(DEBUG_LEVELING_FEATURE)
|
|
1612
|
+ if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("<<< do_blocking_move_to");
|
|
1613
|
+ #endif
|
1610
|
1614
|
}
|
1611
|
1615
|
void do_blocking_move_to_x(const float &x, const float &fr_mm_s/*=0.0*/) {
|
1612
|
1616
|
do_blocking_move_to(x, current_position[Y_AXIS], current_position[Z_AXIS], fr_mm_s);
|
|
@@ -1999,12 +2003,12 @@ static void clean_up_after_endstop_or_probe_move() {
|
1999
|
2003
|
// Clear endstop flags
|
2000
|
2004
|
endstops.hit_on_purpose();
|
2001
|
2005
|
|
|
2006
|
+ // Tell the planner where we actually are
|
|
2007
|
+ planner.sync_from_steppers();
|
|
2008
|
+
|
2002
|
2009
|
// Get Z where the steppers were interrupted
|
2003
|
2010
|
set_current_from_steppers_for_axis(Z_AXIS);
|
2004
|
2011
|
|
2005
|
|
- // Tell the planner where we actually are
|
2006
|
|
- SYNC_PLAN_POSITION_KINEMATIC();
|
2007
|
|
-
|
2008
|
2012
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
2009
|
2013
|
if (DEBUGGING(LEVELING)) DEBUG_POS("<<< do_probe_move", current_position);
|
2010
|
2014
|
#endif
|
|
@@ -2122,8 +2126,13 @@ static void clean_up_after_endstop_or_probe_move() {
|
2122
|
2126
|
|
2123
|
2127
|
/**
|
2124
|
2128
|
* Reset calibration results to zero.
|
|
2129
|
+ *
|
|
2130
|
+ * TODO: Proper functions to disable / enable
|
|
2131
|
+ * bed leveling via a flag, correcting the
|
|
2132
|
+ * current position in each case.
|
2125
|
2133
|
*/
|
2126
|
2134
|
void reset_bed_level() {
|
|
2135
|
+ planner.abl_enabled = false;
|
2127
|
2136
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
2128
|
2137
|
if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("reset_bed_level");
|
2129
|
2138
|
#endif
|
|
@@ -2131,7 +2140,6 @@ static void clean_up_after_endstop_or_probe_move() {
|
2131
|
2140
|
planner.bed_level_matrix.set_to_identity();
|
2132
|
2141
|
#elif ENABLED(AUTO_BED_LEVELING_NONLINEAR)
|
2133
|
2142
|
memset(bed_level_grid, 0, sizeof(bed_level_grid));
|
2134
|
|
- nonlinear_grid_spacing[X_AXIS] = nonlinear_grid_spacing[Y_AXIS] = 0;
|
2135
|
2143
|
#endif
|
2136
|
2144
|
}
|
2137
|
2145
|
|
|
@@ -2188,18 +2196,27 @@ static void clean_up_after_endstop_or_probe_move() {
|
2188
|
2196
|
/**
|
2189
|
2197
|
* Home an individual linear axis
|
2190
|
2198
|
*/
|
2191
|
|
-
|
2192
|
|
-static void do_homing_move(AxisEnum axis, float where, float fr_mm_s=0.0) {
|
|
2199
|
+static void do_homing_move(const AxisEnum axis, float distance, float fr_mm_s=0.0) {
|
2193
|
2200
|
|
2194
|
2201
|
#if HOMING_Z_WITH_PROBE && ENABLED(BLTOUCH)
|
2195
|
2202
|
bool deploy_bltouch = (axis == Z_AXIS && where < 0);
|
2196
|
2203
|
if (deploy_bltouch) set_bltouch_deployed(true);
|
2197
|
2204
|
#endif
|
2198
|
2205
|
|
|
2206
|
+ // Tell the planner we're at Z=0
|
2199
|
2207
|
current_position[axis] = 0;
|
2200
|
|
- sync_plan_position();
|
2201
|
|
- current_position[axis] = where;
|
2202
|
|
- planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], fr_mm_s ? fr_mm_s : homing_feedrate_mm_s[axis], active_extruder);
|
|
2208
|
+
|
|
2209
|
+ #if IS_SCARA
|
|
2210
|
+ SYNC_PLAN_POSITION_KINEMATIC();
|
|
2211
|
+ current_position[axis] = distance;
|
|
2212
|
+ inverse_kinematics(current_position);
|
|
2213
|
+ planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], current_position[E_AXIS], fr_mm_s ? fr_mm_s : homing_feedrate_mm_s[axis], active_extruder);
|
|
2214
|
+ #else
|
|
2215
|
+ sync_plan_position();
|
|
2216
|
+ current_position[axis] = distance;
|
|
2217
|
+ planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], fr_mm_s ? fr_mm_s : homing_feedrate_mm_s[axis], active_extruder);
|
|
2218
|
+ #endif
|
|
2219
|
+
|
2203
|
2220
|
stepper.synchronize();
|
2204
|
2221
|
|
2205
|
2222
|
#if HOMING_Z_WITH_PROBE && ENABLED(BLTOUCH)
|
|
@@ -2256,6 +2273,9 @@ static void homeaxis(AxisEnum axis) {
|
2256
|
2273
|
if (axis == Z_AXIS) stepper.set_homing_flag(true);
|
2257
|
2274
|
#endif
|
2258
|
2275
|
|
|
2276
|
+ // Fast move towards endstop until triggered
|
|
2277
|
+ do_homing_move(axis, 1.5 * max_length(axis) * axis_home_dir);
|
|
2278
|
+
|
2259
|
2279
|
// When homing Z with probe respect probe clearance
|
2260
|
2280
|
const float bump = axis_home_dir * (
|
2261
|
2281
|
#if HOMING_Z_WITH_PROBE
|
|
@@ -2264,12 +2284,13 @@ static void homeaxis(AxisEnum axis) {
|
2264
|
2284
|
home_bump_mm(axis)
|
2265
|
2285
|
);
|
2266
|
2286
|
|
2267
|
|
- // 1. Fast move towards endstop until triggered
|
2268
|
|
- // 2. Move away from the endstop by the axis HOME_BUMP_MM
|
2269
|
|
- // 3. Slow move towards endstop until triggered
|
2270
|
|
- do_homing_move(axis, 1.5 * max_length(axis) * axis_home_dir);
|
2271
|
|
- do_homing_move(axis, -bump);
|
2272
|
|
- do_homing_move(axis, 2 * bump, get_homing_bump_feedrate(axis));
|
|
2287
|
+ // If a second homing move is configured...
|
|
2288
|
+ if (bump) {
|
|
2289
|
+ // Move away from the endstop by the axis HOME_BUMP_MM
|
|
2290
|
+ do_homing_move(axis, -bump);
|
|
2291
|
+ // Slow move towards endstop until triggered
|
|
2292
|
+ do_homing_move(axis, 2 * bump, get_homing_bump_feedrate(axis));
|
|
2293
|
+ }
|
2273
|
2294
|
|
2274
|
2295
|
#if ENABLED(Z_DUAL_ENDSTOPS)
|
2275
|
2296
|
if (axis == Z_AXIS) {
|
|
@@ -2849,7 +2870,8 @@ inline void gcode_G4() {
|
2849
|
2870
|
|
2850
|
2871
|
// Move all carriages together linearly until an endstop is hit.
|
2851
|
2872
|
current_position[X_AXIS] = current_position[Y_AXIS] = current_position[Z_AXIS] = (Z_MAX_LENGTH + 10);
|
2852
|
|
- planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate_mm_s[X_AXIS], active_extruder);
|
|
2873
|
+ feedrate_mm_s = homing_feedrate_mm_s[X_AXIS];
|
|
2874
|
+ line_to_current_position();
|
2853
|
2875
|
stepper.synchronize();
|
2854
|
2876
|
endstops.hit_on_purpose(); // clear endstop hit flags
|
2855
|
2877
|
|
|
@@ -2902,22 +2924,20 @@ inline void gcode_G4() {
|
2902
|
2924
|
destination[Y_AXIS] = LOGICAL_Y_POSITION(Z_SAFE_HOMING_Y_POINT);
|
2903
|
2925
|
destination[Z_AXIS] = current_position[Z_AXIS]; // Z is already at the right height
|
2904
|
2926
|
|
2905
|
|
- #if HAS_BED_PROBE
|
2906
|
|
- destination[X_AXIS] -= X_PROBE_OFFSET_FROM_EXTRUDER;
|
2907
|
|
- destination[Y_AXIS] -= Y_PROBE_OFFSET_FROM_EXTRUDER;
|
2908
|
|
- #endif
|
2909
|
|
-
|
2910
|
|
- #if ENABLED(DEBUG_LEVELING_FEATURE)
|
2911
|
|
- if (DEBUGGING(LEVELING)) DEBUG_POS("Z_SAFE_HOMING", destination);
|
2912
|
|
- #endif
|
2913
|
|
-
|
2914
|
2927
|
if (position_is_reachable(
|
2915
|
2928
|
destination
|
2916
|
|
- #if HAS_BED_PROBE
|
|
2929
|
+ #if HOMING_Z_WITH_PROBE
|
2917
|
2930
|
, true
|
2918
|
2931
|
#endif
|
2919
|
2932
|
)
|
2920
|
2933
|
) {
|
|
2934
|
+ #if HOMING_Z_WITH_PROBE
|
|
2935
|
+ destination[X_AXIS] -= X_PROBE_OFFSET_FROM_EXTRUDER;
|
|
2936
|
+ destination[Y_AXIS] -= Y_PROBE_OFFSET_FROM_EXTRUDER;
|
|
2937
|
+ #endif
|
|
2938
|
+ #if ENABLED(DEBUG_LEVELING_FEATURE)
|
|
2939
|
+ if (DEBUGGING(LEVELING)) DEBUG_POS("Z_SAFE_HOMING", destination);
|
|
2940
|
+ #endif
|
2921
|
2941
|
do_blocking_move_to_xy(destination[X_AXIS], destination[Y_AXIS]);
|
2922
|
2942
|
HOMEAXIS(Z);
|
2923
|
2943
|
}
|
|
@@ -3133,19 +3153,13 @@ inline void gcode_G28() {
|
3133
|
3153
|
#if ENABLED(MESH_G28_REST_ORIGIN)
|
3134
|
3154
|
current_position[Z_AXIS] = 0.0;
|
3135
|
3155
|
set_destination_to_current();
|
3136
|
|
- feedrate_mm_s = homing_feedrate_mm_s[Z_AXIS];
|
3137
|
|
- line_to_destination();
|
|
3156
|
+ line_to_destination(homing_feedrate_mm_s[Z_AXIS]);
|
3138
|
3157
|
stepper.synchronize();
|
3139
|
3158
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
3140
|
3159
|
if (DEBUGGING(LEVELING)) DEBUG_POS("MBL Rest Origin", current_position);
|
3141
|
3160
|
#endif
|
3142
|
3161
|
#else
|
3143
|
|
- current_position[Z_AXIS] = MESH_HOME_SEARCH_Z -
|
3144
|
|
- mbl.get_z(RAW_CURRENT_POSITION(X_AXIS), RAW_CURRENT_POSITION(Y_AXIS))
|
3145
|
|
- #if Z_HOME_DIR > 0
|
3146
|
|
- + Z_MAX_POS
|
3147
|
|
- #endif
|
3148
|
|
- ;
|
|
3162
|
+ planner.unapply_leveling(current_position);
|
3149
|
3163
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
3150
|
3164
|
if (DEBUGGING(LEVELING)) DEBUG_POS("MBL adjusted MESH_HOME_SEARCH_Z", current_position);
|
3151
|
3165
|
#endif
|
|
@@ -3155,8 +3169,7 @@ inline void gcode_G28() {
|
3155
|
3169
|
current_position[Z_AXIS] = pre_home_z;
|
3156
|
3170
|
SYNC_PLAN_POSITION_KINEMATIC();
|
3157
|
3171
|
mbl.set_active(true);
|
3158
|
|
- current_position[Z_AXIS] = pre_home_z -
|
3159
|
|
- mbl.get_z(RAW_CURRENT_POSITION(X_AXIS), RAW_CURRENT_POSITION(Y_AXIS));
|
|
3172
|
+ planner.unapply_leveling(current_position);
|
3160
|
3173
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
3161
|
3174
|
if (DEBUGGING(LEVELING)) DEBUG_POS("MBL Home X or Y", current_position);
|
3162
|
3175
|
#endif
|
|
@@ -3505,16 +3518,15 @@ inline void gcode_G28() {
|
3505
|
3518
|
|
3506
|
3519
|
stepper.synchronize();
|
3507
|
3520
|
|
3508
|
|
- if (!dryrun) {
|
|
3521
|
+ // Disable auto bed leveling during G29
|
|
3522
|
+ bool auto_bed_leveling_was_enabled = planner.abl_enabled,
|
|
3523
|
+ abl_should_reenable = auto_bed_leveling_was_enabled;
|
3509
|
3524
|
|
3510
|
|
- // Reset the bed_level_matrix because leveling
|
3511
|
|
- // needs to be done without leveling enabled.
|
3512
|
|
- reset_bed_level();
|
|
3525
|
+ planner.abl_enabled = false;
|
3513
|
3526
|
|
3514
|
|
- //
|
|
3527
|
+ if (!dryrun) {
|
3515
|
3528
|
// Re-orient the current position without leveling
|
3516
|
3529
|
// based on where the steppers are positioned.
|
3517
|
|
- //
|
3518
|
3530
|
get_cartesian_from_steppers();
|
3519
|
3531
|
memcpy(current_position, cartes, sizeof(cartes));
|
3520
|
3532
|
|
|
@@ -3525,9 +3537,12 @@ inline void gcode_G28() {
|
3525
|
3537
|
setup_for_endstop_or_probe_move();
|
3526
|
3538
|
|
3527
|
3539
|
// Deploy the probe. Probe will raise if needed.
|
3528
|
|
- if (DEPLOY_PROBE()) return;
|
|
3540
|
+ if (DEPLOY_PROBE()) {
|
|
3541
|
+ planner.abl_enabled = abl_should_reenable;
|
|
3542
|
+ return;
|
|
3543
|
+ }
|
3529
|
3544
|
|
3530
|
|
- float xProbe, yProbe, measured_z = 0;
|
|
3545
|
+ float xProbe = 0, yProbe = 0, measured_z = 0;
|
3531
|
3546
|
|
3532
|
3547
|
#if ENABLED(AUTO_BED_LEVELING_GRID)
|
3533
|
3548
|
|
|
@@ -3537,11 +3552,16 @@ inline void gcode_G28() {
|
3537
|
3552
|
|
3538
|
3553
|
#if ENABLED(AUTO_BED_LEVELING_NONLINEAR)
|
3539
|
3554
|
|
3540
|
|
- nonlinear_grid_spacing[X_AXIS] = xGridSpacing;
|
3541
|
|
- nonlinear_grid_spacing[Y_AXIS] = yGridSpacing;
|
3542
|
3555
|
float zoffset = zprobe_zoffset;
|
3543
|
3556
|
if (code_seen('Z')) zoffset += code_value_axis_units(Z_AXIS);
|
3544
|
3557
|
|
|
3558
|
+ if (xGridSpacing != nonlinear_grid_spacing[X_AXIS] || yGridSpacing != nonlinear_grid_spacing[Y_AXIS]) {
|
|
3559
|
+ nonlinear_grid_spacing[X_AXIS] = xGridSpacing;
|
|
3560
|
+ nonlinear_grid_spacing[Y_AXIS] = yGridSpacing;
|
|
3561
|
+ // Can't re-enable (on error) until the new grid is written
|
|
3562
|
+ abl_should_reenable = false;
|
|
3563
|
+ }
|
|
3564
|
+
|
3545
|
3565
|
#elif ENABLED(AUTO_BED_LEVELING_LINEAR_GRID)
|
3546
|
3566
|
|
3547
|
3567
|
/**
|
|
@@ -3600,6 +3620,11 @@ inline void gcode_G28() {
|
3600
|
3620
|
|
3601
|
3621
|
measured_z = probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level);
|
3602
|
3622
|
|
|
3623
|
+ if (measured_z == NAN) {
|
|
3624
|
+ planner.abl_enabled = abl_should_reenable;
|
|
3625
|
+ return;
|
|
3626
|
+ }
|
|
3627
|
+
|
3603
|
3628
|
#if ENABLED(AUTO_BED_LEVELING_LINEAR_GRID)
|
3604
|
3629
|
|
3605
|
3630
|
mean += measured_z;
|
|
@@ -3639,6 +3664,11 @@ inline void gcode_G28() {
|
3639
|
3664
|
measured_z = points[i].z = probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level);
|
3640
|
3665
|
}
|
3641
|
3666
|
|
|
3667
|
+ if (measured_z == NAN) {
|
|
3668
|
+ planner.abl_enabled = abl_should_reenable;
|
|
3669
|
+ return;
|
|
3670
|
+ }
|
|
3671
|
+
|
3642
|
3672
|
if (!dryrun) {
|
3643
|
3673
|
vector_3 planeNormal = vector_3::cross(points[0] - points[1], points[2] - points[1]).get_normal();
|
3644
|
3674
|
if (planeNormal.z < 0) {
|
|
@@ -3647,12 +3677,23 @@ inline void gcode_G28() {
|
3647
|
3677
|
planeNormal.z *= -1;
|
3648
|
3678
|
}
|
3649
|
3679
|
planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
|
|
3680
|
+
|
|
3681
|
+ // Can't re-enable (on error) until the new grid is written
|
|
3682
|
+ abl_should_reenable = false;
|
3650
|
3683
|
}
|
3651
|
3684
|
|
3652
|
3685
|
#endif // AUTO_BED_LEVELING_3POINT
|
3653
|
3686
|
|
3654
|
3687
|
// Raise to _Z_CLEARANCE_DEPLOY_PROBE. Stow the probe.
|
3655
|
|
- if (STOW_PROBE()) return;
|
|
3688
|
+ if (STOW_PROBE()) {
|
|
3689
|
+ planner.abl_enabled = abl_should_reenable;
|
|
3690
|
+ return;
|
|
3691
|
+ }
|
|
3692
|
+
|
|
3693
|
+ //
|
|
3694
|
+ // Unless this is a dry run, auto bed leveling will
|
|
3695
|
+ // definitely be enabled after this point
|
|
3696
|
+ //
|
3656
|
3697
|
|
3657
|
3698
|
// Restore state after probing
|
3658
|
3699
|
clean_up_after_endstop_or_probe_move();
|
|
@@ -3842,6 +3883,9 @@ inline void gcode_G28() {
|
3842
|
3883
|
report_current_position();
|
3843
|
3884
|
|
3844
|
3885
|
KEEPALIVE_STATE(IN_HANDLER);
|
|
3886
|
+
|
|
3887
|
+ // Auto Bed Leveling is complete! Enable if possible.
|
|
3888
|
+ planner.abl_enabled = dryrun ? abl_should_reenable : true;
|
3845
|
3889
|
}
|
3846
|
3890
|
|
3847
|
3891
|
#endif // AUTO_BED_LEVELING_FEATURE
|
|
@@ -3925,6 +3969,8 @@ inline void gcode_G92() {
|
3925
|
3969
|
SYNC_PLAN_POSITION_KINEMATIC();
|
3926
|
3970
|
else if (didE)
|
3927
|
3971
|
sync_plan_position_e();
|
|
3972
|
+
|
|
3973
|
+ report_current_position();
|
3928
|
3974
|
}
|
3929
|
3975
|
|
3930
|
3976
|
#if ENABLED(ULTIPANEL) || ENABLED(EMERGENCY_PARSER)
|
|
@@ -4186,7 +4232,11 @@ inline void gcode_M42() {
|
4186
|
4232
|
if (pin_number < 0) return;
|
4187
|
4233
|
|
4188
|
4234
|
for (uint8_t i = 0; i < COUNT(sensitive_pins); i++)
|
4189
|
|
- if (pin_number == sensitive_pins[i]) return;
|
|
4235
|
+ if (pin_number == sensitive_pins[i]) {
|
|
4236
|
+ SERIAL_ERROR_START;
|
|
4237
|
+ SERIAL_ERRORLNPGM(MSG_ERR_PROTECTED_PIN);
|
|
4238
|
+ return;
|
|
4239
|
+ }
|
4190
|
4240
|
|
4191
|
4241
|
pinMode(pin_number, OUTPUT);
|
4192
|
4242
|
digitalWrite(pin_number, pin_status);
|
|
@@ -7736,7 +7786,7 @@ void ok_to_send() {
|
7736
|
7786
|
|
7737
|
7787
|
// Get the Z adjustment for non-linear bed leveling
|
7738
|
7788
|
float nonlinear_z_offset(float cartesian[XYZ]) {
|
7739
|
|
- if (nonlinear_grid_spacing[X_AXIS] == 0 || nonlinear_grid_spacing[Y_AXIS] == 0) return 0; // G29 not done!
|
|
7789
|
+ if (planner.abl_enabled) return;
|
7740
|
7790
|
|
7741
|
7791
|
int half_x = (ABL_GRID_POINTS_X - 1) / 2,
|
7742
|
7792
|
half_y = (ABL_GRID_POINTS_Y - 1) / 2;
|
|
@@ -7846,15 +7896,19 @@ void ok_to_send() {
|
7846
|
7896
|
) \
|
7847
|
7897
|
)
|
7848
|
7898
|
|
|
7899
|
+ #define DELTA_RAW_IK() do { \
|
|
7900
|
+ delta[A_AXIS] = DELTA_Z(1); \
|
|
7901
|
+ delta[B_AXIS] = DELTA_Z(2); \
|
|
7902
|
+ delta[C_AXIS] = DELTA_Z(3); \
|
|
7903
|
+ } while(0)
|
|
7904
|
+
|
7849
|
7905
|
#define DELTA_LOGICAL_IK() do { \
|
7850
|
7906
|
const float raw[XYZ] = { \
|
7851
|
7907
|
RAW_X_POSITION(logical[X_AXIS]), \
|
7852
|
7908
|
RAW_Y_POSITION(logical[Y_AXIS]), \
|
7853
|
7909
|
RAW_Z_POSITION(logical[Z_AXIS]) \
|
7854
|
7910
|
}; \
|
7855
|
|
- delta[A_AXIS] = DELTA_Z(1); \
|
7856
|
|
- delta[B_AXIS] = DELTA_Z(2); \
|
7857
|
|
- delta[C_AXIS] = DELTA_Z(3); \
|
|
7911
|
+ DELTA_RAW_IK(); \
|
7858
|
7912
|
} while(0)
|
7859
|
7913
|
|
7860
|
7914
|
#define DELTA_DEBUG() do { \
|
|
@@ -8012,7 +8066,7 @@ void get_cartesian_from_steppers() {
|
8012
|
8066
|
void set_current_from_steppers_for_axis(const AxisEnum axis) {
|
8013
|
8067
|
get_cartesian_from_steppers();
|
8014
|
8068
|
#if PLANNER_LEVELING
|
8015
|
|
- planner.unapply_leveling(cartes[X_AXIS], cartes[Y_AXIS], cartes[Z_AXIS]);
|
|
8069
|
+ planner.unapply_leveling(cartes);
|
8016
|
8070
|
#endif
|
8017
|
8071
|
if (axis == ALL_AXES)
|
8018
|
8072
|
memcpy(current_position, cartes, sizeof(cartes));
|
|
@@ -8091,101 +8145,123 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
|
8091
|
8145
|
* This calls planner.buffer_line several times, adding
|
8092
|
8146
|
* small incremental moves for DELTA or SCARA.
|
8093
|
8147
|
*/
|
8094
|
|
- inline bool prepare_kinematic_move_to(float logical[NUM_AXIS]) {
|
|
8148
|
+ inline bool prepare_kinematic_move_to(float ltarget[NUM_AXIS]) {
|
8095
|
8149
|
|
8096
|
8150
|
// Get the top feedrate of the move in the XY plane
|
8097
|
8151
|
float _feedrate_mm_s = MMS_SCALED(feedrate_mm_s);
|
8098
|
8152
|
|
8099
|
|
- // If the move is only in Z don't split up the move.
|
8100
|
|
- // This shortcut cannot be used if planar bed leveling
|
8101
|
|
- // is in use, but is fine with mesh-based bed leveling
|
8102
|
|
- if (logical[X_AXIS] == current_position[X_AXIS] && logical[Y_AXIS] == current_position[Y_AXIS]) {
|
8103
|
|
- inverse_kinematics(logical);
|
8104
|
|
- planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], _feedrate_mm_s, active_extruder);
|
|
8153
|
+ // If the move is only in Z/E don't split up the move
|
|
8154
|
+ if (ltarget[X_AXIS] == current_position[X_AXIS] && ltarget[Y_AXIS] == current_position[Y_AXIS]) {
|
|
8155
|
+ inverse_kinematics(ltarget);
|
|
8156
|
+ planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], ltarget[E_AXIS], _feedrate_mm_s, active_extruder);
|
8105
|
8157
|
return true;
|
8106
|
8158
|
}
|
8107
|
8159
|
|
8108
|
|
- // Get the distance moved in XYZ
|
|
8160
|
+ // Get the cartesian distances moved in XYZE
|
8109
|
8161
|
float difference[NUM_AXIS];
|
8110
|
|
- LOOP_XYZE(i) difference[i] = logical[i] - current_position[i];
|
|
8162
|
+ LOOP_XYZE(i) difference[i] = ltarget[i] - current_position[i];
|
8111
|
8163
|
|
|
8164
|
+ // Get the linear distance in XYZ
|
8112
|
8165
|
float cartesian_mm = sqrt(sq(difference[X_AXIS]) + sq(difference[Y_AXIS]) + sq(difference[Z_AXIS]));
|
|
8166
|
+
|
|
8167
|
+ // If the move is very short, check the E move distance
|
8113
|
8168
|
if (UNEAR_ZERO(cartesian_mm)) cartesian_mm = abs(difference[E_AXIS]);
|
|
8169
|
+
|
|
8170
|
+ // No E move either? Game over.
|
8114
|
8171
|
if (UNEAR_ZERO(cartesian_mm)) return false;
|
8115
|
8172
|
|
8116
|
8173
|
// Minimum number of seconds to move the given distance
|
8117
|
8174
|
float seconds = cartesian_mm / _feedrate_mm_s;
|
8118
|
8175
|
|
8119
|
8176
|
// The number of segments-per-second times the duration
|
8120
|
|
- // gives the number of segments we should produce
|
|
8177
|
+ // gives the number of segments
|
8121
|
8178
|
uint16_t segments = delta_segments_per_second * seconds;
|
8122
|
8179
|
|
|
8180
|
+ // For SCARA minimum segment size is 0.5mm
|
8123
|
8181
|
#if IS_SCARA
|
8124
|
8182
|
NOMORE(segments, cartesian_mm * 2);
|
8125
|
8183
|
#endif
|
8126
|
8184
|
|
|
8185
|
+ // At least one segment is required
|
8127
|
8186
|
NOLESS(segments, 1);
|
8128
|
8187
|
|
8129
|
|
- // Each segment produces this much of the move
|
8130
|
|
- float inv_segments = 1.0 / segments,
|
8131
|
|
- segment_distance[XYZE] = {
|
8132
|
|
- difference[X_AXIS] * inv_segments,
|
8133
|
|
- difference[Y_AXIS] * inv_segments,
|
8134
|
|
- difference[Z_AXIS] * inv_segments,
|
8135
|
|
- difference[E_AXIS] * inv_segments
|
|
8188
|
+ // The approximate length of each segment
|
|
8189
|
+ float segment_distance[XYZE] = {
|
|
8190
|
+ difference[X_AXIS] / segments,
|
|
8191
|
+ difference[Y_AXIS] / segments,
|
|
8192
|
+ difference[Z_AXIS] / segments,
|
|
8193
|
+ difference[E_AXIS] / segments
|
8136
|
8194
|
};
|
8137
|
8195
|
|
8138
|
8196
|
// SERIAL_ECHOPAIR("mm=", cartesian_mm);
|
8139
|
8197
|
// SERIAL_ECHOPAIR(" seconds=", seconds);
|
8140
|
8198
|
// SERIAL_ECHOLNPAIR(" segments=", segments);
|
8141
|
8199
|
|
8142
|
|
- // Send all the segments to the planner
|
|
8200
|
+ // Drop one segment so the last move is to the exact target.
|
|
8201
|
+ // If there's only 1 segment, loops will be skipped entirely.
|
|
8202
|
+ --segments;
|
8143
|
8203
|
|
8144
|
|
- #if ENABLED(DELTA) && ENABLED(USE_RAW_KINEMATICS)
|
|
8204
|
+ // Using "raw" coordinates saves 6 float subtractions
|
|
8205
|
+ // per segment, saving valuable CPU cycles
|
8145
|
8206
|
|
8146
|
|
- #define DELTA_E raw[E_AXIS]
|
8147
|
|
- #define DELTA_NEXT(ADDEND) LOOP_XYZE(i) raw[i] += ADDEND;
|
8148
|
|
- #define DELTA_IK() do { \
|
8149
|
|
- delta[A_AXIS] = DELTA_Z(1); \
|
8150
|
|
- delta[B_AXIS] = DELTA_Z(2); \
|
8151
|
|
- delta[C_AXIS] = DELTA_Z(3); \
|
8152
|
|
- } while(0)
|
|
8207
|
+ #if ENABLED(USE_RAW_KINEMATICS)
|
8153
|
8208
|
|
8154
|
8209
|
// Get the raw current position as starting point
|
8155
|
|
- float raw[ABC] = {
|
|
8210
|
+ float raw[XYZE] = {
|
8156
|
8211
|
RAW_CURRENT_POSITION(X_AXIS),
|
8157
|
8212
|
RAW_CURRENT_POSITION(Y_AXIS),
|
8158
|
|
- RAW_CURRENT_POSITION(Z_AXIS)
|
|
8213
|
+ RAW_CURRENT_POSITION(Z_AXIS),
|
|
8214
|
+ current_position[E_AXIS]
|
8159
|
8215
|
};
|
8160
|
8216
|
|
|
8217
|
+ #define DELTA_VAR raw
|
|
8218
|
+
|
|
8219
|
+ // Delta can inline its kinematics
|
|
8220
|
+ #if ENABLED(DELTA)
|
|
8221
|
+ #define DELTA_IK() DELTA_RAW_IK()
|
|
8222
|
+ #else
|
|
8223
|
+ #define DELTA_IK() inverse_kinematics(raw)
|
|
8224
|
+ #endif
|
|
8225
|
+
|
8161
|
8226
|
#else
|
8162
|
8227
|
|
8163
|
|
- #define DELTA_E logical[E_AXIS]
|
8164
|
|
- #define DELTA_NEXT(ADDEND) LOOP_XYZE(i) logical[i] += ADDEND;
|
|
8228
|
+ // Get the logical current position as starting point
|
|
8229
|
+ float logical[XYZE];
|
|
8230
|
+ memcpy(logical, current_position, sizeof(logical));
|
|
8231
|
+
|
|
8232
|
+ #define DELTA_VAR logical
|
8165
|
8233
|
|
|
8234
|
+ // Delta can inline its kinematics
|
8166
|
8235
|
#if ENABLED(DELTA)
|
8167
|
8236
|
#define DELTA_IK() DELTA_LOGICAL_IK()
|
8168
|
8237
|
#else
|
8169
|
8238
|
#define DELTA_IK() inverse_kinematics(logical)
|
8170
|
8239
|
#endif
|
8171
|
8240
|
|
8172
|
|
- // Get the logical current position as starting point
|
8173
|
|
- LOOP_XYZE(i) logical[i] = current_position[i];
|
8174
|
|
-
|
8175
|
8241
|
#endif
|
8176
|
8242
|
|
8177
|
8243
|
#if ENABLED(USE_DELTA_IK_INTERPOLATION)
|
8178
|
8244
|
|
8179
|
|
- // Get the starting delta for interpolation
|
8180
|
|
- if (segments >= 2) inverse_kinematics(logical);
|
|
8245
|
+ // Only interpolate XYZ. Advance E normally.
|
|
8246
|
+ #define DELTA_NEXT(ADDEND) LOOP_XYZ(i) DELTA_VAR[i] += ADDEND;
|
|
8247
|
+
|
|
8248
|
+ // Get the starting delta if interpolation is possible
|
|
8249
|
+ if (segments >= 2) DELTA_IK();
|
8181
|
8250
|
|
|
8251
|
+ // Loop using decrement
|
8182
|
8252
|
for (uint16_t s = segments + 1; --s;) {
|
8183
|
|
- if (s > 1) {
|
|
8253
|
+ // Are there at least 2 moves left?
|
|
8254
|
+ if (s >= 2) {
|
8184
|
8255
|
// Save the previous delta for interpolation
|
8185
|
8256
|
float prev_delta[ABC] = { delta[A_AXIS], delta[B_AXIS], delta[C_AXIS] };
|
8186
|
8257
|
|
8187
|
8258
|
// Get the delta 2 segments ahead (rather than the next)
|
8188
|
8259
|
DELTA_NEXT(segment_distance[i] + segment_distance[i]);
|
|
8260
|
+
|
|
8261
|
+ // Advance E normally
|
|
8262
|
+ DELTA_VAR[E_AXIS] += segment_distance[E_AXIS];
|
|
8263
|
+
|
|
8264
|
+ // Get the exact delta for the move after this
|
8189
|
8265
|
DELTA_IK();
|
8190
|
8266
|
|
8191
|
8267
|
// Move to the interpolated delta position first
|
|
@@ -8193,33 +8269,43 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
|
8193
|
8269
|
(prev_delta[A_AXIS] + delta[A_AXIS]) * 0.5,
|
8194
|
8270
|
(prev_delta[B_AXIS] + delta[B_AXIS]) * 0.5,
|
8195
|
8271
|
(prev_delta[C_AXIS] + delta[C_AXIS]) * 0.5,
|
8196
|
|
- logical[E_AXIS], _feedrate_mm_s, active_extruder
|
|
8272
|
+ DELTA_VAR[E_AXIS], _feedrate_mm_s, active_extruder
|
8197
|
8273
|
);
|
8198
|
8274
|
|
|
8275
|
+ // Advance E once more for the next move
|
|
8276
|
+ DELTA_VAR[E_AXIS] += segment_distance[E_AXIS];
|
|
8277
|
+
|
8199
|
8278
|
// Do an extra decrement of the loop
|
8200
|
8279
|
--s;
|
8201
|
8280
|
}
|
8202
|
8281
|
else {
|
8203
|
|
- // Get the last segment delta (only when segments is odd)
|
8204
|
|
- DELTA_NEXT(segment_distance[i])
|
|
8282
|
+ // Get the last segment delta. (Used when segments is odd)
|
|
8283
|
+ DELTA_NEXT(segment_distance[i]);
|
|
8284
|
+ DELTA_VAR[E_AXIS] += segment_distance[E_AXIS];
|
8205
|
8285
|
DELTA_IK();
|
8206
|
8286
|
}
|
8207
|
8287
|
|
8208
|
8288
|
// Move to the non-interpolated position
|
8209
|
|
- planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], DELTA_E, _feedrate_mm_s, active_extruder);
|
|
8289
|
+ planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], DELTA_VAR[E_AXIS], _feedrate_mm_s, active_extruder);
|
8210
|
8290
|
}
|
8211
|
8291
|
|
8212
|
8292
|
#else
|
8213
|
8293
|
|
|
8294
|
+ #define DELTA_NEXT(ADDEND) LOOP_XYZE(i) DELTA_VAR[i] += ADDEND;
|
|
8295
|
+
|
8214
|
8296
|
// For non-interpolated delta calculate every segment
|
8215
|
8297
|
for (uint16_t s = segments + 1; --s;) {
|
8216
|
|
- DELTA_NEXT(segment_distance[i])
|
|
8298
|
+ DELTA_NEXT(segment_distance[i]);
|
8217
|
8299
|
DELTA_IK();
|
8218
|
|
- planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], _feedrate_mm_s, active_extruder);
|
|
8300
|
+ planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], DELTA_VAR[E_AXIS], _feedrate_mm_s, active_extruder);
|
8219
|
8301
|
}
|
8220
|
8302
|
|
8221
|
8303
|
#endif
|
8222
|
8304
|
|
|
8305
|
+ // Since segment_distance is only approximate,
|
|
8306
|
+ // the final move must be to the exact destination.
|
|
8307
|
+ inverse_kinematics(ltarget);
|
|
8308
|
+ planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], ltarget[E_AXIS], _feedrate_mm_s, active_extruder);
|
8223
|
8309
|
return true;
|
8224
|
8310
|
}
|
8225
|
8311
|
|
|
@@ -8554,7 +8640,7 @@ void prepare_move_to_destination() {
|
8554
|
8640
|
cartes[Y_AXIS] = a_sin + b_sin + SCARA_OFFSET_Y; //theta+phi
|
8555
|
8641
|
|
8556
|
8642
|
/*
|
8557
|
|
- SERIAL_ECHOPAIR("Angle a=", a);
|
|
8643
|
+ SERIAL_ECHOPAIR("SCARA FK Angle a=", a);
|
8558
|
8644
|
SERIAL_ECHOPAIR(" b=", b);
|
8559
|
8645
|
SERIAL_ECHOPAIR(" a_sin=", a_sin);
|
8560
|
8646
|
SERIAL_ECHOPAIR(" a_cos=", a_cos);
|