src/modules/tools/zprobe/ZGridStrategy.cpp

   1 /*
   2     Author: Quentin Harley (quentin.harley@gmail.com)
   3     License: GPL3 or better see <http://www.gnu.org/licenses/>
   4
   5     Summary
   6     -------
   7     Probes user defined amount of calculated points on the bed and creates compensation grid data of the bed surface.
   8     Bilinear
   9     As the head moves in X and Y it will adjust Z to keep the head level with the bed.
  10
  11     Configuration
  12     -------------
  13     The strategy must be enabled in the config as well as zprobe.
  14
  15        leveling-strategy.ZGrid-leveling.enable         true
  16
  17     The bed size limits must be defined, in order for the module to calculate the calibration points
  18
  19        leveling-strategy.ZGrid-leveling.bed_x           200
  20        leveling-strategy.ZGrid-leveling.bed_y           200
  21
  22     Machine height, used to determine probe attachment point (bed_z / 2)
  23
  24        leveling-strategy.ZGrid-leveling.bed_z           20
  25
  26     Configure for Machines with bed 0:0 at center of platform
  27        leveling-strategy.ZGrid-leveling.bed_zero        false
  28
  29     configure for Machines with circular beds
  30        leveling-strategy.ZGrid-leveling.bed_circular    false
  31
  32
  33     The number of divisions for X and Y should be defined
  34
  35        leveling-strategy.ZGrid-leveling.rows           7          # X divisions (Default 5)
  36        leveling-strategy.ZGrid-leveling.cols           9          # Y divisions (Default 5)
  37
  38
  39     The probe offset should be defined, default to zero offset
  40
  41        leveling-strategy.ZGrid-leveling.probe_offsets  0,0,16.3
  42
  43     The machine can be told to wait for probe attachment and confirmation
  44
  45        leveling-strategy.ZGrid-leveling.wait_for_probe  true
  46
  47     The machine can be told to home in one of the following modes:
  48
  49        leveling-strategy.ZGrid-leveling.home_before_probe  homexyz;    #  nohome homexy homexyz (default)
  50
  51
  52     Slow feedrate can be defined for probe positioning speed.  Note this is not Probing slow rate - it can be set to a fast speed if required.
  53
  54        leveling-strategy.ZGrid-leveling.slow_feedrate  100         # ZGrid probe positioning feedrate
  55
  56
  57
  58     Usage
  59     -----
  60     G32                  : probes the probe points and defines the bed ZGrid, this will remain in effect until reset or M370
  61     G31                  : reports the status - Display probe data points
  62
  63     M370                 : clears the ZGrid and the bed levelling is disabled until G32 is run again
  64     M370 X7 Y9           : allocates a new grid size of 7x9 and clears as above
  65
  66     M371                 : moves the head to the next calibration position without saving for manual calibration
  67     M372                 : move the head to the next calibration position after saving the current probe point to memory - manual calbration
  68     M373                 : completes calibration and enables the Z compensation grid
  69
  70     M374                 : Save the grid to "Zgrid" on SD card
  71     M374 S###            : Save custom grid to "Zgrid.###" on SD card
  72
  73     M375                 : Loads grid file "Zgrid" from SD
  74     M375 S###            : Load custom grid file "Zgrid.###"
  75
  76     M565 X### Y### Z###  : Set new probe offsets
  77
  78     M500                 : saves the probe offsets
  79     M503                 : displays the current settings
  80 */
  81
  82 #include "ZGridStrategy.h"
  83 #include "Kernel.h"
  84 #include "Config.h"
  85 #include "Robot.h"
  86 #include "StreamOutputPool.h"
  87 #include "Gcode.h"
  88 #include "checksumm.h"
  89 #include "ConfigValue.h"
  90 #include "PublicDataRequest.h"
  91 #include "PublicData.h"
  92 #include "EndstopsPublicAccess.h"
  93 #include "Conveyor.h"
  94 #include "ZProbe.h"
  95 #include "libs/FileStream.h"
  96 #include "nuts_bolts.h"
  97 #include "platform_memory.h"
  98 #include "MemoryPool.h"
  99 #include "libs/utils.h"
 100
 101 #include <string>
 102 #include <algorithm>
 103 #include <cstdlib>
 104 #include <cmath>
 105 #include <unistd.h>
 106
 107 #define bed_x_checksum               CHECKSUM("bed_x")
 108 #define bed_y_checksum               CHECKSUM("bed_y")
 109 #define bed_z_checksum               CHECKSUM("bed_z")
 110
 111 #define slow_feedrate_checksum       CHECKSUM("slow_feedrate")
 112 #define probe_offsets_checksum       CHECKSUM("probe_offsets")
 113 #define wait_for_probe_checksum      CHECKSUM("wait_for_probe")
 114 #define home_before_probe_checksum   CHECKSUM("home_before_probe")
 115 #define center_zero_checksum         CHECKSUM("center_zero")
 116 #define circular_bed_checksum        CHECKSUM("circular_bed")
 117 #define cal_offset_x_checksum        CHECKSUM("cal_offset_x")
 118 #define cal_offset_y_checksum        CHECKSUM("cal_offset_y")
 119
 120 #define NOHOME                       0
 121 #define HOMEXY                       1
 122 #define HOMEXYZ                      2
 123
 124 #define cols_checksum                CHECKSUM("cols")
 125 #define rows_checksum                CHECKSUM("rows")
 126
 127 #define probe_points                 (this->numRows * this->numCols)
 128
 129
 130
 131 ZGridStrategy::ZGridStrategy(ZProbe *zprobe) : LevelingStrategy(zprobe)
 132 {
 133     this->cal[X_AXIS] = 0.0f;
 134     this->cal[Y_AXIS] = 0.0f;
 135     this->cal[Z_AXIS] = 30.0f;
 136
 137     this->in_cal = false;
 138     this->pData = nullptr;
 139 }
 140
 141 ZGridStrategy::~ZGridStrategy()
 142 {
 143     // Free program memory for the pData grid
 144     if(this->pData != nullptr) AHB0.dealloc(this->pData);
 145 }
 146
 147 bool ZGridStrategy::handleConfig()
 148 {
 149     this->bed_x = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, bed_x_checksum)->by_default(200.0F)->as_number();
 150     this->bed_y = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, bed_y_checksum)->by_default(200.0F)->as_number();
 151     this->bed_z = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, bed_z_checksum)->by_default(20.0F)->as_number();
 152
 153     this->slow_rate = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, slow_feedrate_checksum)->by_default(20.0F)->as_number();
 154
 155     this->numRows = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, rows_checksum)->by_default(5)->as_number();
 156     this->numCols = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, cols_checksum)->by_default(5)->as_number();
 157
 158     this->wait_for_probe   = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, wait_for_probe_checksum)->by_default(true)->as_bool();  // Morgan default = true
 159
 160     std::string home_mode  = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, home_before_probe_checksum)->by_default("homexyz")->as_string();
 161     if (home_mode.compare("nohome") == 0) {
 162         this->home_before_probe = NOHOME;
 163     }
 164     else if (home_mode.compare("homexy") == 0) {
 165         this->home_before_probe = HOMEXY;
 166     }
 167     else { // Default
 168         this->home_before_probe = HOMEXYZ;
 169     }
 170
 171
 172     //this->home_before_probe = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, home_before_probe_checksum)->by_default(HOMEXYZ)->as_number();  // Morgan default = HOMEXYZ
 173
 174     this->center_zero = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, center_zero_checksum)->by_default(false)->as_bool();
 175     this->circular_bed = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, circular_bed_checksum)->by_default(false)->as_bool();
 176
 177     // configures calbration positioning offset.  Defaults to 0 for standard cartesian space machines, and to negative half of the current bed size in X and Y
 178     this->cal_offset_x = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, cal_offset_x_checksum)->by_default( this->center_zero ? this->bed_x / -2.0F : 0.0F )->as_number();
 179     this->cal_offset_y = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, cal_offset_y_checksum)->by_default( this->center_zero ? this->bed_y / -2.0F : 0.0F )->as_number();
 180
 181
 182     // Probe offsets xxx,yyy,zzz
 183     std::string po = THEKERNEL->config->value(leveling_strategy_checksum, ZGrid_leveling_checksum, probe_offsets_checksum)->by_default("0,0,0")->as_string();
 184     this->probe_offsets= parseXYZ(po.c_str());
 185
 186     this->calcConfig();                // Run calculations for Grid size and allocate initial grid memory
 187
 188     for (int i=0; i<(probe_points); i++){
 189         this->pData[i] = 0.0F;        // Clear the grid
 190     }
 191
 192     return true;
 193 }
 194
 195 void ZGridStrategy::calcConfig()
 196 {
 197     this->bed_div_x = this->bed_x / float(this->numRows-1);    // Find divisors to calculate the calbration points
 198     this->bed_div_y = this->bed_y / float(this->numCols-1);
 199
 200     // Ensure free program memory for the pData grid
 201     if(this->pData != nullptr) AHB0.dealloc(this->pData);
 202
 203     // Allocate program memory for the pData grid
 204     this->pData = (float *)AHB0.alloc(probe_points * sizeof(float));
 205 }
 206
 207 bool ZGridStrategy::handleGcode(Gcode *gcode)
 208 {
 209     string args = get_arguments(gcode->get_command());
 210
 211      // G code processing
 212     if(gcode->has_g) {
 213         if( gcode->g == 31 ) { // report status
 214
 215                // Bed ZGrid data as gcode:
 216                 gcode->stream->printf(";Bed Level settings:\r\n");
 217
 218                 for (int x=0; x<this->numRows; x++){
 219                     gcode->stream->printf("X%i",x);
 220                     for (int y=0; y<this->numCols; y++){
 221                          gcode->stream->printf(" %c%1.2f", 'A'+y, this->pData[(x*this->numCols)+y]);
 222                     }
 223                     gcode->stream->printf("\r\n");
 224                 }
 225             return true;
 226
 227         } else if( gcode->g == 32 ) { //run probe
 228             // first wait for an empty queue i.e. no moves left
 229             THEKERNEL->conveyor->wait_for_empty_queue();
 230
 231             this->setAdjustFunction(false); // Disable leveling code
 232             if(!doProbing(gcode->stream)) {
 233                 gcode->stream->printf("Probe failed to complete, probe not triggered or other error\n");
 234             } else {
 235                 this->setAdjustFunction(true); // Enable leveling code
 236                 gcode->stream->printf("Probe completed, bed grid defined\n");
 237             }
 238             return true;
 239         }
 240
 241     } else if(gcode->has_m) {
 242         switch( gcode->m ) {
 243
 244             // manual bed ZGrid calbration: M370 - M375
 245             // M370: Clear current ZGrid for calibration, and move to first position
 246             case 370: {
 247                 this->setAdjustFunction(false); // Disable leveling code
 248                 this->cal[Z_AXIS] = std::get<Z_AXIS>(this->probe_offsets) + zprobe->getProbeHeight();
 249
 250
 251                 if(gcode->has_letter('X'))  // Rows (X)
 252                     this->numRows = gcode->get_value('X');
 253                 if(gcode->has_letter('Y'))  // Cols (Y)
 254                     this->numCols = gcode->get_value('Y');
 255
 256                 this->calcConfig();                // Run calculations for Grid size and allocate grid memory
 257
 258                 this->homexyz();
 259                 for (int i=0; i<probe_points; i++){
 260                     this->pData[i] = 0.0F;        // Clear the ZGrid
 261                 }
 262
 263                 this->cal[X_AXIS] = 0.0f;                                              // Clear calibration position
 264                 this->cal[Y_AXIS] = 0.0f;
 265                 this->in_cal = true;                                         // In calbration mode
 266
 267             }
 268             return true;
 269             // M371: Move to next manual calibration position
 270             case 371: {
 271                 if (in_cal){
 272                     this->move(this->cal, slow_rate);
 273                     this->next_cal();
 274                 }
 275
 276             }
 277             return true;
 278             // M372: save current position in ZGrid, and move to next calibration position
 279             case 372: {
 280                 if (in_cal){
 281                     float cartesian[3];
 282                     int pindex = 0;
 283
 284                     THEKERNEL->robot->get_axis_position(cartesian);         // get actual position from robot
 285
 286                     pindex = int(cartesian[X_AXIS]/this->bed_div_x + 0.25)*this->numCols + int(cartesian[Y_AXIS]/this->bed_div_y + 0.25);
 287
 288                     this->move(this->cal, slow_rate);                       // move to the next position
 289                     this->next_cal();                                       // to not cause damage to machine due to Z-offset
 290
 291                     this->pData[pindex] = cartesian[Z_AXIS];  // save the offset
 292
 293                 }
 294             }
 295             return true;
 296             // M373: finalize calibration
 297             case 373: {
 298                  // normalize the grid
 299                  this->normalize_grid();
 300
 301                  this->in_cal = false;
 302                  this->setAdjustFunction(true); // Enable leveling code
 303
 304             }
 305             return true;
 306
 307             // M374: Save grid
 308             case 374:{
 309                 char gridname[5];
 310
 311                 if(gcode->has_letter('S'))  // Custom grid number
 312                     snprintf(gridname, sizeof(gridname), "S%03.0f", gcode->get_value('S'));
 313                 else
 314                     gridname[0] = '\0';
 315
 316                 if(this->saveGrid(gridname)) {
 317                     gcode->stream->printf("Grid saved: Filename: /sd/Zgrid.%s\n",gridname);
 318                 }
 319                 else {
 320                     gcode->stream->printf("Error: Grid not saved: Filename: /sd/Zgrid.%s\n",gridname);
 321                 }
 322             }
 323             return true;
 324
 325             case 375:{ // Load grid values
 326                 char gridname[5];
 327
 328                 if(gcode->has_letter('S'))  // Custom grid number
 329                     snprintf(gridname, sizeof(gridname), "S%03.0f", gcode->get_value('S'));
 330                 else
 331                     gridname[0] = '\0';
 332
 333                 if(this->loadGrid(gridname)) {
 334                     this->setAdjustFunction(true); // Enable leveling code
 335                     gcode->stream->printf("Grid loaded: /sd/Zgrid.%s\n",gridname);
 336                 }
 337                 else {
 338                     gcode->stream->printf("Error: Grid not loaded: /sd/Zgrid.%s\n",gridname);
 339                 }
 340             }
 341             return true;
 342
 343 /*          case 376: { // Check grid value calculations: For debug only.
 344                 float target[3];
 345
 346                 for(char letter = 'X'; letter <= 'Z'; letter++) {
 347                     if( gcode->has_letter(letter) ) {
 348                          target[letter - 'X'] = gcode->get_value(letter);
 349                     }
 350                 }
 351                 gcode->stream->printf(" Z0 %1.3f\n",getZOffset(target[0], target[1]));
 352
 353             }
 354             return true;
 355 */
 356             case 565: { // M565: Set Z probe offsets
 357                 float x= 0, y= 0, z= 0;
 358                 if(gcode->has_letter('X')) x = gcode->get_value('X');
 359                 if(gcode->has_letter('Y')) y = gcode->get_value('Y');
 360                 if(gcode->has_letter('Z')) z = gcode->get_value('Z');
 361                 probe_offsets = std::make_tuple(x, y, z);
 362             }
 363             return true;
 364
 365             case 500: // M500 saves probe_offsets config override file
 366                 gcode->stream->printf(";Load default grid\nM375\n");
 367
 368
 369             case 503: { // M503 just prints the settings
 370
 371                 float x,y,z;
 372                 gcode->stream->printf(";Probe offsets:\n");
 373                 std::tie(x, y, z) = probe_offsets;
 374                 gcode->stream->printf("M565 X%1.5f Y%1.5f Z%1.5f\n", x, y, z);
 375                 break;
 376             }
 377
 378             return true;
 379         }
 380     }
 381
 382     return false;
 383 }
 384
 385
 386 bool ZGridStrategy::saveGrid(std::string args)
 387 {
 388     args = "/sd/Zgrid." + args;
 389     StreamOutput *ZMap_file = new FileStream(args.c_str());
 390
 391     ZMap_file->printf("P%i %i %i %1.3f\n", probe_points, this->numRows, this->numCols, getZhomeoffset());    // Store probe points to prevent loading undefined grid files
 392
 393     for (int pos = 0; pos < probe_points; pos++){
 394         ZMap_file->printf("%1.3f\n", this->pData[pos]);
 395     }
 396     delete ZMap_file;
 397
 398     return true;
 399
 400 }
 401
 402 bool ZGridStrategy::loadGrid(std::string args)
 403 {
 404     char flag[20];
 405
 406     int fpoints, GridX = 5, GridY = 5;   // for 25point file
 407     float val, GridZ;
 408
 409     args = "/sd/Zgrid." + args;
 410     FILE *fd = fopen(args.c_str(), "r");
 411     if(fd != NULL) {
 412         fscanf(fd, "%s\n", flag);
 413
 414         if (flag[0] == 'P'){
 415
 416             sscanf(flag, "P%i\n", &fpoints);                        // read number of points, and Grid X and Y
 417             fscanf(fd, "%i %i %f\n", &GridX, &GridY, &GridZ);       // read number of points, and Grid X and Y and ZHoming offset
 418             fscanf(fd, "%f\n", &val);                               // read first value from file
 419
 420         } else {  // original 25point file -- Backwards compatibility
 421             fpoints = 25;
 422             sscanf(flag, "%f\n", &val);                             // read first value from string
 423         }
 424
 425         if (GridX != this->numRows || GridY != this->numCols){
 426             this->numRows = GridX;                                  // Change Rows and Columns to match the saved data
 427             this->numCols = GridY;
 428             this->calcConfig();                                     // Reallocate memory for the grid according to the grid loaded
 429         }
 430
 431         this->pData[0] = val;    // Place the first read value in grid
 432
 433         for (int pos = 1; pos < probe_points; pos++){
 434             fscanf(fd, "%f\n", &val);
 435             this->pData[pos] = val;
 436         }
 437
 438         fclose(fd);
 439
 440         this->setZoffset(GridZ);
 441
 442         return true;
 443
 444     } else {
 445         return false;
 446     }
 447
 448 }
 449
 450 float ZGridStrategy::getZhomeoffset()
 451 {
 452     void* rd;
 453
 454     bool ok = PublicData::get_value( endstops_checksum, home_offset_checksum, &rd );
 455
 456     if (ok) {
 457       return ((float*)rd)[2];
 458     }
 459
 460     return 0;
 461 }
 462
 463 void ZGridStrategy::setZoffset(float zval)
 464 {
 465     char cmd[64];
 466
 467     // Assemble Gcode to add onto the queue
 468     snprintf(cmd, sizeof(cmd), "M206 Z%1.3f", zval); // Send saved Z homing offset
 469
 470     Gcode gc(cmd, &(StreamOutput::NullStream));
 471     THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc);
 472
 473 }
 474
 475 bool ZGridStrategy::doProbing(StreamOutput *stream)  // probed calibration
 476 {
 477     // home first using selected mode: NOHOME, HOMEXY, HOMEXYZ
 478     this->homexyz();
 479
 480     // deactivate correction during moves
 481     this->setAdjustFunction(false);
 482
 483     for (int i=0; i<probe_points; i++){
 484        this->pData[i] = 0.0F;        // Clear the ZGrid
 485     }
 486
 487     if (this->wait_for_probe){
 488
 489         this->cal[X_AXIS] = this->bed_x/2.0f;
 490         this->cal[Y_AXIS] = this->bed_y/2.0f;
 491         this->cal[Z_AXIS] = this->bed_z/2.0f;           // Position head for probe attachment
 492         this->move(this->cal, slow_rate);               // Move to probe attachment point
 493
 494         stream->printf("*** Ensure probe is attached and press probe when done ***\n");
 495
 496         while(!zprobe->getProbeStatus()){            // Wait for button press
 497             THEKERNEL->call_event(ON_IDLE);
 498         };
 499     }
 500
 501     this->in_cal = true;                         // In calbration mode
 502
 503     this->cal[X_AXIS] = 0.0f;                    // Clear calibration position
 504     this->cal[Y_AXIS] = 0.0f;
 505     this->cal[Z_AXIS] = std::get<Z_AXIS>(this->probe_offsets) + zprobe->getProbeHeight();
 506
 507     this->move(this->cal, slow_rate);            // Move to probe start point
 508
 509     for (int probes = 0; probes < probe_points; probes++){
 510         int pindex = 0;
 511
 512         // z = z home offset - probed distance
 513         float z = getZhomeoffset() -zprobe->probeDistance((this->cal[X_AXIS] + this->cal_offset_x)-std::get<X_AXIS>(this->probe_offsets),
 514                                                (this->cal[Y_AXIS] + this->cal_offset_y)-std::get<Y_AXIS>(this->probe_offsets));
 515
 516         pindex = int(this->cal[X_AXIS]/this->bed_div_x + 0.25)*this->numCols + int(this->cal[Y_AXIS]/this->bed_div_y + 0.25);
 517
 518         if (probes == (probe_points-1) && this->wait_for_probe){  // Only move to removal position if probe confirmation was selected
 519             this->cal[X_AXIS] = this->bed_x/2.0f;                 // Else machine will return to first probe position when done
 520             this->cal[Y_AXIS] = this->bed_y/2.0f;
 521             this->cal[Z_AXIS] = this->bed_z/2.0f;                 // Position head for probe removal
 522         } else {
 523             this->next_cal();                                     // to not cause damage to machine due to Z-offset
 524         }
 525         this->pData[pindex] = z ;                                 // save the offset
 526     }
 527
 528     stream->printf("\nCalibration done.  Please remove probe\n");
 529
 530     // activate correction
 531     this->normalize_grid();
 532     this->setAdjustFunction(true);
 533
 534     this->in_cal = false;
 535
 536     return true;
 537 }
 538
 539
 540 void ZGridStrategy::normalize_grid()
 541 {
 542     float min = 100.0F,    // set large start value
 543           norm_offset = 0;
 544
 545     // find minimum value in offset grid
 546     for (int i = 0; i < probe_points; i++)
 547         if (this->pData[i] < min)
 548           min = this->pData[i];
 549
 550     // creates addition offset to set minimum value to zero.
 551     norm_offset = -min;
 552
 553     // adds the offset to create a table of deltas, normalzed to minimum zero
 554     for (int i = 0; i < probe_points; i++)
 555         this->pData[i] += norm_offset;
 556
 557    // add the offset to the current Z homing offset to preserve full probed offset.
 558    this->setZoffset(getZhomeoffset() + norm_offset);
 559 }
 560
 561 void ZGridStrategy::homexyz()
 562 {
 563
 564   switch(this->home_before_probe) {
 565     case NOHOME : return;
 566
 567     case HOMEXY : {
 568         Gcode gc("G28 X0 Y0", &(StreamOutput::NullStream));
 569         THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc);
 570         break;
 571     }
 572
 573     case HOMEXYZ : {
 574         Gcode gc("G28", &(StreamOutput::NullStream));
 575         THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc);
 576         break;
 577     }
 578   }
 579 }
 580
 581 void ZGridStrategy::move(float *position, float feed)
 582 {
 583     char cmd[64];
 584
 585     // Assemble Gcode to add onto the queue.  Also translate the position for non standard cartesian spaces (cal_offset)
 586     snprintf(cmd, sizeof(cmd), "G0 X%1.3f Y%1.3f Z%1.3f F%1.1f", position[0] + this->cal_offset_x, position[1] + this->cal_offset_y, position[2], feed * 60); // use specified feedrate (mm/sec)
 587
 588     //THEKERNEL->streams->printf("DEBUG: move: %s cent: %i\n", cmd, this->center_zero);
 589
 590     Gcode gc(cmd, &(StreamOutput::NullStream));
 591     THEKERNEL->robot->on_gcode_received(&gc); // send to robot directly
 592 }
 593
 594
 595 void ZGridStrategy::next_cal(void){
 596     if ((((int) roundf(this->cal[X_AXIS] / this->bed_div_x)) & 1) != 0){  // Odd row
 597         this->cal[Y_AXIS] -= this->bed_div_y;
 598         if (this->cal[Y_AXIS] < (0.0F - (bed_div_y / 2.0f))){
 599
 600             //THEKERNEL->streams->printf("DEBUG: Y (%f) < cond (%f)\n",this->cal[Y_AXIS], 0.0F);
 601
 602             this->cal[X_AXIS] += bed_div_x;
 603             if (this->cal[X_AXIS] > (this->bed_x + (this->bed_div_x / 2.0f))){
 604                 this->cal[X_AXIS] = 0.0F;
 605                 this->cal[Y_AXIS] = 0.0F;
 606             }
 607             else
 608                 this->cal[Y_AXIS] = 0.0F;
 609         }
 610     }
 611     else {                                          // Even row (0 is an even row - starting point)
 612         this->cal[Y_AXIS] += bed_div_y;
 613       if (this->cal[Y_AXIS] > (this->bed_y + (bed_div_y / 2.0f))){
 614
 615             //THEKERNEL->streams->printf("DEBUG: Y (%f) > cond (%f)\n",this->cal[Y_AXIS], this->bed_y);
 616
 617             this->cal[X_AXIS] += bed_div_x;
 618             if (this->cal[X_AXIS] > (this->bed_x + (this->bed_div_x / 2.0f))){
 619                 this->cal[X_AXIS] = 0.0F;
 620                 this->cal[Y_AXIS] = 0.0F;
 621             }
 622             else
 623                 this->cal[Y_AXIS] = this->bed_y;
 624         }
 625     }
 626 }
 627
 628
 629 void ZGridStrategy::setAdjustFunction(bool on)
 630 {
 631     if(on) {
 632         // set the compensationTransform in robot
 633         THEKERNEL->robot->compensationTransform= [this](float target[3]) { target[2] += this->getZOffset(target[0], target[1]); };
 634     }else{
 635         // clear it
 636         THEKERNEL->robot->compensationTransform= nullptr;
 637     }
 638 }
 639
 640
 641 // find the Z offset for the point on the plane at x, y
 642 float ZGridStrategy::getZOffset(float X, float Y)
 643 {
 644     int xIndex2, yIndex2;
 645
 646     // Subtract calibration offsets as applicable
 647     X -= this->cal_offset_x;
 648     Y -= this->cal_offset_y;
 649
 650     float xdiff = X / this->bed_div_x;
 651     float ydiff = Y / this->bed_div_y;
 652
 653     float dCartX1, dCartX2;
 654
 655     // Get floor of xdiff.  Note that (int) of a negative number is its
 656     // ceiling, not its floor.
 657
 658     int xIndex = (int)(floorf(xdiff));  // Get the current sector (X)
 659     int yIndex = (int)(floorf(ydiff));  // Get the current sector (Y)
 660
 661     // Index bounds limited to be inside the table
 662     if (xIndex < 0) xIndex = 0;
 663     else if (xIndex > (this->numRows - 2)) xIndex = this->numRows - 2;
 664
 665     if (yIndex < 0) yIndex = 0;
 666     else if (yIndex > (this->numCols - 2)) yIndex = this->numCols - 2;
 667
 668     xIndex2 = xIndex+1;
 669     yIndex2 = yIndex+1;
 670
 671     xdiff -= xIndex;                    // Find floating point
 672     ydiff -= yIndex;                    // Find floating point
 673
 674     dCartX1 = (1-xdiff) * this->pData[(xIndex*this->numCols)+yIndex] + (xdiff) * this->pData[(xIndex2)*this->numCols+yIndex];
 675     dCartX2 = (1-xdiff) * this->pData[(xIndex*this->numCols)+yIndex2] + (xdiff) * this->pData[(xIndex2)*this->numCols+yIndex2];
 676
 677     return ydiff * dCartX2 + (1-ydiff) * dCartX1;    // Calculated Z-delta
 678
 679 }
 680
 681 // parse a "X,Y,Z" string return x,y,z tuple
 682 std::tuple<float, float, float> ZGridStrategy::parseXYZ(const char *str)
 683 {
 684     float x = 0, y = 0, z= 0;
 685     char *p;
 686     x = strtof(str, &p);
 687     if(p + 1 < str + strlen(str)) {
 688         y = strtof(p + 1, &p);
 689         if(p + 1 < str + strlen(str)) {
 690             z = strtof(p + 1, nullptr);
 691         }
 692     }
 693     return std::make_tuple(x, y, z);
 694 }
 695