Browse Source

Merge pull request #4868 from thinkyhead/rc_autumn_fixups

Cleanups for the Autumn release
Scott Lahteine 8 years ago
parent
commit
d0796fc54c
6 changed files with 511 additions and 389 deletions
  1. 208
    122
      Marlin/Marlin_main.cpp
  2. 8
    1
      Marlin/SanityCheck.h
  3. 1
    0
      Marlin/language.h
  4. 46
    25
      Marlin/planner.cpp
  5. 7
    1
      Marlin/planner.h
  6. 241
    240
      Marlin/stepper.cpp

+ 208
- 122
Marlin/Marlin_main.cpp View File

@@ -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);

+ 8
- 1
Marlin/SanityCheck.h View File

@@ -48,7 +48,7 @@
48 48
   #error "You are using an old Configuration_adv.h file, update it before building Marlin."
49 49
 #endif
50 50
 
51
- /**
51
+/**
52 52
  * Warnings for old configurations
53 53
  */
54 54
 #if WATCH_TEMP_PERIOD > 500
@@ -451,6 +451,13 @@
451 451
 #endif
452 452
 
453 453
 /**
454
+ * Homing Bump
455
+ */
456
+#if X_HOME_BUMP_MM < 0 || Y_HOME_BUMP_MM < 0 || Z_HOME_BUMP_MM < 0
457
+  #error "[XYZ]_HOME_BUMP_MM must be greater than or equal to 0."
458
+#endif
459
+
460
+/**
454 461
  * Make sure Z_SAFE_HOMING point is reachable
455 462
  */
456 463
 #if ENABLED(Z_SAFE_HOMING)

+ 1
- 0
Marlin/language.h View File

@@ -151,6 +151,7 @@
151 151
 #define MSG_ERR_M421_PARAMETERS             "M421 requires XYZ or IJZ parameters"
152 152
 #define MSG_ERR_MESH_XY                     "Mesh XY or IJ cannot be resolved"
153 153
 #define MSG_ERR_ARC_ARGS                    "G2/G3 bad parameters"
154
+#define MSG_ERR_PROTECTED_PIN               "Protected Pin"
154 155
 #define MSG_ERR_M428_TOO_FAR                "Too far from reference point"
155 156
 #define MSG_ERR_M303_DISABLED               "PIDTEMP disabled"
156 157
 #define MSG_M119_REPORT                     "Reporting endstop status"

+ 46
- 25
Marlin/planner.cpp View File

@@ -98,6 +98,10 @@ float Planner::min_feedrate_mm_s,
98 98
       Planner::max_e_jerk,
99 99
       Planner::min_travel_feedrate_mm_s;
100 100
 
101
+#if ENABLED(AUTO_BED_LEVELING_FEATURE)
102
+  bool Planner::abl_enabled = false; // Flag that auto bed leveling is enabled
103
+#endif
104
+
101 105
 #if ENABLED(AUTO_BED_LEVELING_LINEAR)
102 106
   matrix_3x3 Planner::bed_level_matrix; // Transform to compensate for bed level
103 107
 #endif
@@ -135,8 +139,8 @@ Planner::Planner() { init(); }
135 139
 
136 140
 void Planner::init() {
137 141
   block_buffer_head = block_buffer_tail = 0;
138
-  memset(position, 0, sizeof(position)); // clear position
139
-  LOOP_XYZE(i) previous_speed[i] = 0.0;
142
+  memset(position, 0, sizeof(position));
143
+  memset(previous_speed, 0, sizeof(previous_speed));
140 144
   previous_nominal_speed = 0.0;
141 145
   #if ENABLED(AUTO_BED_LEVELING_LINEAR)
142 146
     bed_level_matrix.set_to_identity();
@@ -524,6 +528,11 @@ void Planner::check_axes_activity() {
524 528
 #if PLANNER_LEVELING
525 529
 
526 530
   void Planner::apply_leveling(float &lx, float &ly, float &lz) {
531
+
532
+    #if ENABLED(AUTO_BED_LEVELING_FEATURE)
533
+      if (!abl_enabled) return;
534
+    #endif
535
+
527 536
     #if ENABLED(MESH_BED_LEVELING)
528 537
 
529 538
       if (mbl.active())
@@ -561,28 +570,34 @@ void Planner::check_axes_activity() {
561 570
     #endif
562 571
   }
563 572
 
564
-  void Planner::unapply_leveling(float &lx, float &ly, float &lz) {
573
+  void Planner::unapply_leveling(float logical[XYZ]) {
574
+
575
+    #if ENABLED(AUTO_BED_LEVELING_FEATURE)
576
+      if (!abl_enabled) return;
577
+    #endif
578
+
565 579
     #if ENABLED(MESH_BED_LEVELING)
566 580
 
567 581
       if (mbl.active())
568
-        lz -= mbl.get_z(RAW_X_POSITION(lx), RAW_Y_POSITION(ly));
582
+        logical[Z_AXIS] -= mbl.get_z(RAW_X_POSITION(logical[X_AXIS]), RAW_Y_POSITION(logical[Y_AXIS]));
569 583
 
570 584
     #elif ENABLED(AUTO_BED_LEVELING_LINEAR)
571 585
 
572 586
       matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix);
573 587
 
574
-      float dx = lx - (X_TILT_FULCRUM), dy = ly - (Y_TILT_FULCRUM), dz = lz;
588
+      float dx = RAW_X_POSITION(logical[X_AXIS]) - (X_TILT_FULCRUM),
589
+            dy = RAW_Y_POSITION(logical[Y_AXIS]) - (Y_TILT_FULCRUM),
590
+            dz = RAW_Z_POSITION(logical[Z_AXIS]);
575 591
 
576 592
       apply_rotation_xyz(inverse, dx, dy, dz);
577 593
 
578
-      lx = LOGICAL_X_POSITION(dx + X_TILT_FULCRUM);
579
-      ly = LOGICAL_Y_POSITION(dy + Y_TILT_FULCRUM);
580
-      lz = LOGICAL_Z_POSITION(dz);
594
+      logical[X_AXIS] = LOGICAL_X_POSITION(dx + X_TILT_FULCRUM);
595
+      logical[Y_AXIS] = LOGICAL_Y_POSITION(dy + Y_TILT_FULCRUM);
596
+      logical[Z_AXIS] = LOGICAL_Z_POSITION(dz);
581 597
 
582 598
     #elif ENABLED(AUTO_BED_LEVELING_NONLINEAR)
583 599
 
584
-      float tmp[XYZ] = { lx, ly, 0 };
585
-      lz -= nonlinear_z_offset(tmp);
600
+      logical[Z_AXIS] -= nonlinear_z_offset(logical);
586 601
 
587 602
     #endif
588 603
   }
@@ -625,30 +640,29 @@ void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, co
625 640
        dz = target[Z_AXIS] - position[Z_AXIS];
626 641
 
627 642
   /*
628
-  SERIAL_ECHO_START;
629
-  SERIAL_ECHOPGM("Planner ", x);
643
+  SERIAL_ECHOPAIR("  Planner FR:", fr_mm_s);
644
+  SERIAL_CHAR(' ');
630 645
   #if IS_KINEMATIC
631
-    SERIAL_ECHOPAIR("A:", x);
646
+    SERIAL_ECHOPAIR("A:", lx);
632 647
     SERIAL_ECHOPAIR(" (", dx);
633
-    SERIAL_ECHOPAIR(") B:", y);
648
+    SERIAL_ECHOPAIR(") B:", ly);
634 649
   #else
635
-    SERIAL_ECHOPAIR("X:", x);
650
+    SERIAL_ECHOPAIR("X:", lx);
636 651
     SERIAL_ECHOPAIR(" (", dx);
637
-    SERIAL_ECHOPAIR(") Y:", y);
652
+    SERIAL_ECHOPAIR(") Y:", ly);
638 653
   #endif
639 654
   SERIAL_ECHOPAIR(" (", dy);
640
-  #elif ENABLED(DELTA)
641
-    SERIAL_ECHOPAIR(") C:", z);
655
+  #if ENABLED(DELTA)
656
+    SERIAL_ECHOPAIR(") C:", lz);
642 657
   #else
643
-    SERIAL_ECHOPAIR(") Z:", z);
658
+    SERIAL_ECHOPAIR(") Z:", lz);
644 659
   #endif
645 660
   SERIAL_ECHOPAIR(" (", dz);
646 661
   SERIAL_ECHOLNPGM(")");
647 662
   //*/
648 663
 
649 664
   // DRYRUN ignores all temperature constraints and assures that the extruder is instantly satisfied
650
-  if (DEBUGGING(DRYRUN))
651
-    position[E_AXIS] = target[E_AXIS];
665
+  if (DEBUGGING(DRYRUN)) position[E_AXIS] = target[E_AXIS];
652 666
 
653 667
   long de = target[E_AXIS] - position[E_AXIS];
654 668
 
@@ -1131,7 +1145,7 @@ void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, co
1131 1145
   block->recalculate_flag = true; // Always calculate trapezoid for new block
1132 1146
 
1133 1147
   // Update previous path unit_vector and nominal speed
1134
-  LOOP_XYZE(i) previous_speed[i] = current_speed[i];
1148
+  memcpy(previous_speed, current_speed, sizeof(previous_speed));
1135 1149
   previous_nominal_speed = block->nominal_speed;
1136 1150
 
1137 1151
   #if ENABLED(LIN_ADVANCE)
@@ -1177,8 +1191,8 @@ void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, co
1177 1191
   // Move buffer head
1178 1192
   block_buffer_head = next_buffer_head;
1179 1193
 
1180
-  // Update position
1181
-  LOOP_XYZE(i) position[i] = target[i];
1194
+  // Update the position (only when a move was queued)
1195
+  memcpy(position, target, sizeof(position));
1182 1196
 
1183 1197
   recalculate();
1184 1198
 
@@ -1204,7 +1218,14 @@ void Planner::set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) {
1204 1218
   stepper.set_position(nx, ny, nz, ne);
1205 1219
   previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
1206 1220
 
1207
-  LOOP_XYZE(i) previous_speed[i] = 0.0;
1221
+  memset(previous_speed, 0, sizeof(previous_speed));
1222
+}
1223
+
1224
+/**
1225
+ * Sync from the stepper positions. (e.g., after an interrupted move)
1226
+ */
1227
+void Planner::sync_from_steppers() {
1228
+  LOOP_XYZE(i) position[i] = stepper.position((AxisEnum)i);
1208 1229
 }
1209 1230
 
1210 1231
 /**

+ 7
- 1
Marlin/planner.h View File

@@ -137,6 +137,7 @@ class Planner {
137 137
     static float min_travel_feedrate_mm_s;
138 138
 
139 139
     #if ENABLED(AUTO_BED_LEVELING_FEATURE)
140
+      static bool abl_enabled;            // Flag that bed leveling is enabled
140 141
       static matrix_3x3 bed_level_matrix; // Transform to compensate for bed level
141 142
     #endif
142 143
 
@@ -218,7 +219,7 @@ class Planner {
218 219
        * as it will be given to the planner and steppers.
219 220
        */
220 221
       static void apply_leveling(float &lx, float &ly, float &lz);
221
-      static void unapply_leveling(float &lx, float &ly, float &lz);
222
+      static void unapply_leveling(float logical[XYZ]);
222 223
 
223 224
     #endif
224 225
 
@@ -243,6 +244,11 @@ class Planner {
243 244
     static void set_position_mm(ARG_X, ARG_Y, ARG_Z, const float& e);
244 245
 
245 246
     /**
247
+     * Sync from the stepper positions. (e.g., after an interrupted move)
248
+     */
249
+    static void sync_from_steppers();
250
+
251
+    /**
246 252
      * Set the E position (mm) of the planner (and the E stepper)
247 253
      */
248 254
     static void set_e_position_mm(const float& e);

+ 241
- 240
Marlin/stepper.cpp View File

@@ -357,316 +357,314 @@ void Stepper::isr() {
357 357
     }
358 358
     else {
359 359
       OCR1A = 2000; // 1kHz.
360
+      return;
360 361
     }
361 362
   }
362 363
 
363
-  if (current_block) {
364
+  // Update endstops state, if enabled
365
+  if (endstops.enabled
366
+    #if HAS_BED_PROBE
367
+      || endstops.z_probe_enabled
368
+    #endif
369
+  ) endstops.update();
364 370
 
365
-    // Update endstops state, if enabled
366
-    if (endstops.enabled
367
-      #if HAS_BED_PROBE
368
-        || endstops.z_probe_enabled
369
-      #endif
370
-    ) endstops.update();
371
+  // Take multiple steps per interrupt (For high speed moves)
372
+  bool all_steps_done = false;
373
+  for (int8_t i = 0; i < step_loops; i++) {
374
+    #ifndef USBCON
375
+      customizedSerial.checkRx(); // Check for serial chars.
376
+    #endif
371 377
 
372
-    // Take multiple steps per interrupt (For high speed moves)
373
-    bool all_steps_done = false;
374
-    for (int8_t i = 0; i < step_loops; i++) {
375
-      #ifndef USBCON
376
-        customizedSerial.checkRx(); // Check for serial chars.
377
-      #endif
378
+    #if ENABLED(LIN_ADVANCE)
378 379
 
379
-      #if ENABLED(LIN_ADVANCE)
380
+      counter_E += current_block->steps[E_AXIS];
381
+      if (counter_E > 0) {
382
+        counter_E -= current_block->step_event_count;
383
+        #if DISABLED(MIXING_EXTRUDER)
384
+          // Don't step E here for mixing extruder
385
+          count_position[E_AXIS] += count_direction[E_AXIS];
386
+          motor_direction(E_AXIS) ? --e_steps[TOOL_E_INDEX] : ++e_steps[TOOL_E_INDEX];
387
+        #endif
388
+      }
380 389
 
381
-        counter_E += current_block->steps[E_AXIS];
382
-        if (counter_E > 0) {
383
-          counter_E -= current_block->step_event_count;
384
-          #if DISABLED(MIXING_EXTRUDER)
385
-            // Don't step E here for mixing extruder
386
-            count_position[E_AXIS] += count_direction[E_AXIS];
387
-            motor_direction(E_AXIS) ? --e_steps[TOOL_E_INDEX] : ++e_steps[TOOL_E_INDEX];
388
-          #endif
390
+      #if ENABLED(MIXING_EXTRUDER)
391
+        // Step mixing steppers proportionally
392
+        bool dir = motor_direction(E_AXIS);
393
+        MIXING_STEPPERS_LOOP(j) {
394
+          counter_m[j] += current_block->steps[E_AXIS];
395
+          if (counter_m[j] > 0) {
396
+            counter_m[j] -= current_block->mix_event_count[j];
397
+            dir ? --e_steps[j] : ++e_steps[j];
398
+          }
389 399
         }
400
+      #endif
390 401
 
402
+      if (current_block->use_advance_lead) {
403
+        int delta_adv_steps = (((long)extruder_advance_k * current_estep_rate[TOOL_E_INDEX]) >> 9) - current_adv_steps[TOOL_E_INDEX];
391 404
         #if ENABLED(MIXING_EXTRUDER)
392
-          // Step mixing steppers proportionally
393
-          bool dir = motor_direction(E_AXIS);
405
+          // Mixing extruders apply advance lead proportionally
394 406
           MIXING_STEPPERS_LOOP(j) {
395
-            counter_m[j] += current_block->steps[E_AXIS];
396
-            if (counter_m[j] > 0) {
397
-              counter_m[j] -= current_block->mix_event_count[j];
398
-              dir ? --e_steps[j] : ++e_steps[j];
399
-            }
407
+            int steps = delta_adv_steps * current_block->step_event_count / current_block->mix_event_count[j];
408
+            e_steps[j] += steps;
409
+            current_adv_steps[j] += steps;
400 410
           }
411
+        #else
412
+          // For most extruders, advance the single E stepper
413
+          e_steps[TOOL_E_INDEX] += delta_adv_steps;
414
+          current_adv_steps[TOOL_E_INDEX] += delta_adv_steps;
401 415
         #endif
416
+      }
402 417
 
403
-        if (current_block->use_advance_lead) {
404
-          int delta_adv_steps = (((long)extruder_advance_k * current_estep_rate[TOOL_E_INDEX]) >> 9) - current_adv_steps[TOOL_E_INDEX];
405
-          #if ENABLED(MIXING_EXTRUDER)
406
-            // Mixing extruders apply advance lead proportionally
407
-            MIXING_STEPPERS_LOOP(j) {
408
-              int steps = delta_adv_steps * current_block->step_event_count / current_block->mix_event_count[j];
409
-              e_steps[j] += steps;
410
-              current_adv_steps[j] += steps;
411
-            }
412
-          #else
413
-            // For most extruders, advance the single E stepper
414
-            e_steps[TOOL_E_INDEX] += delta_adv_steps;
415
-            current_adv_steps[TOOL_E_INDEX] += delta_adv_steps;
416
-          #endif
417
-        }
418
-
419
-      #elif ENABLED(ADVANCE)
418
+    #elif ENABLED(ADVANCE)
420 419
 
421
-        // Always count the unified E axis
422
-        counter_E += current_block->steps[E_AXIS];
423
-        if (counter_E > 0) {
424
-          counter_E -= current_block->step_event_count;
425
-          #if DISABLED(MIXING_EXTRUDER)
426
-            // Don't step E here for mixing extruder
427
-            motor_direction(E_AXIS) ? --e_steps[TOOL_E_INDEX] : ++e_steps[TOOL_E_INDEX];
428
-          #endif
429
-        }
420
+      // Always count the unified E axis
421
+      counter_E += current_block->steps[E_AXIS];
422
+      if (counter_E > 0) {
423
+        counter_E -= current_block->step_event_count;
424
+        #if DISABLED(MIXING_EXTRUDER)
425
+          // Don't step E here for mixing extruder
426
+          motor_direction(E_AXIS) ? --e_steps[TOOL_E_INDEX] : ++e_steps[TOOL_E_INDEX];
427
+        #endif
428
+      }
430 429
 
431
-        #if ENABLED(MIXING_EXTRUDER)
430
+      #if ENABLED(MIXING_EXTRUDER)
432 431
 
433
-          // Step mixing steppers proportionally
434
-          bool dir = motor_direction(E_AXIS);
435
-          MIXING_STEPPERS_LOOP(j) {
436
-            counter_m[j] += current_block->steps[E_AXIS];
437
-            if (counter_m[j] > 0) {
438
-              counter_m[j] -= current_block->mix_event_count[j];
439
-              dir ? --e_steps[j] : ++e_steps[j];
440
-            }
432
+        // Step mixing steppers proportionally
433
+        bool dir = motor_direction(E_AXIS);
434
+        MIXING_STEPPERS_LOOP(j) {
435
+          counter_m[j] += current_block->steps[E_AXIS];
436
+          if (counter_m[j] > 0) {
437
+            counter_m[j] -= current_block->mix_event_count[j];
438
+            dir ? --e_steps[j] : ++e_steps[j];
441 439
           }
440
+        }
442 441
 
443
-        #endif // MIXING_EXTRUDER
444
-
445
-      #endif // ADVANCE or LIN_ADVANCE
446
-
447
-      #define _COUNTER(AXIS) counter_## AXIS
448
-      #define _APPLY_STEP(AXIS) AXIS ##_APPLY_STEP
449
-      #define _INVERT_STEP_PIN(AXIS) INVERT_## AXIS ##_STEP_PIN
442
+      #endif // MIXING_EXTRUDER
450 443
 
451
-      // Advance the Bresenham counter; start a pulse if the axis needs a step
452
-      #define PULSE_START(AXIS) \
453
-        _COUNTER(AXIS) += current_block->steps[_AXIS(AXIS)]; \
454
-        if (_COUNTER(AXIS) > 0) { _APPLY_STEP(AXIS)(!_INVERT_STEP_PIN(AXIS),0); }
444
+    #endif // ADVANCE or LIN_ADVANCE
455 445
 
456
-      // Stop an active pulse, reset the Bresenham counter, update the position
457
-      #define PULSE_STOP(AXIS) \
458
-        if (_COUNTER(AXIS) > 0) { \
459
-          _COUNTER(AXIS) -= current_block->step_event_count; \
460
-          count_position[_AXIS(AXIS)] += count_direction[_AXIS(AXIS)]; \
461
-          _APPLY_STEP(AXIS)(_INVERT_STEP_PIN(AXIS),0); \
462
-        }
446
+    #define _COUNTER(AXIS) counter_## AXIS
447
+    #define _APPLY_STEP(AXIS) AXIS ##_APPLY_STEP
448
+    #define _INVERT_STEP_PIN(AXIS) INVERT_## AXIS ##_STEP_PIN
463 449
 
464
-      // If a minimum pulse time was specified get the CPU clock
465
-      #if MINIMUM_STEPPER_PULSE > 0
466
-        static uint32_t pulse_start;
467
-        pulse_start = TCNT0;
468
-      #endif
450
+    // Advance the Bresenham counter; start a pulse if the axis needs a step
451
+    #define PULSE_START(AXIS) \
452
+      _COUNTER(AXIS) += current_block->steps[_AXIS(AXIS)]; \
453
+      if (_COUNTER(AXIS) > 0) { _APPLY_STEP(AXIS)(!_INVERT_STEP_PIN(AXIS),0); }
469 454
 
470
-      #if HAS_X_STEP
471
-        PULSE_START(X);
472
-      #endif
473
-      #if HAS_Y_STEP
474
-        PULSE_START(Y);
475
-      #endif
476
-      #if HAS_Z_STEP
477
-        PULSE_START(Z);
478
-      #endif
455
+    // Stop an active pulse, reset the Bresenham counter, update the position
456
+    #define PULSE_STOP(AXIS) \
457
+      if (_COUNTER(AXIS) > 0) { \
458
+        _COUNTER(AXIS) -= current_block->step_event_count; \
459
+        count_position[_AXIS(AXIS)] += count_direction[_AXIS(AXIS)]; \
460
+        _APPLY_STEP(AXIS)(_INVERT_STEP_PIN(AXIS),0); \
461
+      }
479 462
 
480
-      // For non-advance use linear interpolation for E also
481
-      #if DISABLED(ADVANCE) && DISABLED(LIN_ADVANCE)
482
-        #if ENABLED(MIXING_EXTRUDER)
483
-          // Keep updating the single E axis
484
-          counter_E += current_block->steps[E_AXIS];
485
-          // Tick the counters used for this mix
486
-          MIXING_STEPPERS_LOOP(j) {
487
-            // Step mixing steppers (proportionally)
488
-            counter_m[j] += current_block->steps[E_AXIS];
489
-            // Step when the counter goes over zero
490
-            if (counter_m[j] > 0) En_STEP_WRITE(j, !INVERT_E_STEP_PIN);
491
-          }
492
-        #else // !MIXING_EXTRUDER
493
-          PULSE_START(E);
494
-        #endif
495
-      #endif // !ADVANCE && !LIN_ADVANCE
463
+    // If a minimum pulse time was specified get the CPU clock
464
+    #if MINIMUM_STEPPER_PULSE > 0
465
+      static uint32_t pulse_start;
466
+      pulse_start = TCNT0;
467
+    #endif
496 468
 
497
-      // For a minimum pulse time wait before stopping pulses
498
-      #if MINIMUM_STEPPER_PULSE > 0
499
-        #define CYCLES_EATEN_BY_CODE 10
500
-        while ((uint32_t)(TCNT0 - pulse_start) < (MINIMUM_STEPPER_PULSE * (F_CPU / 1000000UL)) - CYCLES_EATEN_BY_CODE) { /* nada */ }
501
-      #endif
469
+    #if HAS_X_STEP
470
+      PULSE_START(X);
471
+    #endif
472
+    #if HAS_Y_STEP
473
+      PULSE_START(Y);
474
+    #endif
475
+    #if HAS_Z_STEP
476
+      PULSE_START(Z);
477
+    #endif
502 478
 
503
-      #if HAS_X_STEP
504
-        PULSE_STOP(X);
505
-      #endif
506
-      #if HAS_Y_STEP
507
-        PULSE_STOP(Y);
508
-      #endif
509
-      #if HAS_Z_STEP
510
-        PULSE_STOP(Z);
479
+    // For non-advance use linear interpolation for E also
480
+    #if DISABLED(ADVANCE) && DISABLED(LIN_ADVANCE)
481
+      #if ENABLED(MIXING_EXTRUDER)
482
+        // Keep updating the single E axis
483
+        counter_E += current_block->steps[E_AXIS];
484
+        // Tick the counters used for this mix
485
+        MIXING_STEPPERS_LOOP(j) {
486
+          // Step mixing steppers (proportionally)
487
+          counter_m[j] += current_block->steps[E_AXIS];
488
+          // Step when the counter goes over zero
489
+          if (counter_m[j] > 0) En_STEP_WRITE(j, !INVERT_E_STEP_PIN);
490
+        }
491
+      #else // !MIXING_EXTRUDER
492
+        PULSE_START(E);
511 493
       #endif
494
+    #endif // !ADVANCE && !LIN_ADVANCE
512 495
 
513
-      #if DISABLED(ADVANCE) && DISABLED(LIN_ADVANCE)
514
-        #if ENABLED(MIXING_EXTRUDER)
515
-          // Always step the single E axis
516
-          if (counter_E > 0) {
517
-            counter_E -= current_block->step_event_count;
518
-            count_position[E_AXIS] += count_direction[E_AXIS];
519
-          }
520
-          MIXING_STEPPERS_LOOP(j) {
521
-            if (counter_m[j] > 0) {
522
-              counter_m[j] -= current_block->mix_event_count[j];
523
-              En_STEP_WRITE(j, INVERT_E_STEP_PIN);
524
-            }
525
-          }
526
-        #else // !MIXING_EXTRUDER
527
-          PULSE_STOP(E);
528
-        #endif
529
-      #endif // !ADVANCE && !LIN_ADVANCE
530
-
531
-      if (++step_events_completed >= current_block->step_event_count) {
532
-        all_steps_done = true;
533
-        break;
534
-      }
535
-    }
536
-
537
-    #if ENABLED(ADVANCE) || ENABLED(LIN_ADVANCE)
538
-      // If we have esteps to execute, fire the next advance_isr "now"
539
-      if (e_steps[TOOL_E_INDEX]) OCR0A = TCNT0 + 2;
496
+    // For a minimum pulse time wait before stopping pulses
497
+    #if MINIMUM_STEPPER_PULSE > 0
498
+      #define CYCLES_EATEN_BY_CODE 10
499
+      while ((uint32_t)(TCNT0 - pulse_start) < (MINIMUM_STEPPER_PULSE * (F_CPU / 1000000UL)) - CYCLES_EATEN_BY_CODE) { /* nada */ }
540 500
     #endif
541 501
 
542
-    // Calculate new timer value
543
-    uint16_t timer, step_rate;
544
-    if (step_events_completed <= (uint32_t)current_block->accelerate_until) {
502
+    #if HAS_X_STEP
503
+      PULSE_STOP(X);
504
+    #endif
505
+    #if HAS_Y_STEP
506
+      PULSE_STOP(Y);
507
+    #endif
508
+    #if HAS_Z_STEP
509
+      PULSE_STOP(Z);
510
+    #endif
545 511
 
546
-      MultiU24X32toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
547
-      acc_step_rate += current_block->initial_rate;
512
+    #if DISABLED(ADVANCE) && DISABLED(LIN_ADVANCE)
513
+      #if ENABLED(MIXING_EXTRUDER)
514
+        // Always step the single E axis
515
+        if (counter_E > 0) {
516
+          counter_E -= current_block->step_event_count;
517
+          count_position[E_AXIS] += count_direction[E_AXIS];
518
+        }
519
+        MIXING_STEPPERS_LOOP(j) {
520
+          if (counter_m[j] > 0) {
521
+            counter_m[j] -= current_block->mix_event_count[j];
522
+            En_STEP_WRITE(j, INVERT_E_STEP_PIN);
523
+          }
524
+        }
525
+      #else // !MIXING_EXTRUDER
526
+        PULSE_STOP(E);
527
+      #endif
528
+    #endif // !ADVANCE && !LIN_ADVANCE
548 529
 
549
-      // upper limit
550
-      NOMORE(acc_step_rate, current_block->nominal_rate);
530
+    if (++step_events_completed >= current_block->step_event_count) {
531
+      all_steps_done = true;
532
+      break;
533
+    }
534
+  }
551 535
 
552
-      // step_rate to timer interval
553
-      timer = calc_timer(acc_step_rate);
554
-      OCR1A = timer;
555
-      acceleration_time += timer;
536
+  #if ENABLED(ADVANCE) || ENABLED(LIN_ADVANCE)
537
+    // If we have esteps to execute, fire the next advance_isr "now"
538
+    if (e_steps[TOOL_E_INDEX]) OCR0A = TCNT0 + 2;
539
+  #endif
556 540
 
557
-      #if ENABLED(LIN_ADVANCE)
541
+  // Calculate new timer value
542
+  uint16_t timer, step_rate;
543
+  if (step_events_completed <= (uint32_t)current_block->accelerate_until) {
558 544
 
559
-        if (current_block->use_advance_lead)
560
-          current_estep_rate[TOOL_E_INDEX] = ((uint32_t)acc_step_rate * current_block->e_speed_multiplier8) >> 8;
545
+    MultiU24X32toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
546
+    acc_step_rate += current_block->initial_rate;
561 547
 
562
-        if (current_block->use_advance_lead) {
563
-          #if ENABLED(MIXING_EXTRUDER)
564
-            MIXING_STEPPERS_LOOP(j)
565
-              current_estep_rate[j] = ((uint32_t)acc_step_rate * current_block->e_speed_multiplier8 * current_block->step_event_count / current_block->mix_event_count[j]) >> 8;
566
-          #else
567
-            current_estep_rate[TOOL_E_INDEX] = ((uint32_t)acc_step_rate * current_block->e_speed_multiplier8) >> 8;
568
-          #endif
569
-        }
548
+    // upper limit
549
+    NOMORE(acc_step_rate, current_block->nominal_rate);
570 550
 
571
-      #elif ENABLED(ADVANCE)
551
+    // step_rate to timer interval
552
+    timer = calc_timer(acc_step_rate);
553
+    OCR1A = timer;
554
+    acceleration_time += timer;
572 555
 
573
-        advance += advance_rate * step_loops;
574
-        //NOLESS(advance, current_block->advance);
556
+    #if ENABLED(LIN_ADVANCE)
575 557
 
576
-        long advance_whole = advance >> 8,
577
-             advance_factor = advance_whole - old_advance;
558
+      if (current_block->use_advance_lead)
559
+        current_estep_rate[TOOL_E_INDEX] = ((uint32_t)acc_step_rate * current_block->e_speed_multiplier8) >> 8;
578 560
 
579
-        // Do E steps + advance steps
561
+      if (current_block->use_advance_lead) {
580 562
         #if ENABLED(MIXING_EXTRUDER)
581
-          // ...for mixing steppers proportionally
582 563
           MIXING_STEPPERS_LOOP(j)
583
-            e_steps[j] += advance_factor * current_block->step_event_count / current_block->mix_event_count[j];
564
+            current_estep_rate[j] = ((uint32_t)acc_step_rate * current_block->e_speed_multiplier8 * current_block->step_event_count / current_block->mix_event_count[j]) >> 8;
584 565
         #else
585
-          // ...for the active extruder
586
-          e_steps[TOOL_E_INDEX] += advance_factor;
566
+          current_estep_rate[TOOL_E_INDEX] = ((uint32_t)acc_step_rate * current_block->e_speed_multiplier8) >> 8;
587 567
         #endif
568
+      }
588 569
 
589
-        old_advance = advance_whole;
570
+    #elif ENABLED(ADVANCE)
590 571
 
591
-      #endif // ADVANCE or LIN_ADVANCE
572
+      advance += advance_rate * step_loops;
573
+      //NOLESS(advance, current_block->advance);
592 574
 
593
-      #if ENABLED(ADVANCE) || ENABLED(LIN_ADVANCE)
594
-        eISR_Rate = (timer >> 2) * step_loops / abs(e_steps[TOOL_E_INDEX]);
575
+      long advance_whole = advance >> 8,
576
+           advance_factor = advance_whole - old_advance;
577
+
578
+      // Do E steps + advance steps
579
+      #if ENABLED(MIXING_EXTRUDER)
580
+        // ...for mixing steppers proportionally
581
+        MIXING_STEPPERS_LOOP(j)
582
+          e_steps[j] += advance_factor * current_block->step_event_count / current_block->mix_event_count[j];
583
+      #else
584
+        // ...for the active extruder
585
+        e_steps[TOOL_E_INDEX] += advance_factor;
595 586
       #endif
596
-    }
597
-    else if (step_events_completed > (uint32_t)current_block->decelerate_after) {
598
-      MultiU24X32toH16(step_rate, deceleration_time, current_block->acceleration_rate);
599 587
 
600
-      if (step_rate < acc_step_rate) { // Still decelerating?
601
-        step_rate = acc_step_rate - step_rate;
602
-        NOLESS(step_rate, current_block->final_rate);
603
-      }
604
-      else
605
-        step_rate = current_block->final_rate;
588
+      old_advance = advance_whole;
606 589
 
607
-      // step_rate to timer interval
608
-      timer = calc_timer(step_rate);
609
-      OCR1A = timer;
610
-      deceleration_time += timer;
590
+    #endif // ADVANCE or LIN_ADVANCE
611 591
 
612
-      #if ENABLED(LIN_ADVANCE)
613
-
614
-        if (current_block->use_advance_lead) {
615
-          #if ENABLED(MIXING_EXTRUDER)
616
-            MIXING_STEPPERS_LOOP(j)
617
-              current_estep_rate[j] = ((uint32_t)step_rate * current_block->e_speed_multiplier8 * current_block->step_event_count / current_block->mix_event_count[j]) >> 8;
618
-          #else
619
-            current_estep_rate[TOOL_E_INDEX] = ((uint32_t)step_rate * current_block->e_speed_multiplier8) >> 8;
620
-          #endif
621
-        }
592
+    #if ENABLED(ADVANCE) || ENABLED(LIN_ADVANCE)
593
+      eISR_Rate = (timer >> 2) * step_loops / abs(e_steps[TOOL_E_INDEX]);
594
+    #endif
595
+  }
596
+  else if (step_events_completed > (uint32_t)current_block->decelerate_after) {
597
+    MultiU24X32toH16(step_rate, deceleration_time, current_block->acceleration_rate);
622 598
 
623
-      #elif ENABLED(ADVANCE)
599
+    if (step_rate < acc_step_rate) { // Still decelerating?
600
+      step_rate = acc_step_rate - step_rate;
601
+      NOLESS(step_rate, current_block->final_rate);
602
+    }
603
+    else
604
+      step_rate = current_block->final_rate;
624 605
 
625
-        advance -= advance_rate * step_loops;
626
-        NOLESS(advance, final_advance);
606
+    // step_rate to timer interval
607
+    timer = calc_timer(step_rate);
608
+    OCR1A = timer;
609
+    deceleration_time += timer;
627 610
 
628
-        // Do E steps + advance steps
629
-        long advance_whole = advance >> 8,
630
-             advance_factor = advance_whole - old_advance;
611
+    #if ENABLED(LIN_ADVANCE)
631 612
 
613
+      if (current_block->use_advance_lead) {
632 614
         #if ENABLED(MIXING_EXTRUDER)
633 615
           MIXING_STEPPERS_LOOP(j)
634
-            e_steps[j] += advance_factor * current_block->step_event_count / current_block->mix_event_count[j];
616
+            current_estep_rate[j] = ((uint32_t)step_rate * current_block->e_speed_multiplier8 * current_block->step_event_count / current_block->mix_event_count[j]) >> 8;
635 617
         #else
636
-          e_steps[TOOL_E_INDEX] += advance_factor;
618
+          current_estep_rate[TOOL_E_INDEX] = ((uint32_t)step_rate * current_block->e_speed_multiplier8) >> 8;
637 619
         #endif
620
+      }
621
+
622
+    #elif ENABLED(ADVANCE)
638 623
 
639
-        old_advance = advance_whole;
624
+      advance -= advance_rate * step_loops;
625
+      NOLESS(advance, final_advance);
640 626
 
641
-      #endif // ADVANCE or LIN_ADVANCE
627
+      // Do E steps + advance steps
628
+      long advance_whole = advance >> 8,
629
+           advance_factor = advance_whole - old_advance;
642 630
 
643
-      #if ENABLED(ADVANCE) || ENABLED(LIN_ADVANCE)
644
-        eISR_Rate = (timer >> 2) * step_loops / abs(e_steps[TOOL_E_INDEX]);
631
+      #if ENABLED(MIXING_EXTRUDER)
632
+        MIXING_STEPPERS_LOOP(j)
633
+          e_steps[j] += advance_factor * current_block->step_event_count / current_block->mix_event_count[j];
634
+      #else
635
+        e_steps[TOOL_E_INDEX] += advance_factor;
645 636
       #endif
646
-    }
647
-    else {
648 637
 
649
-      #if ENABLED(LIN_ADVANCE)
638
+      old_advance = advance_whole;
639
+
640
+    #endif // ADVANCE or LIN_ADVANCE
650 641
 
651
-        if (current_block->use_advance_lead)
652
-          current_estep_rate[TOOL_E_INDEX] = final_estep_rate;
642
+    #if ENABLED(ADVANCE) || ENABLED(LIN_ADVANCE)
643
+      eISR_Rate = (timer >> 2) * step_loops / abs(e_steps[TOOL_E_INDEX]);
644
+    #endif
645
+  }
646
+  else {
653 647
 
654
-        eISR_Rate = (OCR1A_nominal >> 2) * step_loops_nominal / abs(e_steps[TOOL_E_INDEX]);
648
+    #if ENABLED(LIN_ADVANCE)
655 649
 
656
-      #endif
650
+      if (current_block->use_advance_lead)
651
+        current_estep_rate[TOOL_E_INDEX] = final_estep_rate;
657 652
 
658
-      OCR1A = OCR1A_nominal;
659
-      // ensure we're running at the correct step rate, even if we just came off an acceleration
660
-      step_loops = step_loops_nominal;
661
-    }
653
+      eISR_Rate = (OCR1A_nominal >> 2) * step_loops_nominal / abs(e_steps[TOOL_E_INDEX]);
662 654
 
663
-    NOLESS(OCR1A, TCNT1 + 16);
655
+    #endif
664 656
 
665
-    // If current block is finished, reset pointer
666
-    if (all_steps_done) {
667
-      current_block = NULL;
668
-      planner.discard_current_block();
669
-    }
657
+    OCR1A = OCR1A_nominal;
658
+    // ensure we're running at the correct step rate, even if we just came off an acceleration
659
+    step_loops = step_loops_nominal;
660
+  }
661
+
662
+  NOLESS(OCR1A, TCNT1 + 16);
663
+
664
+  // If current block is finished, reset pointer
665
+  if (all_steps_done) {
666
+    current_block = NULL;
667
+    planner.discard_current_block();
670 668
   }
671 669
 }
672 670
 
@@ -936,6 +934,9 @@ void Stepper::synchronize() { while (planner.blocks_queued()) idle(); }
936 934
  * derive the current XYZ position later on.
937 935
  */
938 936
 void Stepper::set_position(const long& x, const long& y, const long& z, const long& e) {
937
+
938
+  synchronize(); // Bad to set stepper counts in the middle of a move
939
+
939 940
   CRITICAL_SECTION_START;
940 941
 
941 942
   #if ENABLED(COREXY)

Loading…
Cancel
Save