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