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