From 1fbcbc05f6e48a444ab72b7ed3258da347baaf19 Mon Sep 17 00:00:00 2001
From: Roxy-3D <Roxy-3D@users.noreply.github.com>
Date: Mon, 15 May 2017 16:25:01 -0500
Subject: [PATCH] UBL no longer flout's the sacred GCode standard (#6745)

Also clean up ubl_motion.cpp debug info and fix declaration of cx & cy
---
 Marlin/G26_Mesh_Validation_Tool.cpp |  26 +++---
 Marlin/Marlin_main.cpp              |   2 +-
 Marlin/ubl_G29.cpp                  | 131 ++++++++++------------------
 Marlin/ubl_motion.cpp               |  37 ++------
 4 files changed, 63 insertions(+), 133 deletions(-)

diff --git a/Marlin/G26_Mesh_Validation_Tool.cpp b/Marlin/G26_Mesh_Validation_Tool.cpp
index 105c786906..b6cd3e1e92 100644
--- a/Marlin/G26_Mesh_Validation_Tool.cpp
+++ b/Marlin/G26_Mesh_Validation_Tool.cpp
@@ -88,17 +88,6 @@
    *
    *   L #  Layer       Layer height.  (Height of nozzle above bed)  If not specified .20mm will be used.
    *
-   *   Q #  Multiplier  Retraction Multiplier.  Normally not needed.  Retraction defaults to 1.0mm and
-   *                    un-retraction is at 1.2mm   These numbers will be scaled by the specified amount
-   *
-   *   M #  Random      Randomize the order that the circles are drawn on the bed.  The search for the closest
-   *                    undrawn cicle is still done.  But the distance to the location for each circle has a
-   *                    random number of the size specified added to it.  Specifying R50 will give an interesting
-   *                    deviation from the normal behaviour on a 10 x 10 Mesh.
-
-   *   N #  Nozzle      Used to control the size of nozzle diameter.  If not specified, a .4mm nozzle is assumed.
-   *                    'n' can be used instead if your host program does not appreciate you using 'N'.
-   *
    *   O #  Ooooze      How much your nozzle will Ooooze filament while getting in position to print.  This
    *                    is over kill, but using this parameter will let you get the very first 'circle' perfect
    *                    so you have a trophy to peel off of the bed and hang up to show how perfectly you have your
@@ -111,10 +100,20 @@
    *                    printing the Mesh.  You can carefully remove the spent filament with a needle nose
    *                    pliers while holding the LCD Click wheel in a depressed state.
    *
+   *   Q #  Multiplier  Retraction Multiplier.  Normally not needed.  Retraction defaults to 1.0mm and
+   *                    un-retraction is at 1.2mm   These numbers will be scaled by the specified amount
+   *
    *   R #  Repeat      Prints the number of patterns given as a parameter, starting at the current location.
    *                    If a parameter isn't given, every point will be printed unless G26 is interrupted.
    *                    This works the same way that the UBL G29 P4 R parameter works.
    *
+   *   S #  Nozzle      Used to control the size of nozzle diameter.  If not specified, a .4mm nozzle is assumed.
+   *
+   *   U #  Random      Randomize the order that the circles are drawn on the bed.  The search for the closest
+   *                    undrawn cicle is still done.  But the distance to the location for each circle has a
+   *                    random number of the size specified added to it.  Specifying S50 will give an interesting
+   *                    deviation from the normal behaviour on a 10 x 10 Mesh.
+   *
    *   X #  X Coord.    Specify the starting location of the drawing activity.
    *
    *   Y #  Y Coord.    Specify the starting location of the drawing activity.
@@ -686,7 +685,7 @@
       }
     }
 
-    if (code_seen('N') || code_seen('n')) { // Warning! Use of 'N' / lowercase flouts established standards.
+    if (code_seen('S')) { 
       nozzle = code_value_float();
       if (!WITHIN(nozzle, 0.1, 1.0)) {
         SERIAL_PROTOCOLLNPGM("?Specified nozzle size not plausible.");
@@ -728,9 +727,8 @@
       }
     }
 
-    if (code_seen('M')) { // Warning! Use of 'M' flouts established standards.
+    if (code_seen('U')) { 
       randomSeed(millis());
-      // This setting will persist for the next G26
       random_deviation = code_has_value() ? code_value_float() : 50.0;
     }
 
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 3269de46a5..70d5f36e6d 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -5761,7 +5761,7 @@ inline void gcode_M31() {
   /**
    * M32: Select file and start SD Print
    */
-  inline void gcode_M32() {
+  inline void gcode_M32() {  // Why is M32 allowed to flout the sacred GCode standard?
     if (card.sdprinting)
       stepper.synchronize();
 
diff --git a/Marlin/ubl_G29.cpp b/Marlin/ubl_G29.cpp
index 0259be69b6..629eaaae0a 100644
--- a/Marlin/ubl_G29.cpp
+++ b/Marlin/ubl_G29.cpp
@@ -116,8 +116,7 @@
    *                    invalidate.
    *
    *   J #   Grid   *   Perform a Grid Based Leveling of the current Mesh using a grid with n points on a side.
-   *
-   *   j  EEPROM Dump   This function probably goes away after debug is complete.
+   *                    Not specifying a grid size will invoke the 3-Point leveling function.
    *
    *   K #   Kompare    Kompare current Mesh with stored Mesh # replacing current Mesh with the result. This
    *                    command literally performs a diff between two Meshes.
@@ -264,8 +263,6 @@
    *                    at a later date. The GCode output can be saved and later replayed by the host software
    *                    to reconstruct the current mesh on another machine.
    *
-   *   T     3-Point    Perform a 3 Point Bed Leveling on the current Mesh
-   *
    *   U     Unlevel    Perform a probe of the outer perimeter to assist in physically leveling unlevel beds.
    *                    Only used for G29 P1 O U   It will speed up the probing of the edge of the bed.  This
    *                    is useful when the entire bed does not need to be probed because it will be adjusted.
@@ -276,12 +273,6 @@
    *
    *   Y #   *      *   Y Location for this line of commands
    *
-   *   Z     Zero   *   Probes to set the Z Height of the nozzle. The entire Mesh can be raised or lowered
-   *                    by just doing a G29 Z
-   *
-   *   Z #   Zero   *   The entire Mesh can be raised or lowered to conform with the specified difference.
-   *                    zprobe_zoffset is added to the calculation.
-   *
    *
    *   Release Notes:
    *   You MUST do M502, M500 to initialize the storage. Failure to do this will cause all
@@ -329,7 +320,7 @@
     }
 
     // Don't allow auto-leveling without homing first
-    if (!(code_seen('N') && code_value_bool()) && axis_unhomed_error()) // Warning! Use of 'N' flouts established standards.
+    if (axis_unhomed_error()) 
       home_all_axes();
 
     if (g29_parameter_parsing()) return; // abort if parsing the simple parameters causes a problem,
@@ -353,13 +344,16 @@
     }
 
     if (code_seen('Q')) {
-      const int test_pattern = code_has_value() ? code_value_int() : -1;
-      if (!WITHIN(test_pattern, 0, 2)) {
+      const int test_pattern = code_has_value() ? code_value_int() : -99;
+      if (!WITHIN(test_pattern, -1, 2)) {
         SERIAL_PROTOCOLLNPGM("Invalid test_pattern value. (0-2)\n");
         return;
       }
       SERIAL_PROTOCOLLNPGM("Loading test_pattern values.\n");
       switch (test_pattern) {
+        case -1:
+          g29_eeprom_dump();
+          break;
         case 0:
           for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) {   // Create a bowl shape - similar to
             for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++) { // a poorly calibrated Delta.
@@ -385,9 +379,33 @@
     }
 
     if (code_seen('J')) {
-      ubl.save_ubl_active_state_and_disable();
-      ubl.tilt_mesh_based_on_probed_grid(code_seen('O') || code_seen('M')); // Warning! Use of 'M' flouts established standards.
-      ubl.restore_ubl_active_state_and_leave();
+      if (grid_size!=0) {  // if not 0 it is a normal n x n grid being probed
+        ubl.save_ubl_active_state_and_disable();
+        ubl.tilt_mesh_based_on_probed_grid(code_seen('O')); 
+        ubl.restore_ubl_active_state_and_leave();
+      } else { // grid_size==0 which means a 3-Point leveling has been requested
+        float z1 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_1_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_1_Y), false, g29_verbose_level),
+              z2 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_2_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_2_Y), false, g29_verbose_level),
+              z3 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_3_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_3_Y), true, g29_verbose_level);
+
+        if ( isnan(z1) || isnan(z2) || isnan(z3)) {   // probe_pt will return NAN if unreachable
+          SERIAL_ERROR_START;
+          SERIAL_ERRORLNPGM("Attempt to probe off the bed.");
+          goto LEAVE;
+        }
+
+      //  We need to adjust z1, z2, z3 by the Mesh Height at these points. Just because they are non-zero doesn't mean
+      //  the Mesh is tilted!  (We need to compensate each probe point by what the Mesh says that location's height is)
+
+        ubl.save_ubl_active_state_and_disable();
+        z1 -= ubl.get_z_correction(LOGICAL_X_POSITION(UBL_PROBE_PT_1_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_1_Y)) /* + zprobe_zoffset */ ;
+        z2 -= ubl.get_z_correction(LOGICAL_X_POSITION(UBL_PROBE_PT_2_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_2_Y)) /* + zprobe_zoffset */ ;
+        z3 -= ubl.get_z_correction(LOGICAL_X_POSITION(UBL_PROBE_PT_3_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_3_Y)) /* + zprobe_zoffset */ ;
+
+        do_blocking_move_to_xy(0.5 * (UBL_MESH_MAX_X - (UBL_MESH_MIN_X)), 0.5 * (UBL_MESH_MAX_Y - (UBL_MESH_MIN_Y)));
+        ubl.tilt_mesh_based_on_3pts(z1, z2, z3);
+        ubl.restore_ubl_active_state_and_leave();
+      }
     }
 
     if (code_seen('P')) {
@@ -420,7 +438,7 @@
             SERIAL_PROTOCOLLNPGM(").\n");
           }
           ubl.probe_entire_mesh(x_pos + X_PROBE_OFFSET_FROM_EXTRUDER, y_pos + Y_PROBE_OFFSET_FROM_EXTRUDER,
-                            code_seen('O') || code_seen('M'), code_seen('E'), code_seen('U')); // Warning! Use of 'M' flouts established standards.
+                            code_seen('O'), code_seen('E'), code_seen('U')); 
           break;
 
         case 2: {
@@ -469,7 +487,7 @@
             return;
           }
 
-          manually_probe_remaining_mesh(x_pos, y_pos, height, card_thickness, code_seen('O') || code_seen('M')); // Warning! Use of 'M' flouts established standards.
+          manually_probe_remaining_mesh(x_pos, y_pos, height, card_thickness, code_seen('O'));
           SERIAL_PROTOCOLLNPGM("G29 P2 finished.");
         } break;
 
@@ -505,7 +523,7 @@
           //
           // Fine Tune (i.e., Edit) the Mesh
           //
-          fine_tune_mesh(x_pos, y_pos, code_seen('O') || code_seen('M')); // Warning! Use of 'M' flouts established standards.
+          fine_tune_mesh(x_pos, y_pos, code_seen('O')); 
           break;
 
         case 5: ubl.find_mean_mesh_height(); break;
@@ -515,43 +533,12 @@
 
     }
 
-    if (code_seen('T')) {
-
-      float z1 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_1_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_1_Y), false, g29_verbose_level),
-            z2 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_2_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_2_Y), false, g29_verbose_level),
-            z3 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_3_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_3_Y), true, g29_verbose_level);
-
-      if ( isnan(z1) || isnan(z2) || isnan(z3)) {   // probe_pt will return NAN if unreachable
-          SERIAL_ERROR_START;
-          SERIAL_ERRORLNPGM("Attempt to probe off the bed.");
-          goto LEAVE;
-      }
-
-      //  We need to adjust z1, z2, z3 by the Mesh Height at these points. Just because they are non-zero doesn't mean
-      //  the Mesh is tilted!  (We need to compensate each probe point by what the Mesh says that location's height is)
-
-      ubl.save_ubl_active_state_and_disable();
-      z1 -= ubl.get_z_correction(LOGICAL_X_POSITION(UBL_PROBE_PT_1_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_1_Y)) /* + zprobe_zoffset */ ;
-      z2 -= ubl.get_z_correction(LOGICAL_X_POSITION(UBL_PROBE_PT_2_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_2_Y)) /* + zprobe_zoffset */ ;
-      z3 -= ubl.get_z_correction(LOGICAL_X_POSITION(UBL_PROBE_PT_3_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_3_Y)) /* + zprobe_zoffset */ ;
-
-      do_blocking_move_to_xy(0.5 * (UBL_MESH_MAX_X - (UBL_MESH_MIN_X)), 0.5 * (UBL_MESH_MAX_Y - (UBL_MESH_MIN_Y)));
-      ubl.tilt_mesh_based_on_3pts(z1, z2, z3);
-      ubl.restore_ubl_active_state_and_leave();
-    }
-
     //
     // Much of the 'What?' command can be eliminated. But until we are fully debugged, it is
     // good to have the extra information. Soon... we prune this to just a few items
     //
     if (code_seen('W')) ubl.g29_what_command();
  
-    //
-    // When we are fully debugged, the EEPROM dump command will get deleted also. But
-    // right now, it is good to have the extra information. Soon... we prune this.
-    //
-    if (code_seen('j')) g29_eeprom_dump(); // Warning! Use of lowercase flouts established standards.
-
     //
     // When we are fully debugged, this may go away. But there are some valid
     // use cases for the users. So we can wait and see what to do with it.
@@ -614,9 +601,12 @@
       SERIAL_PROTOCOLLNPGM("Done.\n");
     }
 
-    if (code_seen('O') || code_seen('M')) // Warning! Use of 'M' flouts established standards.
+    if (code_seen('O'))
       ubl.display_map(code_has_value() ? code_value_int() : 0);
 
+    /*
+     * This code may not be needed...   Prepare for its removal...
+     *
     if (code_seen('Z')) {
       if (code_has_value())
         ubl.state.z_offset = code_value_float();   // do the simple case. Just lock in the specified value
@@ -669,6 +659,7 @@
         ubl.restore_ubl_active_state_and_leave();
       }
     }
+    */
 
     LEAVE:
 
@@ -1069,8 +1060,8 @@
     }
 
     if (code_seen('J')) {
-      grid_size = code_has_value() ? code_value_int() : 3;
-      if (!WITHIN(grid_size, 2, 9)) {
+      grid_size = code_has_value() ? code_value_int() : 0;
+      if (grid_size!=0 && !WITHIN(grid_size, 2, 9)) {
         SERIAL_PROTOCOLLNPGM("?Invalid grid size (J) specified (2-9).\n");
         err_flag = true;
       }
@@ -1126,43 +1117,9 @@
       SERIAL_PROTOCOLLNPGM("Invalid map type.\n");
       return UBL_ERR;
     }
-
-    // Check if a map type was specified
-    if (code_seen('M')) { // Warning! Use of 'M' flouts established standards.
-      map_type = code_has_value() ? code_value_int() : 0;
-      if (!WITHIN(map_type, 0, 1)) {
-        SERIAL_PROTOCOLLNPGM("Invalid map type.\n");
-        return UBL_ERR;
-      }
-    }
-
     return UBL_OK;
   }
 
-  /**
-   * This function goes away after G29 debug is complete. But for right now, it is a handy
-   * routine to dump binary data structures.
-   */
-  /*
-  void dump(char * const str, const float &f) {
-    char *ptr;
-
-    SERIAL_PROTOCOL(str);
-    SERIAL_PROTOCOL_F(f, 8);
-    SERIAL_PROTOCOLPGM("  ");
-    ptr = (char*)&f;
-    for (uint8_t i = 0; i < 4; i++)
-      SERIAL_PROTOCOLPAIR("  ", hex_byte(*ptr++));
-    SERIAL_PROTOCOLPAIR("  isnan()=", isnan(f));
-    SERIAL_PROTOCOLPAIR("  isinf()=", isinf(f));
-
-    if (f == -INFINITY)
-      SERIAL_PROTOCOLPGM("  Minus Infinity detected.");
-
-    SERIAL_EOL;
-  }
-  //*/
-
   static int ubl_state_at_invocation = 0,
              ubl_state_recursion_chk = 0;
 
diff --git a/Marlin/ubl_motion.cpp b/Marlin/ubl_motion.cpp
index 1d59e31b97..2cc4767dd5 100644
--- a/Marlin/ubl_motion.cpp
+++ b/Marlin/ubl_motion.cpp
@@ -55,12 +55,8 @@
                 dy = current_position[Y_AXIS] - destination[Y_AXIS],
                 xy_dist = HYPOT(dx, dy);
 
-    if (xy_dist == 0.0) {
-      return;
-      //SERIAL_ECHOPGM("   FPMM=");
-      //const float fpmm = de / xy_dist;
-      //SERIAL_PROTOCOL_F(fpmm, 6);
-    }
+    if (xy_dist == 0.0) 
+	    return;
     else {
       SERIAL_ECHOPGM("   fpmm=");
       const float fpmm = de / xy_dist;
@@ -276,16 +272,7 @@
          */
         if (y != start[Y_AXIS]) {
           if (!inf_normalized_flag) {
-
-            //on_axis_distance = y - start[Y_AXIS];
             on_axis_distance = use_x_dist ? x - start[X_AXIS] : y - start[Y_AXIS];
-
-            //on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : y - start[Y_AXIS];
-            //on_axis_distance = use_x_dist ? x - start[X_AXIS] : next_mesh_line_y - start[Y_AXIS];
-
-            //on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : y - start[Y_AXIS];
-            //on_axis_distance = use_x_dist ? x - start[X_AXIS] : next_mesh_line_y - start[Y_AXIS];
-
             e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
             z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
           }
@@ -350,13 +337,7 @@
          */
         if (x != start[X_AXIS]) {
           if (!inf_normalized_flag) {
-
-            //on_axis_distance = x - start[X_AXIS];
             on_axis_distance = use_x_dist ? x - start[X_AXIS] : y - start[Y_AXIS];
-
-            //on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : y - start[Y_AXIS];
-            //on_axis_distance = use_x_dist ? x - start[X_AXIS] : next_mesh_line_y - start[Y_AXIS];
-
             e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;  // is based on X or Y because this is a horizontal move
             z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
           }
@@ -613,20 +594,14 @@
         cell_xi = constrain(cell_xi, 0, (GRID_MAX_POINTS_X) - 1);
         cell_yi = constrain(cell_yi, 0, (GRID_MAX_POINTS_Y) - 1);
 
-        // float x0 = (UBL_MESH_MIN_X) + ((MESH_X_DIST) * cell_xi );         // lower left cell corner
-        // float y0 = (UBL_MESH_MIN_Y) + ((MESH_Y_DIST) * cell_yi );         // lower left cell corner
-        // float x1 = x0 + MESH_X_DIST;                                      // upper right cell corner
-        // float y1 = y0 + MESH_Y_DIST;                                      // upper right cell corner
-
         const float x0 = pgm_read_float(&(ubl.mesh_index_to_xpos[cell_xi  ])),  // 64 byte table lookup avoids mul+add
                     y0 = pgm_read_float(&(ubl.mesh_index_to_ypos[cell_yi  ])),  // 64 byte table lookup avoids mul+add
                     x1 = pgm_read_float(&(ubl.mesh_index_to_xpos[cell_xi+1])),  // 64 byte table lookup avoids mul+add
-                    y1 = pgm_read_float(&(ubl.mesh_index_to_ypos[cell_yi+1])),  // 64 byte table lookup avoids mul+add
+                    y1 = pgm_read_float(&(ubl.mesh_index_to_ypos[cell_yi+1]));  // 64 byte table lookup avoids mul+add
 
-                    cx = rx - x0,   // cell-relative x
-                    cy = ry - y0;   // cell-relative y
-
-        float z_x0y0 = ubl.z_values[cell_xi  ][cell_yi  ],  // z at lower left corner
+        float cx = rx - x0,   // cell-relative x
+              cy = ry - y0;   // cell-relative y
+              z_x0y0 = ubl.z_values[cell_xi  ][cell_yi  ],  // z at lower left corner
               z_x1y0 = ubl.z_values[cell_xi+1][cell_yi  ],  // z at upper left corner
               z_x0y1 = ubl.z_values[cell_xi  ][cell_yi+1],  // z at lower right corner
               z_x1y1 = ubl.z_values[cell_xi+1][cell_yi+1];  // z at upper right corner