src/modules/tools/zprobe/CartGridStrategy.cpp

   1 /*
   2  This code is derived from (and mostly copied from) Johann Rocholls code at https://github.com/jcrocholl/Marlin/blob/deltabot/Marlin/Marlin_main.cpp
   3  license is the same as his code.
   4
   5     Summary
   6     -------
   7     Probes grid_size points in X and Y (total probes grid_size * grid_size) and stores the relative offsets from the 0,0 Z height
   8     When enabled every move will calculate the Z offset based on interpolating the height offset within the grids nearest 4 points.
   9
  10     Configuration
  11     -------------
  12     The strategy must be enabled in the config as well as zprobe.
  13
  14       leveling-strategy.rectangular-grid.enable         true
  15
  16     The size of the grid can be set with...
  17
  18       leveling-strategy.rectangular-grid.grid_x_size        7
  19       leveling-strategy.rectangular-grid.grid_y_size        7
  20
  21       this is the X and Y size of the grid, it must be an odd number, the default is 7 which is 49 probe points
  22
  23     The width and length of the rectangle that is probed is set with...
  24
  25       leveling-strategy.rectangular-grid.x_size       100
  26       leveling-strategy.rectangular-grid.y_size       90
  27
  28    Optionally probe offsets from the nozzle or tool head can be defined with...
  29
  30       leveling-strategy.rectangular-grid.probe_offsets  0,0,0  # probe offsetrs x,y,z
  31
  32       they may also be set with M565 X0 Y0 Z0
  33
  34     If the saved grid is to be loaded on boot then this must be set in the config...
  35
  36       leveling-strategy.rectangular-grid.save        true
  37
  38       Then when M500 is issued it will save M375 which will cause the grid to be loaded on boot. The default is to not autoload the grid on boot
  39
  40     Optionally an initial_height can be set that tell the intial probe where to stop the fast decent before it probes, this should be around 5-10mm above the bed
  41       leveling-strategy.rectangular-grid.initial_height  10
  42
  43
  44     Usage
  45     -----
  46     G29 test probes a rectangle which defaults to the width and height, can be overidden with Xnnn and Ynnn
  47
  48     G31 probes the grid and turns the compensation on, this will remain in effect until reset or M561/M370
  49         optional parameters {{Xn}} {{Yn}} sets the size for this rectangular probe, which gets saved with M375
  50
  51     M370 clears the grid and turns off compensation
  52     M374 Save grid to /sd/cartesian.grid
  53     M374.1 delete /sd/cartesian.grid
  54     M375 Load the grid from /sd/cartesian.grid and enable compensation
  55     M375.1 display the current grid
  56     M561 clears the grid and turns off compensation
  57     M565 defines the probe offsets from the nozzle or tool head
  58
  59
  60     M500 saves the probe points
  61     M503 displays the current settings
  62 */
  63
  64 #include "CartGridStrategy.h"
  65
  66 #include "Kernel.h"
  67 #include "Config.h"
  68 #include "Robot.h"
  69 #include "StreamOutputPool.h"
  70 #include "Gcode.h"
  71 #include "checksumm.h"
  72 #include "ConfigValue.h"
  73 #include "PublicDataRequest.h"
  74 #include "PublicData.h"
  75 #include "Conveyor.h"
  76 #include "ZProbe.h"
  77 #include "nuts_bolts.h"
  78 #include "utils.h"
  79 #include "platform_memory.h"
  80
  81 #include <string>
  82 #include <algorithm>
  83 #include <cstdlib>
  84 #include <cmath>
  85 #include <fastmath.h>
  86
  87 #define grid_size_checksum           CHECKSUM("size")
  88 #define grid_x_size_checksum         CHECKSUM("grid_x_size")
  89 #define grid_y_size_checksum         CHECKSUM("grid_y_size")
  90 #define tolerance_checksum           CHECKSUM("tolerance")
  91 #define save_checksum                CHECKSUM("save")
  92 #define probe_offsets_checksum       CHECKSUM("probe_offsets")
  93 #define initial_height_checksum      CHECKSUM("initial_height")
  94 #define x_size_checksum              CHECKSUM("x_size")
  95 #define y_size_checksum              CHECKSUM("y_size")
  96 #define do_home_checksum             CHECKSUM("do_home")
  97
  98 #define GRIDFILE "/sd/cartesian.grid"
  99 #define GRIDFILE_NM "/sd/cartesian_nm.grid"
 100
 101 CartGridStrategy::CartGridStrategy(ZProbe *zprobe) : LevelingStrategy(zprobe)
 102 {
 103     grid = nullptr;
 104 }
 105
 106 CartGridStrategy::~CartGridStrategy()
 107 {
 108     if(grid != nullptr) AHB0.dealloc(grid);
 109 }
 110
 111 bool CartGridStrategy::handleConfig()
 112 {
 113
 114     uint8_t grid_size = THEKERNEL->config->value(leveling_strategy_checksum, cart_grid_leveling_strategy_checksum, grid_size_checksum)->by_default(7)->as_number();
 115     current_grid_x_size = configured_grid_x_size = THEKERNEL->config->value(leveling_strategy_checksum, cart_grid_leveling_strategy_checksum, grid_x_size_checksum)->by_default(grid_size)->as_number();
 116     current_grid_y_size = configured_grid_y_size = THEKERNEL->config->value(leveling_strategy_checksum, cart_grid_leveling_strategy_checksum, grid_y_size_checksum)->by_default(grid_size)->as_number();
 117     tolerance = THEKERNEL->config->value(leveling_strategy_checksum, cart_grid_leveling_strategy_checksum, tolerance_checksum)->by_default(0.03F)->as_number();
 118     save = THEKERNEL->config->value(leveling_strategy_checksum, cart_grid_leveling_strategy_checksum, save_checksum)->by_default(false)->as_bool();
 119     do_home = THEKERNEL->config->value(leveling_strategy_checksum, cart_grid_leveling_strategy_checksum, do_home_checksum)->by_default(true)->as_bool();
 120
 121     x_size = THEKERNEL->config->value(leveling_strategy_checksum, cart_grid_leveling_strategy_checksum, x_size_checksum)->by_default(0.0F)->as_number();
 122     y_size = THEKERNEL->config->value(leveling_strategy_checksum, cart_grid_leveling_strategy_checksum, y_size_checksum)->by_default(0.0F)->as_number();
 123     if (x_size == 0.0F || y_size == 0.0F) {
 124         THEKERNEL->streams->printf("Error: Invalid config, x_size and y_size must be defined\n");
 125         return false;
 126     }
 127
 128     // the initial height above the bed we stop the intial move down after home to find the bed
 129     // this should be a height that is enough that the probe will not hit the bed and is an offset from max_z (can be set to 0 if max_z takes into account the probe offset)
 130     this->initial_height = THEKERNEL->config->value(leveling_strategy_checksum, cart_grid_leveling_strategy_checksum, initial_height_checksum)->by_default(10)->as_number();
 131
 132     // Probe offsets xxx,yyy,zzz
 133     {
 134         std::string po = THEKERNEL->config->value(leveling_strategy_checksum, cart_grid_leveling_strategy_checksum, probe_offsets_checksum)->by_default("0,0,0")->as_string();
 135         std::vector<float> v = parse_number_list(po.c_str());
 136         if(v.size() >= 3) {
 137             this->probe_offsets = std::make_tuple(v[0], v[1], v[2]);
 138         }
 139     }
 140
 141     // allocate in AHB0
 142     grid = (float *)AHB0.alloc(configured_grid_x_size * configured_grid_y_size * sizeof(float));
 143
 144     if(grid == nullptr) {
 145         THEKERNEL->streams->printf("Error: Not enough memory\n");
 146         return false;
 147     }
 148
 149     reset_bed_level();
 150
 151     return true;
 152 }
 153
 154 void CartGridStrategy::save_grid(StreamOutput *stream)
 155 {
 156     if(isnan(grid[0])) {
 157         stream->printf("error:No grid to save\n");
 158         return;
 159     }
 160
 161     if((current_grid_x_size != configured_grid_x_size) || (current_grid_y_size != configured_grid_y_size)) {
 162         stream->printf("error:Unable to save grid with size different from configured\n");
 163         return;
 164     }
 165
 166     FILE *fp = (configured_grid_x_size == configured_grid_y_size)?fopen(GRIDFILE, "w"):fopen(GRIDFILE_NM, "w");
 167     if(fp == NULL) {
 168         stream->printf("error:Failed to open grid file %s\n", GRIDFILE);
 169         return;
 170     }
 171     uint8_t tmp_configured_grid_size = configured_grid_x_size;
 172     if(fwrite(&tmp_configured_grid_size, sizeof(uint8_t), 1, fp) != 1) {
 173         stream->printf("error:Failed to write grid x size\n");
 174         fclose(fp);
 175         return;
 176     }
 177
 178     tmp_configured_grid_size = configured_grid_y_size;
 179     if(configured_grid_y_size != configured_grid_x_size){
 180         if(fwrite(&tmp_configured_grid_size, sizeof(uint8_t), 1, fp) != 1) {
 181             stream->printf("error:Failed to write grid y size\n");
 182             fclose(fp);
 183             return;
 184         }
 185     }
 186
 187     if(fwrite(&x_size, sizeof(float), 1, fp) != 1)  {
 188         stream->printf("error:Failed to write x_size\n");
 189         fclose(fp);
 190         return;
 191     }
 192
 193     if(fwrite(&y_size, sizeof(float), 1, fp) != 1)  {
 194         stream->printf("error:Failed to write y_size\n");
 195         fclose(fp);
 196         return;
 197     }
 198
 199     for (int y = 0; y < configured_grid_y_size; y++) {
 200         for (int x = 0; x < configured_grid_x_size; x++) {
 201             if(fwrite(&grid[x + (configured_grid_x_size * y)], sizeof(float), 1, fp) != 1) {
 202                 stream->printf("error:Failed to write grid\n");
 203                 fclose(fp);
 204                 return;
 205             }
 206         }
 207     }
 208     stream->printf("grid saved to %s\n", GRIDFILE);
 209     fclose(fp);
 210 }
 211
 212 bool CartGridStrategy::load_grid(StreamOutput *stream)
 213 {
 214     FILE *fp = (configured_grid_x_size == configured_grid_y_size)?fopen(GRIDFILE, "r"):fopen(GRIDFILE_NM, "r");
 215     if(fp == NULL) {
 216         stream->printf("error:Failed to open grid %s\n", GRIDFILE);
 217         return false;
 218     }
 219
 220     uint8_t load_grid_x_size, load_grid_y_size;
 221     float x, y;
 222
 223     if(fread(&load_grid_x_size, sizeof(uint8_t), 1, fp) != 1) {
 224         stream->printf("error:Failed to read grid size\n");
 225         fclose(fp);
 226         return false;
 227     }
 228
 229     if(load_grid_x_size != configured_grid_x_size) {
 230         stream->printf("error:grid size x is different read %d - config %d\n", load_grid_x_size, configured_grid_x_size);
 231         fclose(fp);
 232         return false;
 233     }
 234
 235     load_grid_y_size = load_grid_x_size;
 236
 237     if(configured_grid_x_size != configured_grid_y_size){
 238         if(fread(&load_grid_y_size, sizeof(uint8_t), 1, fp) != 1) {
 239             stream->printf("error:Failed to read grid size\n");
 240             fclose(fp);
 241             return false;
 242         }
 243
 244         if(load_grid_y_size != configured_grid_y_size) {
 245             stream->printf("error:grid size y is different read %d - config %d\n", load_grid_y_size, configured_grid_x_size);
 246             fclose(fp);
 247             return false;
 248         }
 249     }
 250
 251     if(fread(&x, sizeof(float), 1, fp) != 1) {
 252         stream->printf("error:Failed to read grid x size\n");
 253         fclose(fp);
 254         return false;
 255     }
 256
 257     if(fread(&y, sizeof(float), 1, fp) != 1) {
 258         stream->printf("error:Failed to read grid y size\n");
 259         fclose(fp);
 260         return false;
 261     }
 262
 263     if(x != x_size || y != y_size) {
 264         stream->printf("error:bed dimensions changed read (%f, %f) - config (%f,%f)\n", x, y, x_size, y_size);
 265         fclose(fp);
 266         return false;
 267     }
 268
 269     for (int y = 0; y < configured_grid_y_size; y++) {
 270         for (int x = 0; x < configured_grid_x_size; x++) {
 271             if(fread(&grid[x + (configured_grid_x_size * y)], sizeof(float), 1, fp) != 1) {
 272                 stream->printf("error:Failed to read grid\n");
 273                 fclose(fp);
 274                 return false;
 275             }
 276         }
 277     }
 278     stream->printf("grid loaded, grid: (%f, %f), size: %d x %d\n", x_size, y_size, load_grid_x_size, load_grid_y_size);
 279     fclose(fp);
 280     return true;
 281 }
 282
 283 bool CartGridStrategy::probe_grid(int n, int m, float x_size, float y_size, StreamOutput *stream)
 284 {
 285     if((n < 5)||(m < 5)) {
 286         stream->printf("Need at least a 5x5 grid to probe\n");
 287         return true;
 288     }
 289
 290     float initial_z = findBed();
 291     if(isnan(initial_z)) return false;
 292
 293     float x_step= x_size / n;
 294     float y_step= y_size / m;
 295     for (int c = 0; c < n; ++c) {
 296         float y = y_step * c;
 297         for (int r = 0; r < n; ++r) {
 298             float x = x_step * r;
 299             float z = 0.0F;
 300             float mm;
 301             if(!zprobe->doProbeAt(mm, x, y)) return false;
 302             z = zprobe->getProbeHeight() - mm;
 303             stream->printf("%10.4f ", z);
 304         }
 305         stream->printf("\n");
 306     }
 307     return true;
 308 }
 309
 310 bool CartGridStrategy::handleGcode(Gcode *gcode)
 311 {
 312     if(gcode->has_g) {
 313         if (gcode->g == 29) { // do a probe to test flatness
 314             // first wait for an empty queue i.e. no moves left
 315             THEKERNEL->conveyor->wait_for_idle();
 316
 317             int n = gcode->has_letter('I') ? gcode->get_value('I') : configured_grid_x_size;
 318             int m = gcode->has_letter('J') ? gcode->get_value('J') : configured_grid_y_size;
 319
 320             float x = x_size, y = y_size;
 321             if(gcode->has_letter('X')) x = gcode->get_value('X'); // override default probe width
 322             if(gcode->has_letter('Y')) y = gcode->get_value('Y'); // override default probe length
 323             probe_grid(n, m, x, y, gcode->stream);
 324
 325             return true;
 326
 327         } else if( gcode->g == 31 || gcode->g == 32) { // do a grid probe
 328             // first wait for an empty queue i.e. no moves left
 329             THEKERNEL->conveyor->wait_for_idle();
 330
 331             if(!doProbe(gcode)) {
 332                 gcode->stream->printf("Probe failed to complete, check the initial probe height and/or initial_height settings\n");
 333             } else {
 334                 gcode->stream->printf("Probe completed\n");
 335             }
 336             return true;
 337         }
 338
 339     } else if(gcode->has_m) {
 340         if(gcode->m == 370 || gcode->m == 561) { // M370: Clear bed, M561: Set Identity Transform
 341             // delete the compensationTransform in robot
 342             setAdjustFunction(false);
 343             reset_bed_level();
 344             gcode->stream->printf("grid cleared and disabled\n");
 345             return true;
 346
 347         } else if(gcode->m == 374) { // M374: Save grid, M374.1: delete saved grid
 348             if(gcode->subcode == 1) {
 349                 remove(GRIDFILE);
 350                 gcode->stream->printf("%s deleted\n", GRIDFILE);
 351             } else {
 352                 save_grid(gcode->stream);
 353             }
 354
 355             return true;
 356
 357         } else if(gcode->m == 375) { // M375: load grid, M375.1 display grid
 358             if(gcode->subcode == 1) {
 359                 print_bed_level(gcode->stream);
 360             } else {
 361                 if(load_grid(gcode->stream)) setAdjustFunction(true);
 362             }
 363             return true;
 364
 365         } else if(gcode->m == 565) { // M565: Set Z probe offsets
 366             float x = 0, y = 0, z = 0;
 367             if(gcode->has_letter('X')) x = gcode->get_value('X');
 368             if(gcode->has_letter('Y')) y = gcode->get_value('Y');
 369             if(gcode->has_letter('Z')) z = gcode->get_value('Z');
 370             probe_offsets = std::make_tuple(x, y, z);
 371             return true;
 372
 373         } else if(gcode->m == 500 || gcode->m == 503) { // M500 save, M503 display
 374             float x, y, z;
 375             std::tie(x, y, z) = probe_offsets;
 376             gcode->stream->printf(";Probe offsets:\nM565 X%1.5f Y%1.5f Z%1.5f\n", x, y, z);
 377             if(save) {
 378                 if(!isnan(grid[0])) gcode->stream->printf(";Load saved grid\nM375\n");
 379                 else if(gcode->m == 503) gcode->stream->printf(";WARNING No grid to save\n");
 380             }
 381             return true;
 382         }
 383     }
 384
 385     return false;
 386 }
 387
 388 // These are convenience defines to keep the code as close to the original as possible it also saves memory and flash
 389 // set the rectangle in which to probe
 390 #define LEFT_PROBE_BED_POSITION (0)
 391 #define RIGHT_PROBE_BED_POSITION (x_size)
 392 #define BACK_PROBE_BED_POSITION (y_size)
 393 #define FRONT_PROBE_BED_POSITION (0)
 394
 395 // probe at the points of a lattice grid
 396 #define AUTO_BED_LEVELING_GRID_X ((RIGHT_PROBE_BED_POSITION - LEFT_PROBE_BED_POSITION) / (current_grid_x_size - 1))
 397 #define AUTO_BED_LEVELING_GRID_Y ((BACK_PROBE_BED_POSITION - FRONT_PROBE_BED_POSITION) / (current_grid_y_size - 1))
 398
 399 #define X_PROBE_OFFSET_FROM_EXTRUDER std::get<0>(probe_offsets)
 400 #define Y_PROBE_OFFSET_FROM_EXTRUDER std::get<1>(probe_offsets)
 401 #define Z_PROBE_OFFSET_FROM_EXTRUDER std::get<2>(probe_offsets)
 402
 403 void CartGridStrategy::setAdjustFunction(bool on)
 404 {
 405     if(on) {
 406         // set the compensationTransform in robot
 407         using std::placeholders::_1;
 408         using std::placeholders::_2;
 409         THEROBOT->compensationTransform = std::bind(&CartGridStrategy::doCompensation, this, _1, _2); // [this](float *target, bool inverse) { doCompensation(target, inverse); };
 410     } else {
 411         // clear it
 412         THEROBOT->compensationTransform = nullptr;
 413     }
 414 }
 415
 416 float CartGridStrategy::findBed()
 417 {
 418     if (do_home) zprobe->home();
 419     // move to an initial position fast so as to not take all day, we move down max_z - initial_height, which is set in config, default 10mm
 420     float deltaz = initial_height;
 421     zprobe->coordinated_move(NAN, NAN, deltaz, zprobe->getFastFeedrate());
 422     zprobe->coordinated_move(0, 0, NAN, zprobe->getFastFeedrate()); // move to 0,0
 423
 424     // find bed at 0,0 run at slow rate so as to not hit bed hard
 425     float mm;
 426     if(!zprobe->run_probe_return(mm, zprobe->getSlowFeedrate())) return NAN;
 427
 428     float dz = zprobe->getProbeHeight() - mm;
 429     zprobe->coordinated_move(NAN, NAN, dz, zprobe->getFastFeedrate(), true); // relative move
 430
 431     return mm + deltaz - zprobe->getProbeHeight(); // distance to move from home to 5mm above bed
 432 }
 433
 434 bool CartGridStrategy::doProbe(Gcode *gc)
 435 {
 436     gc->stream->printf("Rectangular Grid Probe...\n");
 437     setAdjustFunction(false);
 438     reset_bed_level();
 439
 440     if(gc->has_letter('X')) x_size = gc->get_value('X'); // override default probe width, will get saved
 441     if(gc->has_letter('Y')) y_size = gc->get_value('Y'); // override default probe length, will get saved
 442
 443     if(gc->has_letter('I')) current_grid_x_size = gc->get_value('I'); // override default grid x size
 444     if(gc->has_letter('J')) current_grid_y_size = gc->get_value('J'); // override default grid y size
 445
 446     if((current_grid_x_size * current_grid_y_size)  > (configured_grid_x_size * configured_grid_y_size)){
 447         gc->stream->printf("Grid size (%d x %d = %d) bigger than configured (%d x %d = %d). Change configuration.\n", current_grid_x_size, current_grid_y_size, current_grid_x_size*current_grid_x_size,                             configured_grid_x_size, configured_grid_y_size, configured_grid_x_size*configured_grid_y_size);
 448         return false;
 449     }
 450
 451
 452     // find bed, and leave probe probe height above bed
 453     float initial_z = findBed();
 454     if(isnan(initial_z)) {
 455         gc->stream->printf("Finding bed failed, check the maxz and initial height settings\n");
 456         return false;
 457     }
 458
 459     gc->stream->printf("Probe start ht is %f mm, rectangular bed width %fmm, height %fmm, grid size is %dx%d\n", initial_z, x_size, y_size, current_grid_x_size, current_grid_y_size);
 460
 461     // do first probe for 0,0
 462     float mm;
 463     if(!zprobe->doProbeAt(mm, -X_PROBE_OFFSET_FROM_EXTRUDER, -Y_PROBE_OFFSET_FROM_EXTRUDER)) return false;
 464     float z_reference = zprobe->getProbeHeight() - mm; // this should be zero
 465     gc->stream->printf("probe at 0,0 is %f mm\n", z_reference);
 466
 467     // probe all the points of the grid
 468     for (int yCount = 0; yCount < current_grid_y_size; yCount++) {
 469         float yProbe = FRONT_PROBE_BED_POSITION + AUTO_BED_LEVELING_GRID_Y * yCount;
 470         int xStart, xStop, xInc;
 471         if (yCount % 2) {
 472             xStart = current_grid_x_size - 1;
 473             xStop = -1;
 474             xInc = -1;
 475         } else {
 476             xStart = 0;
 477             xStop = current_grid_x_size;
 478             xInc = 1;
 479         }
 480
 481         for (int xCount = xStart; xCount != xStop; xCount += xInc) {
 482             float xProbe = LEFT_PROBE_BED_POSITION + AUTO_BED_LEVELING_GRID_X * xCount;
 483
 484             if(!zprobe->doProbeAt(mm, xProbe - X_PROBE_OFFSET_FROM_EXTRUDER, yProbe - Y_PROBE_OFFSET_FROM_EXTRUDER)) return false;
 485             float measured_z = zprobe->getProbeHeight() - mm - z_reference; // this is the delta z from bed at 0,0
 486             gc->stream->printf("DEBUG: X%10.4f, Y%10.4f, Z%10.4f\n", xProbe, yProbe, measured_z);
 487             grid[xCount + (current_grid_x_size * yCount)] = measured_z;
 488         }
 489     }
 490
 491     print_bed_level(gc->stream);
 492
 493     setAdjustFunction(true);
 494
 495     return true;
 496 }
 497
 498 void CartGridStrategy::doCompensation(float *target, bool inverse)
 499 {
 500     // Adjust print surface height by linear interpolation over the bed_level array.
 501     float grid_x = std::max(0.001F, target[X_AXIS] / AUTO_BED_LEVELING_GRID_X);
 502     float grid_y = std::max(0.001F, target[Y_AXIS] / AUTO_BED_LEVELING_GRID_Y);
 503     int floor_x = floorf(grid_x);
 504     int floor_y = floorf(grid_y);
 505     float ratio_x = grid_x - floor_x;
 506     float ratio_y = grid_y - floor_y;
 507     float z1 = grid[(floor_x) + ((floor_y) * current_grid_x_size)];
 508     float z2 = grid[(floor_x) + ((floor_y + 1) * current_grid_x_size)];
 509     float z3 = grid[(floor_x + 1) + ((floor_y) * current_grid_x_size)];
 510     float z4 = grid[(floor_x + 1) + ((floor_y + 1) * current_grid_x_size)];
 511     float left = (1 - ratio_y) * z1 + ratio_y * z2;
 512     float right = (1 - ratio_y) * z3 + ratio_y * z4;
 513     float offset = (1 - ratio_x) * left + ratio_x * right;
 514
 515     if(inverse)
 516         target[Z_AXIS] -= offset;
 517     else
 518         target[Z_AXIS] += offset;
 519
 520
 521     /*
 522         THEKERNEL->streams->printf("//DEBUG: TARGET: %f, %f, %f\n", target[0], target[1], target[2]);
 523         THEKERNEL->streams->printf("//DEBUG: grid_x= %f\n", grid_x);
 524         THEKERNEL->streams->printf("//DEBUG: grid_y= %f\n", grid_y);
 525         THEKERNEL->streams->printf("//DEBUG: floor_x= %d\n", floor_x);
 526         THEKERNEL->streams->printf("//DEBUG: floor_y= %d\n", floor_y);
 527         THEKERNEL->streams->printf("//DEBUG: ratio_x= %f\n", ratio_x);
 528         THEKERNEL->streams->printf("//DEBUG: ratio_y= %f\n", ratio_y);
 529         THEKERNEL->streams->printf("//DEBUG: z1= %f\n", z1);
 530         THEKERNEL->streams->printf("//DEBUG: z2= %f\n", z2);
 531         THEKERNEL->streams->printf("//DEBUG: z3= %f\n", z3);
 532         THEKERNEL->streams->printf("//DEBUG: z4= %f\n", z4);
 533         THEKERNEL->streams->printf("//DEBUG: left= %f\n", left);
 534         THEKERNEL->streams->printf("//DEBUG: right= %f\n", right);
 535         THEKERNEL->streams->printf("//DEBUG: offset= %f\n", offset);
 536     */
 537 }
 538
 539
 540 // Print calibration results for plotting or manual frame adjustment.
 541 void CartGridStrategy::print_bed_level(StreamOutput *stream)
 542 {
 543     for (int y = 0; y < current_grid_y_size; y++) {
 544         for (int x = 0; x < current_grid_x_size; x++) {
 545             stream->printf("%10.4f ", grid[x + (current_grid_x_size * y)]);
 546         }
 547         stream->printf("\n");
 548     }
 549 }
 550
 551 // Reset calibration results to zero.
 552 void CartGridStrategy::reset_bed_level()
 553 {
 554     for (int y = 0; y < current_grid_y_size; y++) {
 555         for (int x = 0; x < current_grid_x_size; x++) {
 556             grid[x + (current_grid_x_size * y)] = NAN;
 557         }
 558     }
 559 }