Browse Source

Ubl delta fixes and improvements (#6941)

* Change all direct changes of ubl.state.active to
  set_bed_leveling_enabled() which handles apply/unapply
  leveling to maintain current_position consistency.

Fix invalidation of UBL mesh to invalid unreachable
  mesh points as well (delta corners).

Fix UBL_DELTA unapply_leveling logic and when
it gets applied, including fade_height changes.

Add optional M114 D for detailed position information,
disabled from compilation by default (M114_DETAIL).

* UBL_DELTA raw and inline kinematics

* UBL planner fall through fix

* consistent variable names

* Cleanup orphaned code and whitespace changes.
Use _O2.

* compile warnings cleanup

* Remove redundant #ifdef condition
oldmcg 7 years ago
parent
commit
9f295581f0
7 changed files with 380 additions and 156 deletions
  1. 2
    3
      Marlin/G26_Mesh_Validation_Tool.cpp
  2. 177
    30
      Marlin/Marlin_main.cpp
  3. 20
    18
      Marlin/planner.cpp
  4. 11
    5
      Marlin/ubl.cpp
  5. 2
    1
      Marlin/ubl.h
  6. 42
    19
      Marlin/ubl_G29.cpp
  7. 126
    80
      Marlin/ubl_motion.cpp

+ 2
- 3
Marlin/G26_Mesh_Validation_Tool.cpp View File

@@ -718,7 +718,7 @@
718 718
     /**
719 719
      * Wait until all parameters are verified before altering the state!
720 720
      */
721
-    state.active = !parser.seen('D');
721
+    set_bed_leveling_enabled(!parser.seen('D'));
722 722
 
723 723
     return UBL_OK;
724 724
   }
@@ -734,7 +734,7 @@
734 734
    * wait for them to get up to temperature.
735 735
    */
736 736
   bool unified_bed_leveling::turn_on_heaters() {
737
-    millis_t next;
737
+    millis_t next = millis() + 5000UL;
738 738
     #if HAS_TEMP_BED
739 739
       #if ENABLED(ULTRA_LCD)
740 740
         if (g26_bed_temp > 25) {
@@ -743,7 +743,6 @@
743 743
       #endif
744 744
           has_control_of_lcd_panel = true;
745 745
           thermalManager.setTargetBed(g26_bed_temp);
746
-          next = millis() + 5000UL;
747 746
           while (abs(thermalManager.degBed() - g26_bed_temp) > 3) {
748 747
             if (ubl_lcd_clicked()) return exit_from_g26();
749 748
             if (PENDING(millis(), next)) {

+ 177
- 30
Marlin/Marlin_main.cpp View File

@@ -699,7 +699,8 @@ void set_current_from_steppers_for_axis(const AxisEnum axis);
699 699
 #endif
700 700
 
701 701
 void tool_change(const uint8_t tmp_extruder, const float fr_mm_s=0.0, bool no_move=false);
702
-static void report_current_position();
702
+void report_current_position();
703
+void report_current_position_detail();
703 704
 
704 705
 #if ENABLED(DEBUG_LEVELING_FEATURE)
705 706
   void print_xyz(const char* prefix, const char* suffix, const float x, const float y, const float z) {
@@ -1536,14 +1537,21 @@ inline void set_destination_to_current() { COPY(destination, current_position);
1536 1537
       if (DEBUGGING(LEVELING)) DEBUG_POS("prepare_uninterpolated_move_to_destination", destination);
1537 1538
     #endif
1538 1539
 
1539
-    if ( current_position[X_AXIS] == destination[X_AXIS]
1540
-      && current_position[Y_AXIS] == destination[Y_AXIS]
1541
-      && current_position[Z_AXIS] == destination[Z_AXIS]
1542
-      && current_position[E_AXIS] == destination[E_AXIS]
1543
-    ) return;
1544
-
1545 1540
     refresh_cmd_timeout();
1546
-    planner.buffer_line_kinematic(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s), active_extruder);
1541
+
1542
+    #if UBL_DELTA
1543
+      // ubl segmented line will do z-only moves in single segment
1544
+      ubl.prepare_segmented_line_to(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s));
1545
+    #else
1546
+      if ( current_position[X_AXIS] == destination[X_AXIS]
1547
+        && current_position[Y_AXIS] == destination[Y_AXIS]
1548
+        && current_position[Z_AXIS] == destination[Z_AXIS]
1549
+        && current_position[E_AXIS] == destination[E_AXIS]
1550
+      ) return;
1551
+
1552
+      planner.buffer_line_kinematic(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s), active_extruder);
1553
+    #endif
1554
+    
1547 1555
     set_current_to_destination();
1548 1556
   }
1549 1557
 #endif // IS_KINEMATIC
@@ -2345,18 +2353,21 @@ static void clean_up_after_endstop_or_probe_move() {
2345 2353
         if (enabling) planner.unapply_leveling(current_position);
2346 2354
 
2347 2355
       #elif ENABLED(AUTO_BED_LEVELING_UBL)
2348
-
2349 2356
         #if PLANNER_LEVELING
2350
-
2351
-          if (!enable)   // leveling from on to off
2357
+          if (ubl.state.active) {                       // leveling from on to off
2358
+            // change unleveled current_position to physical current_position without moving steppers.
2352 2359
             planner.apply_leveling(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
2353
-          else
2354
-            planner.unapply_leveling(current_position);
2355
-
2360
+            ubl.state.active = false;                   // disable only AFTER calling apply_leveling
2361
+          }
2362
+          else {                                        // leveling from off to on
2363
+            ubl.state.active = true;                    // enable BEFORE calling unapply_leveling, otherwise ignored
2364
+            // change physical current_position to unleveled current_position without moving steppers.
2365
+            planner.unapply_leveling(current_position); 
2366
+          }
2367
+        #else
2368
+          ubl.state.active = enable;                    // just flip the bit, current_position will be wrong until next move.
2356 2369
         #endif
2357 2370
 
2358
-        ubl.state.active = enable;
2359
-
2360 2371
       #else // ABL
2361 2372
 
2362 2373
         #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
@@ -2384,17 +2395,34 @@ static void clean_up_after_endstop_or_probe_move() {
2384 2395
   #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
2385 2396
 
2386 2397
     void set_z_fade_height(const float zfh) {
2387
-      planner.z_fade_height = zfh;
2388
-      planner.inverse_z_fade_height = RECIPROCAL(zfh);
2389 2398
 
2390
-      if (leveling_is_active())
2391
-        set_current_from_steppers_for_axis(
2392
-          #if ABL_PLANAR
2393
-            ALL_AXES
2394
-          #else
2395
-            Z_AXIS
2396
-          #endif
2397
-        );
2399
+      #if ENABLED(AUTO_BED_LEVELING_UBL)
2400
+
2401
+        const bool level_active = leveling_is_active();
2402
+        if (level_active) {
2403
+          set_bed_leveling_enabled(false);  // turn off before changing fade height for proper apply/unapply leveling to maintain current_position
2404
+        }
2405
+        planner.z_fade_height = zfh;
2406
+        planner.inverse_z_fade_height = RECIPROCAL(zfh);
2407
+        if (level_active) {
2408
+          set_bed_leveling_enabled(true);  // turn back on after changing fade height
2409
+        }
2410
+
2411
+      #else
2412
+
2413
+        planner.z_fade_height = zfh;
2414
+        planner.inverse_z_fade_height = RECIPROCAL(zfh);
2415
+
2416
+        if (leveling_is_active()) {
2417
+          set_current_from_steppers_for_axis(
2418
+            #if ABL_PLANAR
2419
+              ALL_AXES
2420
+            #else
2421
+              Z_AXIS
2422
+            #endif
2423
+          );
2424
+        }
2425
+      #endif
2398 2426
     }
2399 2427
 
2400 2428
   #endif // LEVELING_FADE_HEIGHT
@@ -3656,6 +3684,7 @@ inline void gcode_G28(const bool always_home_all) {
3656 3684
   #if ENABLED(DELTA)
3657 3685
 
3658 3686
     home_delta();
3687
+    UNUSED(always_home_all);
3659 3688
 
3660 3689
   #else // NOT DELTA
3661 3690
 
@@ -7592,7 +7621,7 @@ inline void gcode_M92() {
7592 7621
 /**
7593 7622
  * Output the current position to serial
7594 7623
  */
7595
-static void report_current_position() {
7624
+void report_current_position() {
7596 7625
   SERIAL_PROTOCOLPGM("X:");
7597 7626
   SERIAL_PROTOCOL(current_position[X_AXIS]);
7598 7627
   SERIAL_PROTOCOLPGM(" Y:");
@@ -7611,10 +7640,119 @@ static void report_current_position() {
7611 7640
   #endif
7612 7641
 }
7613 7642
 
7643
+#ifdef M114_DETAIL
7644
+
7645
+  static const char axis_char[XYZE] = {'X','Y','Z','E'};
7646
+
7647
+  void report_xyze(const float pos[XYZE], uint8_t n = 4, uint8_t precision = 3) {
7648
+    char str[12];
7649
+    for(uint8_t i=0; i<n; i++) {
7650
+      SERIAL_CHAR(' ');
7651
+      SERIAL_CHAR(axis_char[i]);
7652
+      SERIAL_CHAR(':');
7653
+      SERIAL_PROTOCOL(dtostrf(pos[i],8,precision,str));
7654
+    }
7655
+    SERIAL_EOL;
7656
+  }
7657
+
7658
+  inline void report_xyz(const float pos[XYZ]) {
7659
+    report_xyze(pos,3);
7660
+  }
7661
+
7662
+  void report_current_position_detail() {
7663
+
7664
+    stepper.synchronize();
7665
+
7666
+    SERIAL_EOL;
7667
+    SERIAL_PROTOCOLPGM("Logical:");
7668
+    report_xyze(current_position);
7669
+
7670
+    SERIAL_PROTOCOLPGM("Raw:    ");
7671
+    const float raw[XYZ] = {
7672
+      RAW_X_POSITION(current_position[X_AXIS]),
7673
+      RAW_Y_POSITION(current_position[Y_AXIS]),
7674
+      RAW_Z_POSITION(current_position[Z_AXIS])
7675
+    };
7676
+    report_xyz(raw);
7677
+
7678
+    SERIAL_PROTOCOLPGM("Leveled:");
7679
+    float leveled[XYZ] = {
7680
+      current_position[X_AXIS],
7681
+      current_position[Y_AXIS],
7682
+      current_position[Z_AXIS]
7683
+    };
7684
+    planner.apply_leveling(leveled);
7685
+    report_xyz(leveled);
7686
+
7687
+    SERIAL_PROTOCOLPGM("UnLevel:");
7688
+    float unleveled[XYZ] = { leveled[X_AXIS], leveled[Y_AXIS], leveled[Z_AXIS] };
7689
+    planner.unapply_leveling(unleveled);
7690
+    report_xyz(unleveled);
7691
+
7692
+    #if IS_KINEMATIC
7693
+      #if IS_SCARA
7694
+        SERIAL_PROTOCOLPGM("ScaraK: ");
7695
+      #else
7696
+        SERIAL_PROTOCOLPGM("DeltaK: ");
7697
+      #endif
7698
+      inverse_kinematics(leveled);  // writes delta[]
7699
+      report_xyz(delta);
7700
+    #endif
7701
+
7702
+    SERIAL_PROTOCOLPGM("Stepper:");
7703
+    const float step_count[XYZE] = {
7704
+      (float)stepper.position(X_AXIS),
7705
+      (float)stepper.position(Y_AXIS),
7706
+      (float)stepper.position(Z_AXIS),
7707
+      (float)stepper.position(E_AXIS)
7708
+    };
7709
+    report_xyze(step_count,4,0);
7710
+
7711
+    #if IS_SCARA
7712
+      const float deg[XYZ] = {
7713
+        stepper.get_axis_position_degrees(A_AXIS),
7714
+        stepper.get_axis_position_degrees(B_AXIS)
7715
+      };
7716
+      SERIAL_PROTOCOLPGM("Degrees:");
7717
+      report_xyze(deg,2);
7718
+    #endif
7719
+
7720
+    SERIAL_PROTOCOLPGM("FromStp:");
7721
+    get_cartesian_from_steppers();  // writes cartes[XYZ] (with forward kinematics)
7722
+    const float from_steppers[XYZE] = {
7723
+      cartes[X_AXIS],
7724
+      cartes[Y_AXIS],
7725
+      cartes[Z_AXIS],
7726
+      stepper.get_axis_position_mm(E_AXIS)
7727
+    };
7728
+    report_xyze(from_steppers);
7729
+
7730
+    const float diff[XYZE] = {
7731
+      from_steppers[X_AXIS] - leveled[X_AXIS],
7732
+      from_steppers[Y_AXIS] - leveled[Y_AXIS],
7733
+      from_steppers[Z_AXIS] - leveled[Z_AXIS],
7734
+      from_steppers[E_AXIS] - current_position[E_AXIS]
7735
+    };
7736
+    SERIAL_PROTOCOLPGM("Differ: ");
7737
+    report_xyze(diff);
7738
+  }
7739
+#endif // M114_DETAIL
7740
+
7614 7741
 /**
7615 7742
  * M114: Output current position to serial port
7616 7743
  */
7617
-inline void gcode_M114() { stepper.synchronize(); report_current_position(); }
7744
+inline void gcode_M114() {
7745
+
7746
+  #ifdef M114_DETAIL
7747
+    if ( parser.seen('D') ) {
7748
+      report_current_position_detail();
7749
+      return;
7750
+    }
7751
+  #endif
7752
+
7753
+  stepper.synchronize();
7754
+  report_current_position();
7755
+  }
7618 7756
 
7619 7757
 /**
7620 7758
  * M115: Capabilities string
@@ -10804,6 +10942,15 @@ void ok_to_send() {
10804 10942
   /**
10805 10943
    * Constrain the given coordinates to the software endstops.
10806 10944
    */
10945
+
10946
+  // NOTE: This makes no sense for delta beds other than Z-axis.
10947
+  //       For delta the X/Y would need to be clamped at
10948
+  //       DELTA_PRINTABLE_RADIUS from center of bed, but delta
10949
+  //       now enforces is_position_reachable for X/Y regardless
10950
+  //       of HAS_SOFTWARE_ENDSTOPS, so that enforcement would be
10951
+  //       redundant here.  Probably should #ifdef out the X/Y
10952
+  //       axis clamps here for delta and just leave the Z clamp.
10953
+
10807 10954
   void clamp_to_software_endstops(float target[XYZ]) {
10808 10955
     if (!soft_endstops_enabled) return;
10809 10956
     #if ENABLED(MIN_SOFTWARE_ENDSTOPS)
@@ -11597,14 +11744,14 @@ void prepare_move_to_destination() {
11597 11744
   if (
11598 11745
     #if IS_KINEMATIC
11599 11746
       #if UBL_DELTA
11600
-        ubl.prepare_linear_move_to(destination, feedrate_mm_s)
11747
+        ubl.prepare_segmented_line_to(destination, feedrate_mm_s)
11601 11748
       #else
11602 11749
         prepare_kinematic_move_to(destination)
11603 11750
       #endif
11604 11751
     #elif ENABLED(DUAL_X_CARRIAGE)
11605 11752
       prepare_move_to_destination_dualx()
11606 11753
     #elif UBL_DELTA // will work for CARTESIAN too (smaller segments follow mesh more closely)
11607
-      ubl.prepare_linear_move_to(destination, feedrate_mm_s)
11754
+      ubl.prepare_segmented_line_to(destination, feedrate_mm_s)
11608 11755
     #else
11609 11756
       prepare_move_to_destination_cartesian()
11610 11757
     #endif

+ 20
- 18
Marlin/planner.cpp View File

@@ -535,7 +535,7 @@ void Planner::check_axes_activity() {
535 535
    */
536 536
   void Planner::apply_leveling(float &lx, float &ly, float &lz) {
537 537
 
538
-    #if ENABLED(AUTO_BED_LEVELING_UBL) && UBL_DELTA  // probably should also be enabled for UBL without UBL_DELTA
538
+    #if ENABLED(AUTO_BED_LEVELING_UBL)
539 539
       if (!ubl.state.active) return;
540 540
       #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
541 541
         // if z_fade_height enabled (nonzero) and raw_z above it, no leveling required
@@ -550,7 +550,7 @@ void Planner::check_axes_activity() {
550 550
       if (!abl_enabled) return;
551 551
     #endif
552 552
 
553
-    #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
553
+    #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) && DISABLED(AUTO_BED_LEVELING_UBL)
554 554
       static float z_fade_factor = 1.0, last_raw_lz = -999.0;
555 555
       if (z_fade_height) {
556 556
         const float raw_lz = RAW_Z_POSITION(lz);
@@ -599,36 +599,38 @@ void Planner::check_axes_activity() {
599 599
 
600 600
   void Planner::unapply_leveling(float logical[XYZ]) {
601 601
 
602
-    #if ENABLED(AUTO_BED_LEVELING_UBL) && UBL_DELTA
602
+    #if ENABLED(AUTO_BED_LEVELING_UBL)
603 603
 
604 604
       if (ubl.state.active) {
605 605
 
606
-        const float z_leveled = RAW_Z_POSITION(logical[Z_AXIS]),
607
-                    z_ublmesh = ubl.get_z_correction(logical[X_AXIS], logical[Y_AXIS]);
608
-              float z_unlevel = z_leveled - ubl.state.z_offset - z_ublmesh;
606
+        const float z_physical = RAW_Z_POSITION(logical[Z_AXIS]);
607
+        const float z_ublmesh  = ubl.get_z_correction(logical[X_AXIS], logical[Y_AXIS]);
608
+        const float z_virtual  = z_physical - ubl.state.z_offset - z_ublmesh;
609
+              float z_logical  = LOGICAL_Z_POSITION(z_virtual);
609 610
 
610 611
         #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
611 612
 
612
-          // for L=leveled, U=unleveled, M=mesh, O=offset, H=fade_height,
613
-          // Given L==U+O+M(1-U/H) (faded mesh correction formula for U<H)
614
-          //  then U==L-O-M(1-U/H)
615
-          //    so U==L-O-M+MU/H
616
-          //    so U-MU/H==L-O-M
617
-          //    so U(1-M/H)==L-O-M
618
-          //    so U==(L-O-M)/(1-M/H) for U<H
613
+          // for P=physical_z, L=logical_z, M=mesh_z, O=z_offset, H=fade_height,
614
+          // Given P=L+O+M(1-L/H) (faded mesh correction formula for L<H)
615
+          //  then L=P-O-M(1-L/H)
616
+          //    so L=P-O-M+ML/H
617
+          //    so L-ML/H=P-O-M
618
+          //    so L(1-M/H)=P-O-M
619
+          //    so L=(P-O-M)/(1-M/H) for L<H
619 620
 
620 621
           if (planner.z_fade_height) {
621
-            const float z_unfaded = z_unlevel / (1.0 - z_ublmesh * planner.inverse_z_fade_height);
622
-            if (z_unfaded < planner.z_fade_height)  // don't know until after compute
623
-              z_unlevel = z_unfaded;
622
+            if (z_logical < planner.z_fade_height )
623
+              z_logical = z_logical / (1.0 - (z_ublmesh * planner.inverse_z_fade_height));
624
+            if (z_logical >= planner.z_fade_height)
625
+              z_logical = LOGICAL_Z_POSITION(z_physical - ubl.state.z_offset);
624 626
           }
625 627
 
626 628
         #endif // ENABLE_LEVELING_FADE_HEIGHT
627 629
 
628
-        logical[Z_AXIS] = z_unlevel;
630
+        logical[Z_AXIS] = z_logical;
629 631
       }
630 632
 
631
-      return; // don't fall thru to HAS_ABL or other ENABLE_LEVELING_FADE_HEIGHT logic
633
+      return; // don't fall thru to other ENABLE_LEVELING_FADE_HEIGHT logic
632 634
 
633 635
     #endif
634 636
 

+ 11
- 5
Marlin/ubl.cpp View File

@@ -83,7 +83,7 @@
83 83
   }
84 84
 
85 85
   void unified_bed_leveling::reset() {
86
-    state.active = false;
86
+    set_bed_leveling_enabled(false);
87 87
     state.z_offset = 0;
88 88
     state.storage_slot = -1;
89 89
     #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
@@ -94,11 +94,17 @@
94 94
   }
95 95
 
96 96
   void unified_bed_leveling::invalidate() {
97
-    state.active = false;
97
+    set_bed_leveling_enabled(false);
98 98
     state.z_offset = 0;
99
-    for (int x = 0; x < GRID_MAX_POINTS_X; x++)
100
-      for (int y = 0; y < GRID_MAX_POINTS_Y; y++)
101
-        z_values[x][y] = NAN;
99
+    set_all_mesh_points_to_value(NAN);
100
+  }
101
+
102
+  void unified_bed_leveling::set_all_mesh_points_to_value(float value) {
103
+    for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) {
104
+      for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++) {
105
+        z_values[x][y] = value;
106
+      }
107
+    }
102 108
   }
103 109
 
104 110
   void unified_bed_leveling::display_map(const int map_type) {

+ 2
- 1
Marlin/ubl.h View File

@@ -154,6 +154,7 @@
154 154
       static mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType, const float&, const float&, const bool, unsigned int[16], bool);
155 155
       static void reset();
156 156
       static void invalidate();
157
+      static void set_all_mesh_points_to_value(float);
157 158
       static bool sanity_check();
158 159
 
159 160
       static void G29() _O0;                          // O0 for no optimization
@@ -385,7 +386,7 @@
385 386
       FORCE_INLINE static float mesh_index_to_xpos(const uint8_t i) { return pgm_read_float(&_mesh_index_to_xpos[i]); }
386 387
       FORCE_INLINE static float mesh_index_to_ypos(const uint8_t i) { return pgm_read_float(&_mesh_index_to_ypos[i]); }
387 388
 
388
-      static bool prepare_linear_move_to(const float ltarget[XYZE], const float &feedrate);
389
+      static bool prepare_segmented_line_to(const float ltarget[XYZE], const float &feedrate);
389 390
       static void line_to_destination_cartesian(const float &fr, uint8_t e);
390 391
 
391 392
   }; // class unified_bed_leveling

+ 42
- 19
Marlin/ubl_G29.cpp View File

@@ -30,6 +30,7 @@
30 30
   #include "configuration_store.h"
31 31
   #include "ultralcd.h"
32 32
   #include "stepper.h"
33
+  #include "planner.h"
33 34
   #include "gcode.h"
34 35
 
35 36
   #include <math.h>
@@ -48,6 +49,7 @@
48 49
   extern long babysteps_done;
49 50
   extern float probe_pt(const float &x, const float &y, bool, int);
50 51
   extern bool set_probe_deployed(bool);
52
+  extern void set_bed_leveling_enabled(bool);
51 53
 
52 54
   #define SIZE_OF_LITTLE_RAISE 1
53 55
   #define BIG_RAISE_NOT_NEEDED 0
@@ -325,15 +327,23 @@
325 327
     if (parser.seen('I')) {
326 328
       uint8_t cnt = 0;
327 329
       g29_repetition_cnt = parser.has_value() ? parser.value_int() : 1;
328
-      while (g29_repetition_cnt--) {
329
-        if (cnt > 20) { cnt = 0; idle(); }
330
-        const mesh_index_pair location = find_closest_mesh_point_of_type(REAL, g29_x_pos, g29_y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false);
331
-        if (location.x_index < 0) {
332
-          SERIAL_PROTOCOLLNPGM("Entire Mesh invalidated.\n");
333
-          break;            // No more invalid Mesh Points to populate
330
+      if (g29_repetition_cnt >= GRID_MAX_POINTS) {
331
+        set_all_mesh_points_to_value(NAN);
332
+      } else {
333
+        while (g29_repetition_cnt--) {
334
+          if (cnt > 20) { cnt = 0; idle(); }
335
+          const mesh_index_pair location = find_closest_mesh_point_of_type(REAL, g29_x_pos, g29_y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false);
336
+          if (location.x_index < 0) {
337
+            // No more REACHABLE mesh points to invalidate, so we ASSUME the user
338
+            // meant to invalidate the ENTIRE mesh, which cannot be done with
339
+            // find_closest_mesh_point loop which only returns REACHABLE points.
340
+            set_all_mesh_points_to_value(NAN);
341
+            SERIAL_PROTOCOLLNPGM("Entire Mesh invalidated.\n");
342
+            break;            // No more invalid Mesh Points to populate
343
+          }
344
+          z_values[location.x_index][location.y_index] = NAN;
345
+          cnt++;
334 346
         }
335
-        z_values[location.x_index][location.y_index] = NAN;
336
-        cnt++;
337 347
       }
338 348
       SERIAL_PROTOCOLLNPGM("Locations invalidated.\n");
339 349
     }
@@ -497,18 +507,26 @@
497 507
            *   - Specify a constant with the 'C' parameter.
498 508
            *   - Allow 'G29 P3' to choose a 'reasonable' constant.
499 509
            */
510
+
500 511
           if (g29_c_flag) {
501 512
             if (g29_repetition_cnt >= GRID_MAX_POINTS) {
502
-              for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) {
503
-                for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++) {
504
-                  z_values[x][y] = g29_constant;
505
-                }
506
-              }
513
+              set_all_mesh_points_to_value(g29_constant);
507 514
             }
508 515
             else {
509 516
               while (g29_repetition_cnt--) {  // this only populates reachable mesh points near
510 517
                 const mesh_index_pair location = find_closest_mesh_point_of_type(INVALID, g29_x_pos, g29_y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false);
511
-                if (location.x_index < 0) break; // No more reachable invalid Mesh Points to populate
518
+                if (location.x_index < 0) {
519
+                  // No more REACHABLE INVALID mesh points to populate, so we ASSUME
520
+                  // user meant to populate ALL INVALID mesh points to value
521
+                  for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) {
522
+                    for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++) {
523
+                      if ( isnan(z_values[x][y])) {
524
+                        z_values[x][y] = g29_constant;
525
+                      }
526
+                    }
527
+                  }
528
+                  break; // No more invalid Mesh Points to populate
529
+                }
512 530
                 z_values[location.x_index][location.y_index] = g29_constant;
513 531
               }
514 532
             }
@@ -999,12 +1017,15 @@
999 1017
 
1000 1018
       serialprintPGM(parser.seen('B') ? PSTR("Place shim & measure") : PSTR("Measure")); // TODO: Make translatable strings
1001 1019
 
1020
+      const float z_step = 0.01;                                        // existing behavior: 0.01mm per click, occasionally step
1021
+      //const float z_step = 1.0 / planner.axis_steps_per_mm[Z_AXIS];   // approx one step each click
1022
+
1002 1023
       while (ubl_lcd_clicked()) delay(50);             // wait for user to release encoder wheel
1003 1024
       delay(50);                                       // debounce
1004 1025
       while (!ubl_lcd_clicked()) {                     // we need the loop to move the nozzle based on the encoder wheel here!
1005 1026
         idle();
1006 1027
         if (encoder_diff) {
1007
-          do_blocking_move_to_z(current_position[Z_AXIS] + float(encoder_diff) / 100.0);
1028
+          do_blocking_move_to_z(current_position[Z_AXIS] + float(encoder_diff) * z_step);
1008 1029
           encoder_diff = 0;
1009 1030
         }
1010 1031
       }
@@ -1115,11 +1136,11 @@
1115 1136
         SERIAL_PROTOCOLLNPGM("?Can't activate and deactivate at the same time.\n");
1116 1137
         return UBL_ERR;
1117 1138
       }
1118
-      state.active = true;
1139
+      set_bed_leveling_enabled(true);
1119 1140
       report_state();
1120 1141
     }
1121 1142
     else if (parser.seen('D')) {
1122
-      state.active = false;
1143
+      set_bed_leveling_enabled(false);
1123 1144
       report_state();
1124 1145
     }
1125 1146
 
@@ -1158,7 +1179,7 @@
1158 1179
       return;
1159 1180
     }
1160 1181
     ubl_state_at_invocation = state.active;
1161
-    state.active = 0;
1182
+    set_bed_leveling_enabled(false);
1162 1183
   }
1163 1184
 
1164 1185
   void unified_bed_leveling::restore_ubl_active_state_and_leave() {
@@ -1168,7 +1189,7 @@
1168 1189
       lcd_quick_feedback();
1169 1190
       return;
1170 1191
     }
1171
-    state.active = ubl_state_at_invocation;
1192
+    set_bed_leveling_enabled(ubl_state_at_invocation);
1172 1193
   }
1173 1194
 
1174 1195
   /**
@@ -1695,6 +1716,8 @@
1695 1716
         SERIAL_EOL;
1696 1717
       }
1697 1718
     #endif
1719
+
1720
+    if (do_ubl_mesh_map) display_map(g29_map_type);
1698 1721
   }
1699 1722
 
1700 1723
   #if ENABLED(UBL_G29_P31)

+ 126
- 80
Marlin/ubl_motion.cpp View File

@@ -32,7 +32,25 @@
32 32
 
33 33
   extern float destination[XYZE];
34 34
   extern void set_current_to_destination();
35
-  extern float delta_segments_per_second;
35
+
36
+#if ENABLED(DELTA)
37
+
38
+  extern float delta[ABC],
39
+               endstop_adj[ABC];
40
+
41
+  extern float delta_radius,
42
+               delta_tower_angle_trim[2],
43
+               delta_tower[ABC][2],
44
+               delta_diagonal_rod,
45
+               delta_calibration_radius,
46
+               delta_diagonal_rod_2_tower[ABC],
47
+               delta_segments_per_second,
48
+               delta_clip_start_height;
49
+
50
+  extern float delta_safe_distance_from_top();
51
+
52
+#endif
53
+
36 54
 
37 55
   static void debug_echo_axis(const AxisEnum axis) {
38 56
     if (current_position[axis] == destination[axis])
@@ -470,51 +488,76 @@
470 488
     #endif
471 489
 
472 490
     // We don't want additional apply_leveling() performed by regular buffer_line or buffer_line_kinematic,
473
-    // so we call _buffer_line directly here.  Per-segmented leveling performed first.
491
+    // so we call _buffer_line directly here.  Per-segmented leveling and kinematics performed first.
474 492
 
475
-    static inline void ubl_buffer_line_segment(const float ltarget[XYZE], const float &fr_mm_s, const uint8_t extruder) {
493
+    inline void _O2 ubl_buffer_segment_raw( float rx, float ry, float rz, float le, float fr ) {
476 494
 
477
-      #if IS_KINEMATIC
495
+      #if ENABLED(DELTA)  // apply delta inverse_kinematics
478 496
 
479
-        inverse_kinematics(ltarget); // this writes delta[ABC] from ltarget[XYZ] but does not modify ltarget
480
-        float feedrate = fr_mm_s;
497
+        const float delta_A = rz + sqrt( delta_diagonal_rod_2_tower[A_AXIS]
498
+                                         - HYPOT2( delta_tower[A_AXIS][X_AXIS] - rx,
499
+                                                   delta_tower[A_AXIS][Y_AXIS] - ry ));
481 500
 
482
-        #if IS_SCARA // scale the feed rate from mm/s to degrees/s
483
-          float adiff = abs(delta[A_AXIS] - scara_oldA),
484
-                bdiff = abs(delta[B_AXIS] - scara_oldB);
485
-          scara_oldA = delta[A_AXIS];
486
-          scara_oldB = delta[B_AXIS];
487
-          feedrate = max(adiff, bdiff) * scara_feed_factor;
488
-        #endif
501
+        const float delta_B = rz + sqrt( delta_diagonal_rod_2_tower[B_AXIS]
502
+                                         - HYPOT2( delta_tower[B_AXIS][X_AXIS] - rx,
503
+                                                   delta_tower[B_AXIS][Y_AXIS] - ry ));
504
+
505
+        const float delta_C = rz + sqrt( delta_diagonal_rod_2_tower[C_AXIS]
506
+                                         - HYPOT2( delta_tower[C_AXIS][X_AXIS] - rx,
507
+                                                   delta_tower[C_AXIS][Y_AXIS] - ry ));
508
+
509
+        planner._buffer_line(delta_A, delta_B, delta_C, le, fr, active_extruder);
510
+
511
+      #elif IS_SCARA  // apply scara inverse_kinematics (should be changed to save raw->logical->raw)
512
+
513
+        const float lseg[XYZ] = { LOGICAL_X_POSITION(rx),
514
+                                  LOGICAL_Y_POSITION(ry),
515
+                                  LOGICAL_Z_POSITION(rz)
516
+                                };
517
+
518
+        inverse_kinematics(lseg); // this writes delta[ABC] from lseg[XYZ]
519
+                                  // should move the feedrate scaling to scara inverse_kinematics
489 520
 
490
-        planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], ltarget[E_AXIS], feedrate, extruder);
521
+        float adiff = abs(delta[A_AXIS] - scara_oldA),
522
+              bdiff = abs(delta[B_AXIS] - scara_oldB);
523
+        scara_oldA = delta[A_AXIS];
524
+        scara_oldB = delta[B_AXIS];
525
+        float s_feedrate = max(adiff, bdiff) * scara_feed_factor;
491 526
 
492
-      #else // cartesian
527
+        planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], le, s_feedrate, active_extruder);
493 528
 
494
-        planner._buffer_line(ltarget[X_AXIS], ltarget[Y_AXIS], ltarget[Z_AXIS], ltarget[E_AXIS], fr_mm_s, extruder);
529
+      #else // CARTESIAN
530
+
531
+        // Cartesian _buffer_line seems to take LOGICAL, not RAW coordinates
532
+
533
+        const float lx = LOGICAL_X_POSITION(rx),
534
+                    ly = LOGICAL_Y_POSITION(ry),
535
+                    lz = LOGICAL_Z_POSITION(rz);
536
+
537
+        planner._buffer_line(lx, ly, lz, le, fr, active_extruder);
495 538
 
496 539
       #endif
540
+
497 541
     }
498 542
 
543
+
499 544
     /**
500
-     * Prepare a linear move for DELTA/SCARA/CARTESIAN with UBL and FADE semantics.
545
+     * Prepare a segmented linear move for DELTA/SCARA/CARTESIAN with UBL and FADE semantics.
501 546
      * This calls planner._buffer_line multiple times for small incremental moves.
502
-     * Returns true if the caller did NOT update current_position, otherwise false.
547
+     * Returns true if did NOT move, false if moved (requires current_position update).
503 548
      */
504 549
 
505
-    static bool unified_bed_leveling::prepare_linear_move_to(const float ltarget[XYZE], const float &feedrate) {
550
+    bool _O2 unified_bed_leveling::prepare_segmented_line_to(const float ltarget[XYZE], const float &feedrate) {
506 551
 
507 552
       if (!position_is_reachable_xy(ltarget[X_AXIS], ltarget[Y_AXIS]))  // fail if moving outside reachable boundary
508 553
         return true; // did not move, so current_position still accurate
509 554
 
510
-      const float difference[XYZE] = {    // cartesian distances moved in XYZE
511
-                    ltarget[X_AXIS] - current_position[X_AXIS],
512
-                    ltarget[Y_AXIS] - current_position[Y_AXIS],
513
-                    ltarget[Z_AXIS] - current_position[Z_AXIS],
514
-                    ltarget[E_AXIS] - current_position[E_AXIS]
515
-                  };
555
+      const float tot_dx = ltarget[X_AXIS] - current_position[X_AXIS],
556
+                  tot_dy = ltarget[Y_AXIS] - current_position[Y_AXIS],
557
+                  tot_dz = ltarget[Z_AXIS] - current_position[Z_AXIS],
558
+                  tot_de = ltarget[E_AXIS] - current_position[E_AXIS];
516 559
 
517
-      const float cartesian_xy_mm = HYPOT(difference[X_AXIS], difference[Y_AXIS]);         // total horizontal xy distance
560
+      const float cartesian_xy_mm = HYPOT(tot_dx, tot_dy);  // total horizontal xy distance
518 561
 
519 562
       #if IS_KINEMATIC
520 563
         const float seconds = cartesian_xy_mm / feedrate;                                  // seconds to move xy distance at requested rate
@@ -534,16 +577,19 @@
534 577
         scara_oldB = stepper.get_axis_position_degrees(B_AXIS);
535 578
       #endif
536 579
 
537
-      const float segment_distance[XYZE] = {            // length for each segment
538
-                    difference[X_AXIS] * inv_segments,
539
-                    difference[Y_AXIS] * inv_segments,
540
-                    difference[Z_AXIS] * inv_segments,
541
-                    difference[E_AXIS] * inv_segments
542
-                  };
580
+      const float seg_dx = tot_dx * inv_segments,
581
+                  seg_dy = tot_dy * inv_segments,
582
+                  seg_dz = tot_dz * inv_segments,
583
+                  seg_de = tot_de * inv_segments;
543 584
 
544 585
       // Note that E segment distance could vary slightly as z mesh height
545 586
       // changes for each segment, but small enough to ignore.
546 587
 
588
+      float seg_rx = RAW_X_POSITION(current_position[X_AXIS]),
589
+            seg_ry = RAW_Y_POSITION(current_position[Y_AXIS]),
590
+            seg_rz = RAW_Z_POSITION(current_position[Z_AXIS]),
591
+            seg_le = current_position[E_AXIS];
592
+
547 593
       const bool above_fade_height = (
548 594
         #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
549 595
           planner.z_fade_height != 0 && planner.z_fade_height < RAW_Z_POSITION(ltarget[Z_AXIS])
@@ -558,21 +604,24 @@
558 604
 
559 605
         const float z_offset = state.active ? state.z_offset : 0.0;
560 606
 
561
-        float seg_dest[XYZE];                   // per-segment destination,
562
-        COPY_XYZE(seg_dest, current_position);  // starting from current position
607
+        do {
608
+
609
+          if (--segments) {     // not the last segment
610
+            seg_rx += seg_dx;
611
+            seg_ry += seg_dy;
612
+            seg_rz += seg_dz;
613
+            seg_le += seg_de;
614
+          } else {              // last segment, use exact destination
615
+            seg_rx = RAW_X_POSITION(ltarget[X_AXIS]);
616
+            seg_ry = RAW_Y_POSITION(ltarget[Y_AXIS]);
617
+            seg_rz = RAW_Z_POSITION(ltarget[Z_AXIS]);
618
+            seg_le = ltarget[E_AXIS];
619
+          }
563 620
 
564
-        while (--segments) {
565
-          LOOP_XYZE(i) seg_dest[i] += segment_distance[i];
566
-          float ztemp = seg_dest[Z_AXIS];
567
-          seg_dest[Z_AXIS] += z_offset;
568
-          ubl_buffer_line_segment(seg_dest, feedrate, active_extruder);
569
-          seg_dest[Z_AXIS] = ztemp;
570
-        }
621
+          ubl_buffer_segment_raw( seg_rx, seg_ry, seg_rz + z_offset, seg_le, feedrate );
622
+
623
+        } while (segments);
571 624
 
572
-        // Since repeated adding segment_distance accumulates small errors, final move to exact destination.
573
-        COPY_XYZE(seg_dest, ltarget);
574
-        seg_dest[Z_AXIS] += z_offset;
575
-        ubl_buffer_line_segment(seg_dest, feedrate, active_extruder);
576 625
         return false; // moved but did not set_current_to_destination();
577 626
       }
578 627
 
@@ -582,14 +631,11 @@
582 631
         const float fade_scaling_factor = fade_scaling_factor_for_z(ltarget[Z_AXIS]);
583 632
       #endif
584 633
 
585
-      float seg_dest[XYZE];  // per-segment destination, initialize to first segment
586
-      LOOP_XYZE(i) seg_dest[i] = current_position[i] + segment_distance[i];
587
-
588
-      const float &dx_seg = segment_distance[X_AXIS];  // alias for clarity
589
-      const float &dy_seg = segment_distance[Y_AXIS];
590
-
591
-      float rx = RAW_X_POSITION(seg_dest[X_AXIS]),  // assume raw vs logical coordinates shifted but not scaled.
592
-            ry = RAW_Y_POSITION(seg_dest[Y_AXIS]);
634
+      // increment to first segment destination
635
+      seg_rx += seg_dx;
636
+      seg_ry += seg_dy;
637
+      seg_rz += seg_dz;
638
+      seg_le += seg_de;
593 639
 
594 640
       for(;;) {  // for each mesh cell encountered during the move
595 641
 
@@ -600,20 +646,16 @@
600 646
         // in top of loop and again re-find same adjacent cell and use it, just less efficient
601 647
         // for mesh inset area.
602 648
 
603
-        int8_t cell_xi = (rx - (UBL_MESH_MIN_X)) * (1.0 / (MESH_X_DIST)),
604
-               cell_yi = (ry - (UBL_MESH_MIN_Y)) * (1.0 / (MESH_X_DIST));
649
+        int8_t cell_xi = (seg_rx - (UBL_MESH_MIN_X)) * (1.0 / (MESH_X_DIST)),
650
+               cell_yi = (seg_ry - (UBL_MESH_MIN_Y)) * (1.0 / (MESH_X_DIST));
605 651
 
606 652
         cell_xi = constrain(cell_xi, 0, (GRID_MAX_POINTS_X) - 1);
607 653
         cell_yi = constrain(cell_yi, 0, (GRID_MAX_POINTS_Y) - 1);
608 654
 
609
-        const float x0 = mesh_index_to_xpos(cell_xi),     // 64 byte table lookup avoids mul+add
610
-                    y0 = mesh_index_to_ypos(cell_yi),     // 64 byte table lookup avoids mul+add
611
-                    x1 = mesh_index_to_xpos(cell_xi + 1), // 64 byte table lookup avoids mul+add
612
-                    y1 = mesh_index_to_ypos(cell_yi + 1); // 64 byte table lookup avoids mul+add
655
+        const float x0 = mesh_index_to_xpos(cell_xi),   // 64 byte table lookup avoids mul+add
656
+                    y0 = mesh_index_to_ypos(cell_yi);
613 657
 
614
-        float cx = rx - x0,   // cell-relative x
615
-              cy = ry - y0,   // cell-relative y
616
-              z_x0y0 = z_values[cell_xi  ][cell_yi  ],  // z at lower left corner
658
+        float z_x0y0 = z_values[cell_xi  ][cell_yi  ],  // z at lower left corner
617 659
               z_x1y0 = z_values[cell_xi+1][cell_yi  ],  // z at upper left corner
618 660
               z_x0y1 = z_values[cell_xi  ][cell_yi+1],  // z at lower right corner
619 661
               z_x1y1 = z_values[cell_xi+1][cell_yi+1];  // z at upper right corner
@@ -623,15 +665,18 @@
623 665
         if (isnan(z_x0y1)) z_x0y1 = 0;              //   in order to avoid isnan tests per cell,
624 666
         if (isnan(z_x1y1)) z_x1y1 = 0;              //   thus guessing zero for undefined points
625 667
 
668
+        float cx = seg_rx - x0,   // cell-relative x and y
669
+              cy = seg_ry - y0;
670
+
626 671
         const float z_xmy0 = (z_x1y0 - z_x0y0) * (1.0 / (MESH_X_DIST)),   // z slope per x along y0 (lower left to lower right)
627 672
                     z_xmy1 = (z_x1y1 - z_x0y1) * (1.0 / (MESH_X_DIST));   // z slope per x along y1 (upper left to upper right)
628 673
 
629
-              float z_cxy0 = z_x0y0 + z_xmy0 * cx;            // z height along y0 at cx
674
+              float z_cxy0 = z_x0y0 + z_xmy0 * cx;            // z height along y0 at cx (changes for each cx in cell)
630 675
 
631 676
         const float z_cxy1 = z_x0y1 + z_xmy1 * cx,            // z height along y1 at cx
632 677
                     z_cxyd = z_cxy1 - z_cxy0;                 // z height difference along cx from y0 to y1
633 678
 
634
-              float z_cxym = z_cxyd * (1.0 / (MESH_Y_DIST));  // z slope per y along cx from y0 to y1
679
+              float z_cxym = z_cxyd * (1.0 / (MESH_Y_DIST));  // z slope per y along cx from y0 to y1 (changes for each cx in cell)
635 680
 
636 681
         //    float z_cxcy = z_cxy0 + z_cxym * cy;            // interpolated mesh z height along cx at cy (do inside the segment loop)
637 682
 
@@ -639,8 +684,8 @@
639 684
         // and the z_cxym slope will change, both as a function of cx within the cell, and
640 685
         // each change by a constant for fixed segment lengths.
641 686
 
642
-        const float z_sxy0 = z_xmy0 * dx_seg,                                     // per-segment adjustment to z_cxy0
643
-                    z_sxym = (z_xmy1 - z_xmy0) * (1.0 / (MESH_Y_DIST)) * dx_seg;  // per-segment adjustment to z_cxym
687
+        const float z_sxy0 = z_xmy0 * seg_dx,                                     // per-segment adjustment to z_cxy0
688
+                    z_sxym = (z_xmy1 - z_xmy0) * (1.0 / (MESH_Y_DIST)) * seg_dx;  // per-segment adjustment to z_cxym
644 689
 
645 690
         for(;;) {  // for all segments within this mesh cell
646 691
 
@@ -650,28 +695,29 @@
650 695
             z_cxcy *= fade_scaling_factor;          // apply fade factor to interpolated mesh height
651 696
           #endif
652 697
 
653
-          z_cxcy += state.z_offset;             // add fixed mesh offset from G29 Z
698
+          z_cxcy += state.z_offset;                 // add fixed mesh offset from G29 Z
654 699
 
655 700
           if (--segments == 0) {                    // if this is last segment, use ltarget for exact
656
-            COPY_XYZE(seg_dest, ltarget);
657
-            seg_dest[Z_AXIS] += z_cxcy;
658
-            ubl_buffer_line_segment(seg_dest, feedrate, active_extruder);
659
-            return false;   // did not set_current_to_destination()
701
+            seg_rx = RAW_X_POSITION(ltarget[X_AXIS]);
702
+            seg_ry = RAW_Y_POSITION(ltarget[Y_AXIS]);
703
+            seg_rz = RAW_Z_POSITION(ltarget[Z_AXIS]);
704
+            seg_le = ltarget[E_AXIS];
660 705
           }
661 706
 
662
-          const float z_orig = seg_dest[Z_AXIS];  // remember the pre-leveled segment z value
663
-          seg_dest[Z_AXIS] = z_orig + z_cxcy;     // adjust segment z height per mesh leveling
664
-          ubl_buffer_line_segment(seg_dest, feedrate, active_extruder);
665
-          seg_dest[Z_AXIS] = z_orig;              // restore pre-leveled z before incrementing
707
+          ubl_buffer_segment_raw( seg_rx, seg_ry, seg_rz + z_cxcy, seg_le, feedrate );
708
+
709
+          if (segments == 0 )                       // done with last segment
710
+            return false;                           // did not set_current_to_destination()
666 711
 
667
-          LOOP_XYZE(i) seg_dest[i] += segment_distance[i];  // adjust seg_dest for next segment
712
+          seg_rx += seg_dx;
713
+          seg_ry += seg_dy;
714
+          seg_rz += seg_dz;
715
+          seg_le += seg_de;
668 716
 
669
-          cx += dx_seg;
670
-          cy += dy_seg;
717
+          cx += seg_dx;
718
+          cy += seg_dy;
671 719
 
672 720
           if (!WITHIN(cx, 0, MESH_X_DIST) || !WITHIN(cy, 0, MESH_Y_DIST)) {  // done within this cell, break to next
673
-            rx = RAW_X_POSITION(seg_dest[X_AXIS]);
674
-            ry = RAW_Y_POSITION(seg_dest[Y_AXIS]);
675 721
             break;
676 722
           }
677 723
 

Loading…
Cancel
Save