src/modules/tools/zprobe/DeltaGridStrategy.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.delta-grid.enable         true
  15
  16     The radius of the bed must be specified with...
  17
  18       leveling-strategy.delta-grid.radius        50
  19
  20       this needs to be at least as big as the maximum printing radius as moves outside of this will not be compensated for correctly
  21
  22     The size of the grid can be set with...
  23
  24       leveling-strategy.delta-grid.size        7
  25
  26       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
  27
  28    Optionally probe offsets from the nozzle or tool head can be defined with...
  29
  30       leveling-strategy.delta-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.delta-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.delta-grid.initial_height  10
  42
  43
  44     Usage
  45     -----
  46     G29 test probes in a spiral pattern within the radius producing a map of offsets, this can be imported into a graphing program to visualize the bed heights
  47         optional parameters {{In}} sets the number of points to the value n, {{Jn}} sets the radius for this probe.
  48
  49     G31 probes the grid and turns the compensation on, this will remain in effect until reset or M561/M370
  50         optional parameters {{Jn}} sets the radius for this probe, which gets saved with M375
  51
  52     M370 clears the grid and turns off compensation
  53     M374 Save grid to /sd/delta.grid
  54     M374.1 delete /sd/delta.grid
  55     M375 Load the grid from /sd/delta.grid and enable compensation
  56     M375.1 display the current grid
  57     M561 clears the grid and turns off compensation
  58     M565 defines the probe offsets from the nozzle or tool head
  59
  60
  61     M500 saves the probe points
  62     M503 displays the current settings
  63 */
  64
  65 #include "DeltaGridStrategy.h"
  66
  67 #include "Kernel.h"
  68 #include "Config.h"
  69 #include "Robot.h"
  70 #include "StreamOutputPool.h"
  71 #include "Gcode.h"
  72 #include "checksumm.h"
  73 #include "ConfigValue.h"
  74 #include "PublicDataRequest.h"
  75 #include "PublicData.h"
  76 #include "Conveyor.h"
  77 #include "ZProbe.h"
  78 #include "nuts_bolts.h"
  79 #include "utils.h"
  80 #include "platform_memory.h"
  81
  82 #include <string>
  83 #include <algorithm>
  84 #include <cstdlib>
  85 #include <cmath>
  86
  87 #define grid_radius_checksum         CHECKSUM("radius")
  88 #define grid_size_checksum           CHECKSUM("size")
  89 #define tolerance_checksum           CHECKSUM("tolerance")
  90 #define save_checksum                CHECKSUM("save")
  91 #define probe_offsets_checksum       CHECKSUM("probe_offsets")
  92 #define initial_height_checksum      CHECKSUM("initial_height")
  93 #define x_max_checksum               CHECKSUM("x_max")
  94 #define y_max_checksum               CHECKSUM("y_max")
  95 #define do_home_checksum             CHECKSUM("do_home")
  96 #define is_square_checksum           CHECKSUM("is_square")
  97
  98 #define GRIDFILE "/sd/delta.grid"
  99
 100 DeltaGridStrategy::DeltaGridStrategy(ZProbe *zprobe) : LevelingStrategy(zprobe)
 101 {
 102     grid= nullptr;
 103 }
 104
 105 DeltaGridStrategy::~DeltaGridStrategy()
 106 {
 107     if(grid != nullptr) AHB0.dealloc(grid);
 108 }
 109
 110 bool DeltaGridStrategy::handleConfig()
 111 {
 112     grid_radius = THEKERNEL->config->value(leveling_strategy_checksum, delta_grid_leveling_strategy_checksum, grid_radius_checksum)->by_default(50.0F)->as_number();
 113     grid_size = THEKERNEL->config->value(leveling_strategy_checksum, delta_grid_leveling_strategy_checksum, grid_size_checksum)->by_default(7)->as_number();
 114     tolerance = THEKERNEL->config->value(leveling_strategy_checksum, delta_grid_leveling_strategy_checksum, tolerance_checksum)->by_default(0.03F)->as_number();
 115     save = THEKERNEL->config->value(leveling_strategy_checksum, delta_grid_leveling_strategy_checksum, save_checksum)->by_default(false)->as_bool();
 116     do_home = THEKERNEL->config->value(leveling_strategy_checksum, delta_grid_leveling_strategy_checksum, do_home_checksum)->by_default(true)->as_bool();
 117     is_square = THEKERNEL->config->value(leveling_strategy_checksum, delta_grid_leveling_strategy_checksum, is_square_checksum)->by_default(false)->as_bool();
 118
 119     if (is_square)
 120     {
 121       x_max = THEKERNEL->config->value(leveling_strategy_checksum, delta_grid_leveling_strategy_checksum, x_max_checksum)->by_default(0.0F)->as_number();
 122       y_max = THEKERNEL->config->value(leveling_strategy_checksum, delta_grid_leveling_strategy_checksum, y_max_checksum)->by_default(0.0F)->as_number();
 123
 124       // intelligently set defaults.
 125       if (x_max >= 1.0F) grid_radius = x_max;
 126       if (x_max < 1.0F) x_max = grid_radius;
 127       if (y_max >= 1.0F) grid_radius = y_max;
 128       if (y_max < 1.0F) y_max = grid_radius;
 129       if (x_max >= 1.0F && y_max >= 1.0F) grid_radius = std::max(x_max, y_max);
 130     }
 131     else
 132     {
 133       x_max = grid_radius;
 134       y_max = grid_radius;
 135     }
 136
 137     // the initial height above the bed we stop the intial move down after home to find the bed
 138     // 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)
 139     this->initial_height = THEKERNEL->config->value(leveling_strategy_checksum, delta_grid_leveling_strategy_checksum, initial_height_checksum)->by_default(10)->as_number();
 140
 141     // Probe offsets xxx,yyy,zzz
 142     {
 143         std::string po = THEKERNEL->config->value(leveling_strategy_checksum, delta_grid_leveling_strategy_checksum, probe_offsets_checksum)->by_default("0,0,0")->as_string();
 144         std::vector<float> v = parse_number_list(po.c_str());
 145         if(v.size() >= 3) {
 146             this->probe_offsets = std::make_tuple(v[0], v[1], v[2]);
 147         }
 148     }
 149
 150     // allocate in AHB0
 151     grid= (float *)AHB0.alloc(grid_size * grid_size * sizeof(float));
 152
 153     reset_bed_level();
 154
 155     return true;
 156 }
 157
 158 void DeltaGridStrategy::save_grid(StreamOutput *stream)
 159 {
 160     if(isnan(grid[0])) {
 161         stream->printf("error:No grid to save\n");
 162         return;
 163     }
 164
 165     FILE *fp = fopen(GRIDFILE, "w");
 166     if(fp == NULL) {
 167         stream->printf("error:Failed to open grid file %s\n", GRIDFILE);
 168         return;
 169     }
 170
 171     if(fwrite(&grid_size, sizeof(uint8_t), 1, fp) != 1) {
 172         stream->printf("error:Failed to write grid size\n");
 173         fclose(fp);
 174         return;
 175     }
 176
 177     if(fwrite(&grid_radius, sizeof(float), 1, fp) != 1) {
 178         stream->printf("error:Failed to write grid radius\n");
 179         fclose(fp);
 180         return;
 181     }
 182
 183     for (int y = 0; y < grid_size; y++) {
 184         for (int x = 0; x < grid_size; x++) {
 185             if(fwrite(&grid[x + (grid_size*y)], sizeof(float), 1, fp) != 1) {
 186                 stream->printf("error:Failed to write grid\n");
 187                 fclose(fp);
 188                 return;
 189             }
 190         }
 191     }
 192     stream->printf("grid saved to %s\n", GRIDFILE);
 193     fclose(fp);
 194 }
 195
 196 bool DeltaGridStrategy::load_grid(StreamOutput *stream)
 197 {
 198     FILE *fp = fopen(GRIDFILE, "r");
 199     if(fp == NULL) {
 200         stream->printf("error:Failed to open grid %s\n", GRIDFILE);
 201         return false;
 202     }
 203
 204     uint8_t size;
 205     float radius;
 206
 207     if(fread(&size, sizeof(uint8_t), 1, fp) != 1) {
 208         stream->printf("error:Failed to read grid size\n");
 209         fclose(fp);
 210         return false;
 211     }
 212
 213     if(size != grid_size) {
 214         stream->printf("error:grid size is different read %d - config %d\n", size, grid_size);
 215         fclose(fp);
 216         return false;
 217     }
 218
 219     if(fread(&radius, sizeof(float), 1, fp) != 1) {
 220         stream->printf("error:Failed to read grid radius\n");
 221         fclose(fp);
 222         return false;
 223     }
 224
 225     if(radius != grid_radius) {
 226         stream->printf("warning:grid radius is different read %f - config %f, overriding config\n", radius, grid_radius);
 227         grid_radius= radius;
 228     }
 229
 230     for (int y = 0; y < grid_size; y++) {
 231         for (int x = 0; x < grid_size; x++) {
 232             if(fread(&grid[x + (grid_size*y)], sizeof(float), 1, fp) != 1) {
 233                 stream->printf("error:Failed to read grid\n");
 234                 fclose(fp);
 235                 return false;
 236             }
 237         }
 238     }
 239     stream->printf("grid loaded, radius: %f, size: %d\n", grid_radius, grid_size);
 240     fclose(fp);
 241     return true;
 242 }
 243
 244 bool DeltaGridStrategy::probe_grid(int n, float radius, StreamOutput *stream)
 245 {
 246     if(n < 5) {
 247         stream->printf("Need at least a 5x5 grid to probe\n");
 248         return true;
 249     }
 250
 251     float initial_z = findBed();
 252     if(isnan(initial_z)) return false;
 253
 254     float d= ((radius*2) / (n - 1));
 255
 256     for (int c = 0; c < n; ++c) {
 257         float y = -radius + d*c;
 258         for (int r = 0; r < n; ++r) {
 259             float x = -radius + d*r;
 260             // Avoid probing the corners (outside the round or hexagon print surface) on a delta printer.
 261             float distance_from_center = sqrtf(x*x + y*y);
 262             float z= 0.0F;
 263             if ((!is_square && (distance_from_center <= radius)) ||
 264                (is_square && (x < -x_max || x > x_max || y < -y_max || y > y_max))) {
 265                 float mm;
 266                 if(!zprobe->doProbeAt(mm, x, y)) return false;
 267                 z = zprobe->getProbeHeight() - mm;
 268             }
 269             stream->printf("%8.4f ", z);
 270         }
 271         stream->printf("\n");
 272     }
 273     return true;
 274 }
 275
 276 // taken from Oskars PR #713
 277 bool DeltaGridStrategy::probe_spiral(int n, float radius, StreamOutput *stream)
 278 {
 279     float a = radius / (2 * sqrtf(n * M_PI));
 280     float step_length = radius * radius / (2 * a * n);
 281
 282     float initial_z = findBed();
 283     if(isnan(initial_z)) return false;
 284
 285     auto theta = [a](float length) {return sqrtf(2*length/a); };
 286
 287     float maxz= NAN, minz= NAN;
 288     for (int i = 0; i < n; i++) {
 289         float angle = theta(i * step_length);
 290         float r = angle * a;
 291         // polar to cartesian
 292         float x = r * cosf(angle);
 293         float y = r * sinf(angle);
 294
 295         float mm;
 296         if (!zprobe->doProbeAt(mm, x, y)) return false;
 297         float z = zprobe->getProbeHeight() - mm;
 298         stream->printf("PROBE: X%1.4f, Y%1.4f, Z%1.4f\n", x, y, z);
 299         if(isnan(maxz) || z > maxz) maxz= z;
 300         if(isnan(minz) || z < minz) minz= z;
 301     }
 302
 303     stream->printf("max: %1.4f, min: %1.4f, delta: %1.4f\n", maxz, minz, maxz-minz);
 304     return true;
 305 }
 306
 307 bool DeltaGridStrategy::handleGcode(Gcode *gcode)
 308 {
 309     if(gcode->has_g) {
 310         if (gcode->g == 29) { // do a probe to test flatness
 311             // first wait for an empty queue i.e. no moves left
 312             THEKERNEL->conveyor->wait_for_idle();
 313
 314             int n= gcode->has_letter('I') ? gcode->get_value('I') : 0;
 315             float radius = grid_radius;
 316             if(gcode->has_letter('J')) radius = gcode->get_value('J'); // override default probe radius
 317             if(gcode->subcode == 1){
 318                 if(n==0) n= 50;
 319                 probe_spiral(n, radius, gcode->stream);
 320             }else{
 321                 if(n==0) n= 7;
 322                 probe_grid(n, radius, gcode->stream);
 323             }
 324
 325             return true;
 326
 327         } else if( gcode->g == 31 ) { // 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 (-grid_radius)
 391 #define RIGHT_PROBE_BED_POSITION (grid_radius)
 392 #define BACK_PROBE_BED_POSITION (grid_radius)
 393 #define FRONT_PROBE_BED_POSITION (-grid_radius)
 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) / (grid_size - 1))
 397 #define AUTO_BED_LEVELING_GRID_Y ((BACK_PROBE_BED_POSITION - FRONT_PROBE_BED_POSITION) / (grid_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 DeltaGridStrategy::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(&DeltaGridStrategy::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 DeltaGridStrategy::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 DeltaGridStrategy::doProbe(Gcode *gc)
 435 {
 436     gc->stream->printf("Delta Grid Probe...\n");
 437     setAdjustFunction(false);
 438     reset_bed_level();
 439
 440     if(gc->has_letter('J')) grid_radius = gc->get_value('J'); // override default probe radius, will get saved
 441
 442     float radius = grid_radius;
 443     // find bed, and leave probe probe height above bed
 444     float initial_z = findBed();
 445     if(isnan(initial_z)) {
 446         gc->stream->printf("Finding bed failed, check the maxz and initial height settings\n");
 447         return false;
 448     }
 449
 450     gc->stream->printf("Probe start ht is %f mm, probe radius is %f mm, grid size is %dx%d\n", initial_z, radius, grid_size, grid_size);
 451
 452     // do first probe for 0,0
 453     float mm;
 454     if(!zprobe->doProbeAt(mm, -X_PROBE_OFFSET_FROM_EXTRUDER, -Y_PROBE_OFFSET_FROM_EXTRUDER)) return false;
 455     float z_reference = zprobe->getProbeHeight() - mm; // this should be zero
 456     gc->stream->printf("probe at 0,0 is %f mm\n", z_reference);
 457
 458     // probe all the points in the grid within the given radius
 459     for (int yCount = 0; yCount < grid_size; yCount++) {
 460         float yProbe = FRONT_PROBE_BED_POSITION + AUTO_BED_LEVELING_GRID_Y * yCount;
 461         int xStart, xStop, xInc;
 462         if (yCount % 2) {
 463             xStart = 0;
 464             xStop = grid_size;
 465             xInc = 1;
 466         } else {
 467             xStart = grid_size - 1;
 468             xStop = -1;
 469             xInc = -1;
 470         }
 471
 472         for (int xCount = xStart; xCount != xStop; xCount += xInc) {
 473             float xProbe = LEFT_PROBE_BED_POSITION + AUTO_BED_LEVELING_GRID_X * xCount;
 474
 475             // avoid probing outside of x min/max on a cartesian
 476             if (is_square)
 477             {
 478               if (xProbe < -x_max || xProbe > x_max || yProbe < -y_max || yProbe > y_max) continue;
 479             }
 480             else
 481             {
 482               // Avoid probing the corners (outside the round or hexagon print surface) on a delta printer.
 483               float distance_from_center = sqrtf(xProbe * xProbe + yProbe * yProbe);
 484               if (distance_from_center > radius) continue;
 485             }
 486
 487             if(!zprobe->doProbeAt(mm, xProbe - X_PROBE_OFFSET_FROM_EXTRUDER, yProbe - Y_PROBE_OFFSET_FROM_EXTRUDER)) return false;
 488             float measured_z = zprobe->getProbeHeight() - mm - z_reference; // this is the delta z from bed at 0,0
 489             gc->stream->printf("DEBUG: X%1.4f, Y%1.4f, Z%1.4f\n", xProbe, yProbe, measured_z);
 490             grid[xCount + (grid_size * yCount)] = measured_z;
 491         }
 492     }
 493
 494     extrapolate_unprobed_bed_level();
 495     print_bed_level(gc->stream);
 496
 497     setAdjustFunction(true);
 498
 499     return true;
 500 }
 501
 502 void DeltaGridStrategy::extrapolate_one_point(int x, int y, int xdir, int ydir)
 503 {
 504     if (!isnan(grid[x + (grid_size*y)])) {
 505         return;  // Don't overwrite good values.
 506     }
 507     float a = 2 * grid[(x + xdir) + (y*grid_size)] - grid[(x + xdir * 2) + (y*grid_size)]; // Left to right.
 508     float b = 2 * grid[x + ((y + ydir) * grid_size)] - grid[x + ((y + ydir * 2) * grid_size)]; // Front to back.
 509     float c = 2 * grid[(x + xdir) + ((y + ydir) * grid_size)] - grid[(x + xdir * 2) + ((y + ydir * 2) * grid_size)]; // Diagonal.
 510     float median = c;  // Median is robust (ignores outliers).
 511     if (a < b) {
 512         if (b < c) median = b;
 513         if (c < a) median = a;
 514     } else {  // b <= a
 515         if (c < b) median = b;
 516         if (a < c) median = a;
 517     }
 518     grid[x + (grid_size*y)] = median;
 519 }
 520
 521 // Fill in the unprobed points (corners of circular print surface)
 522 // using linear extrapolation, away from the center.
 523 void DeltaGridStrategy::extrapolate_unprobed_bed_level()
 524 {
 525     int half = (grid_size - 1) / 2;
 526     for (int y = 0; y <= half; y++) {
 527         for (int x = 0; x <= half; x++) {
 528             if (x + y < 3) continue;
 529             extrapolate_one_point(half - x, half - y, x > 1 ? +1 : 0, y > 1 ? +1 : 0);
 530             extrapolate_one_point(half + x, half - y, x > 1 ? -1 : 0, y > 1 ? +1 : 0);
 531             extrapolate_one_point(half - x, half + y, x > 1 ? +1 : 0, y > 1 ? -1 : 0);
 532             extrapolate_one_point(half + x, half + y, x > 1 ? -1 : 0, y > 1 ? -1 : 0);
 533         }
 534     }
 535 }
 536
 537 void DeltaGridStrategy::doCompensation(float *target, bool inverse)
 538 {
 539     // Adjust print surface height by linear interpolation over the bed_level array.
 540     int half = (grid_size - 1) / 2;
 541     float grid_x = std::max(0.001F - half, std::min(half - 0.001F, target[X_AXIS] / AUTO_BED_LEVELING_GRID_X));
 542     float grid_y = std::max(0.001F - half, std::min(half - 0.001F, target[Y_AXIS] / AUTO_BED_LEVELING_GRID_Y));
 543     int floor_x = floorf(grid_x);
 544     int floor_y = floorf(grid_y);
 545     float ratio_x = grid_x - floor_x;
 546     float ratio_y = grid_y - floor_y;
 547     float z1 = grid[(floor_x + half) + ((floor_y + half) * grid_size)];
 548     float z2 = grid[(floor_x + half) + ((floor_y + half + 1) * grid_size)];
 549     float z3 = grid[(floor_x + half + 1) + ((floor_y + half) * grid_size)];
 550     float z4 = grid[(floor_x + half + 1) + ((floor_y + half + 1) * grid_size)];
 551     float left = (1 - ratio_y) * z1 + ratio_y * z2;
 552     float right = (1 - ratio_y) * z3 + ratio_y * z4;
 553     float offset = (1 - ratio_x) * left + ratio_x * right;
 554
 555     if(inverse)
 556         target[Z_AXIS] -= offset;
 557     else
 558         target[Z_AXIS] += offset;
 559
 560
 561     /*
 562         THEKERNEL->streams->printf("//DEBUG: TARGET: %f, %f, %f\n", target[0], target[1], target[2]);
 563         THEKERNEL->streams->printf("//DEBUG: grid_x= %f\n", grid_x);
 564         THEKERNEL->streams->printf("//DEBUG: grid_y= %f\n", grid_y);
 565         THEKERNEL->streams->printf("//DEBUG: floor_x= %d\n", floor_x);
 566         THEKERNEL->streams->printf("//DEBUG: floor_y= %d\n", floor_y);
 567         THEKERNEL->streams->printf("//DEBUG: ratio_x= %f\n", ratio_x);
 568         THEKERNEL->streams->printf("//DEBUG: ratio_y= %f\n", ratio_y);
 569         THEKERNEL->streams->printf("//DEBUG: z1= %f\n", z1);
 570         THEKERNEL->streams->printf("//DEBUG: z2= %f\n", z2);
 571         THEKERNEL->streams->printf("//DEBUG: z3= %f\n", z3);
 572         THEKERNEL->streams->printf("//DEBUG: z4= %f\n", z4);
 573         THEKERNEL->streams->printf("//DEBUG: left= %f\n", left);
 574         THEKERNEL->streams->printf("//DEBUG: right= %f\n", right);
 575         THEKERNEL->streams->printf("//DEBUG: offset= %f\n", offset);
 576     */
 577 }
 578
 579
 580 // Print calibration results for plotting or manual frame adjustment.
 581 void DeltaGridStrategy::print_bed_level(StreamOutput *stream)
 582 {
 583     for (int y = 0; y < grid_size; y++) {
 584         for (int x = 0; x < grid_size; x++) {
 585             stream->printf("%7.4f ", grid[x + (grid_size*y)]);
 586         }
 587         stream->printf("\n");
 588     }
 589 }
 590
 591 // Reset calibration results to zero.
 592 void DeltaGridStrategy::reset_bed_level()
 593 {
 594     for (int y = 0; y < grid_size; y++) {
 595         for (int x = 0; x < grid_size; x++) {
 596             grid[x + (grid_size*y)] = NAN;
 597         }
 598     }
 599 }