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