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