src/modules/tools/zprobe/DeltaGridStrategy.cpp

   1 // This code is derived from (and mostly copied from) Johann Rocholls code at https://github.com/jcrocholl/Marlin/blob/deltabot/Marlin/Marlin_main.cpp
   2 // license is the same as his code.
   3
   4 #include "DeltaGridStrategy.h"
   5
   6 #include "Kernel.h"
   7 #include "Config.h"
   8 #include "Robot.h"
   9 #include "StreamOutputPool.h"
  10 #include "Gcode.h"
  11 #include "checksumm.h"
  12 #include "ConfigValue.h"
  13 #include "PublicDataRequest.h"
  14 #include "PublicData.h"
  15 #include "Conveyor.h"
  16 #include "ZProbe.h"
  17 #include "nuts_bolts.h"
  18 #include "utils.h"
  19
  20 #include <string>
  21 #include <algorithm>
  22 #include <cstdlib>
  23 #include <cmath>
  24
  25 #define grid_radius_checksum         CHECKSUM("radius")
  26 #define grid_resolution_checksum     CHECKSUM("resolution")
  27 #define tolerance_checksum           CHECKSUM("tolerance")
  28 #define save_checksum                CHECKSUM("save")
  29 #define probe_offsets_checksum       CHECKSUM("probe_offsets")
  30 #define initial_height_checksum      CHECKSUM("initial_height")
  31
  32 #define GRIDFILE "/sd/delta.grid"
  33
  34 DeltaGridStrategy::DeltaGridStrategy(ZProbe *zprobe) : LevelingStrategy(zprobe)
  35 {
  36     // TODO allocate grid in AHB0 or AHB1
  37     //grid = nullptr;
  38 }
  39
  40 DeltaGridStrategy::~DeltaGridStrategy()
  41 {
  42     //delete[] grid;
  43 }
  44
  45 bool DeltaGridStrategy::handleConfig()
  46 {
  47     grid_radius = THEKERNEL->config->value(leveling_strategy_checksum, delta_grid_leveling_strategy_checksum, grid_radius_checksum)->by_default(50.0F)->as_number();
  48     //grid_resolution = THEKERNEL->config->value(leveling_strategy_checksum, delta_grid_leveling_strategy_checksum, grid_radius_checksum)->by_default(7)->as_number();
  49     tolerance = THEKERNEL->config->value(leveling_strategy_checksum, delta_grid_leveling_strategy_checksum, tolerance_checksum)->by_default(0.03F)->as_number();
  50     save = THEKERNEL->config->value(leveling_strategy_checksum, delta_grid_leveling_strategy_checksum, save_checksum)->by_default(false)->as_bool();
  51     // the initial height above the bed we stop the intial move down after home to find the bed
  52     // 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)
  53     this->initial_height= THEKERNEL->config->value(leveling_strategy_checksum, delta_grid_leveling_strategy_checksum, initial_height_checksum)->by_default(10)->as_number();
  54
  55     // Probe offsets xxx,yyy,zzz
  56     {
  57         std::string po = THEKERNEL->config->value(leveling_strategy_checksum, delta_grid_leveling_strategy_checksum, probe_offsets_checksum)->by_default("0,0,0")->as_string();
  58         std::vector<float> v = parse_number_list(po.c_str());
  59         if(v.size() >= 3) {
  60             this->probe_offsets = std::make_tuple(v[0], v[1], v[2]);
  61         }
  62     }
  63
  64     reset_bed_level();
  65     // load the saved grid file
  66     if(save) load_grid(nullptr);
  67
  68     return true;
  69 }
  70
  71 void DeltaGridStrategy::save_grid(StreamOutput *stream)
  72 {
  73     FILE *fp= fopen(GRIDFILE, "w");
  74     if(fp == NULL) {
  75         stream->printf("error:Failed to open grid\n");
  76         return;
  77     }
  78
  79     for (int y = 0; y < AUTO_BED_LEVELING_GRID_POINTS; y++) {
  80         for (int x = 0; x < AUTO_BED_LEVELING_GRID_POINTS; x++) {
  81             if(fwrite(&grid[x][y], sizeof(float), 1, fp) != 1) {
  82                 stream->printf("error:Failed to write grid\n");
  83                 fclose(fp);
  84                 return;
  85             }
  86         }
  87     }
  88     stream->printf("grid saved to %s\n", GRIDFILE);
  89     fclose(fp);
  90 }
  91
  92 bool DeltaGridStrategy::load_grid(StreamOutput *stream)
  93 {
  94     FILE *fp= fopen(GRIDFILE, "r");
  95     if(fp == NULL) {
  96         if(stream != nullptr) stream->printf("error:Failed to open grid\n");
  97         return false;
  98     }
  99
 100     for (int y = 0; y < AUTO_BED_LEVELING_GRID_POINTS; y++) {
 101         for (int x = 0; x < AUTO_BED_LEVELING_GRID_POINTS; x++) {
 102             if(fread(&grid[x][y], sizeof(float), 1, fp) != 1) {
 103                 if(stream != nullptr) stream->printf("error:Failed to read grid\n");
 104                 fclose(fp);
 105                 return false;
 106             }
 107         }
 108     }
 109     stream->printf("grid loaded from %s\n", GRIDFILE);
 110     fclose(fp);
 111     return true;
 112 }
 113
 114 bool DeltaGridStrategy::handleGcode(Gcode *gcode)
 115 {
 116     if(gcode->has_g) {
 117         if( gcode->g == 31 ) { // do probe (should be 32 but use 31 as deltacalibration will usually also be enabled)
 118              // first wait for an empty queue i.e. no moves left
 119             THEKERNEL->conveyor->wait_for_empty_queue();
 120
 121             if(!doProbe(gcode)) {
 122                 gcode->stream->printf("Probe failed to complete, probe not triggered or other error\n");
 123             } else {
 124                 gcode->stream->printf("Probe completed\n");
 125             }
 126             return true;
 127         }
 128
 129     } else if(gcode->has_m) {
 130         if(gcode->m == 370 || gcode->m == 561) { // M370: Clear bed, M561: Set Identity Transform
 131             // delete the compensationTransform in robot
 132             setAdjustFunction(false);
 133             reset_bed_level();
 134             return true;
 135
 136         } else if(gcode->m == 374) { // M374: Save grid, M374.1: delete saved grid
 137             if(gcode->subcode == 1) {
 138                 remove(GRIDFILE);
 139                 gcode->stream->printf("%s deleted\n", GRIDFILE);
 140             }else{
 141                 save_grid(gcode->stream);
 142             }
 143
 144             return true;
 145
 146         } else if(gcode->m == 375) { // M375: load grid, M375.1 display grid
 147             if(gcode->subcode == 1) {
 148                 print_bed_level(gcode->stream);
 149             }else{
 150                 if(load_grid(gcode->stream)) setAdjustFunction(true);
 151             }
 152
 153         } else if(gcode->m == 565) { // M565: Set Z probe offsets
 154             float x= 0, y= 0, z= 0;
 155             if(gcode->has_letter('X')) x = gcode->get_value('X');
 156             if(gcode->has_letter('Y')) y = gcode->get_value('Y');
 157             if(gcode->has_letter('Z')) z = gcode->get_value('Z');
 158             probe_offsets = std::make_tuple(x, y, z);
 159             return true;
 160
 161         } else if(gcode->m == 500 || gcode->m == 503) { // M500 save, M503 display
 162             float x, y, z;
 163             std::tie(x, y, z) = probe_offsets;
 164             gcode->stream->printf(";Probe offsets:\nM565 X%1.5f Y%1.5f Z%1.5f\n", x, y, z);
 165             if(save && gcode->m == 500) gcode->stream->printf(";Load saved grid\nM375\n");
 166             return true;
 167         }
 168     }
 169
 170     return false;
 171 }
 172
 173 // These are convenience defines to keep the code as close to the original as possible
 174 // set the rectangle in which to probe
 175 #define DELTA_PROBABLE_RADIUS (grid_radius)
 176 #define LEFT_PROBE_BED_POSITION (-DELTA_PROBABLE_RADIUS)
 177 #define RIGHT_PROBE_BED_POSITION (DELTA_PROBABLE_RADIUS)
 178 #define BACK_PROBE_BED_POSITION (DELTA_PROBABLE_RADIUS)
 179 #define FRONT_PROBE_BED_POSITION (-DELTA_PROBABLE_RADIUS)
 180
 181 // probe at the points of a lattice grid
 182 //#define AUTO_BED_LEVELING_GRID_POINTS (grid_resolution)
 183 #define AUTO_BED_LEVELING_GRID_X ((RIGHT_PROBE_BED_POSITION - LEFT_PROBE_BED_POSITION) / (AUTO_BED_LEVELING_GRID_POINTS - 1))
 184 #define AUTO_BED_LEVELING_GRID_Y ((BACK_PROBE_BED_POSITION - FRONT_PROBE_BED_POSITION) / (AUTO_BED_LEVELING_GRID_POINTS - 1))
 185
 186 #define X_PROBE_OFFSET_FROM_EXTRUDER std::get<0>(probe_offsets)
 187 #define Y_PROBE_OFFSET_FROM_EXTRUDER std::get<1>(probe_offsets)
 188 #define Z_PROBE_OFFSET_FROM_EXTRUDER std::get<2>(probe_offsets)
 189
 190 void DeltaGridStrategy::setAdjustFunction(bool on)
 191 {
 192     if(on) {
 193         // set the compensationTransform in robot
 194         THEKERNEL->robot->compensationTransform= [this](float target[3]) { doCompensation(target); };
 195     }else{
 196         // clear it
 197         THEKERNEL->robot->compensationTransform= nullptr;
 198     }
 199 }
 200
 201 float DeltaGridStrategy::findBed()
 202 {
 203     // home
 204     zprobe->home();
 205
 206     // 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
 207     float deltaz= zprobe->getMaxZ() - initial_height;
 208     zprobe->coordinated_move(NAN, NAN, -deltaz, zprobe->getFastFeedrate(), true); // relative move
 209     zprobe->coordinated_move(0, 0, NAN, zprobe->getFastFeedrate()); // move to 0,0
 210
 211     // find bed at 0,0 run at slow rate so as to not hit bed hard
 212     int s;
 213     if(!zprobe->run_probe(s, false)) return NAN;
 214
 215     return zprobe->zsteps_to_mm(s) + deltaz - zprobe->getProbeHeight(); // distance to move from home to 5mm above bed
 216 }
 217
 218 bool DeltaGridStrategy::doProbe(Gcode *gc)
 219 {
 220     reset_bed_level();
 221     setAdjustFunction(false);
 222
 223     float initial_z= findBed();
 224     if(isnan(initial_z)) return false;
 225     gc->stream->printf("initial Bed ht is %f mm\n", initial_z);
 226
 227     // we start the probe zprobe->getProbeHeight() above the bed
 228     zprobe->home();
 229     zprobe->coordinated_move(NAN, NAN, -initial_z, zprobe->getFastFeedrate(), true); // do a relative move from home to the point above the bed
 230
 231     // do first probe for 0,0
 232     int s;
 233     if(!zprobe->doProbeAt(s, -X_PROBE_OFFSET_FROM_EXTRUDER, -Y_PROBE_OFFSET_FROM_EXTRUDER)) return false;
 234     float z_reference = zprobe->getProbeHeight() - zprobe->zsteps_to_mm(s); // this should be zero
 235     gc->stream->printf("probe at 0,0 is %f mm\n", z_reference);
 236
 237     float radius= DELTA_PROBABLE_RADIUS;
 238     if(gc->has_letter('J')) radius = gc->get_value('J'); // override default probe radius
 239
 240     for (int yCount = 0; yCount < AUTO_BED_LEVELING_GRID_POINTS; yCount++) {
 241         float yProbe = FRONT_PROBE_BED_POSITION + AUTO_BED_LEVELING_GRID_Y * yCount;
 242         int xStart, xStop, xInc;
 243         if (yCount % 2) {
 244             xStart = 0;
 245             xStop = AUTO_BED_LEVELING_GRID_POINTS;
 246             xInc = 1;
 247         } else {
 248             xStart = AUTO_BED_LEVELING_GRID_POINTS - 1;
 249             xStop = -1;
 250             xInc = -1;
 251         }
 252
 253         for (int xCount = xStart; xCount != xStop; xCount += xInc) {
 254             float xProbe = LEFT_PROBE_BED_POSITION + AUTO_BED_LEVELING_GRID_X * xCount;
 255
 256             // Avoid probing the corners (outside the round or hexagon print surface) on a delta printer.
 257             float distance_from_center = sqrtf(xProbe * xProbe + yProbe * yProbe);
 258             if (distance_from_center > radius) continue;
 259
 260             if(!zprobe->doProbeAt(s, xProbe-X_PROBE_OFFSET_FROM_EXTRUDER, yProbe-Y_PROBE_OFFSET_FROM_EXTRUDER)) return false;
 261             float measured_z = zprobe->getProbeHeight() - zprobe->zsteps_to_mm(s) - z_reference; // this is the delta z from bed at 0,0
 262             gc->stream->printf("DEBUG: X%1.4f, Y%1.4f, Z%1.4f\n", xProbe-X_PROBE_OFFSET_FROM_EXTRUDER, yProbe-Y_PROBE_OFFSET_FROM_EXTRUDER, measured_z);
 263             grid[xCount][yCount] = measured_z;
 264         }
 265     }
 266
 267     extrapolate_unprobed_bed_level();
 268     print_bed_level(gc->stream);
 269
 270     setAdjustFunction(true);
 271
 272     return true;
 273 }
 274
 275 void DeltaGridStrategy::extrapolate_one_point(int x, int y, int xdir, int ydir)
 276 {
 277     if (!isnan(grid[x][y])) {
 278         return;  // Don't overwrite good values.
 279     }
 280     float a = 2 * grid[x + xdir][y] - grid[x + xdir * 2][y]; // Left to right.
 281     float b = 2 * grid[x][y + ydir] - grid[x][y + ydir * 2]; // Front to back.
 282     float c = 2 * grid[x + xdir][y + ydir] - grid[x + xdir * 2][y + ydir * 2]; // Diagonal.
 283     float median = c;  // Median is robust (ignores outliers).
 284     if (a < b) {
 285         if (b < c) median = b;
 286         if (c < a) median = a;
 287     } else {  // b <= a
 288         if (c < b) median = b;
 289         if (a < c) median = a;
 290     }
 291     grid[x][y] = median;
 292 }
 293
 294 // Fill in the unprobed points (corners of circular print surface)
 295 // using linear extrapolation, away from the center.
 296 void DeltaGridStrategy::extrapolate_unprobed_bed_level()
 297 {
 298     int half = (AUTO_BED_LEVELING_GRID_POINTS - 1) / 2;
 299     for (int y = 0; y <= half; y++) {
 300         for (int x = 0; x <= half; x++) {
 301             if (x + y < 3) continue;
 302             extrapolate_one_point(half - x, half - y, x > 1 ? +1 : 0, y > 1 ? +1 : 0);
 303             extrapolate_one_point(half + x, half - y, x > 1 ? -1 : 0, y > 1 ? +1 : 0);
 304             extrapolate_one_point(half - x, half + y, x > 1 ? +1 : 0, y > 1 ? -1 : 0);
 305             extrapolate_one_point(half + x, half + y, x > 1 ? -1 : 0, y > 1 ? -1 : 0);
 306         }
 307     }
 308 }
 309
 310 void DeltaGridStrategy::doCompensation(float target[3])
 311 {
 312     // Adjust print surface height by linear interpolation over the bed_level array.
 313     int half = (AUTO_BED_LEVELING_GRID_POINTS - 1) / 2;
 314     float grid_x = std::max(0.001F - half, std::min(half - 0.001F, target[X_AXIS] / AUTO_BED_LEVELING_GRID_X));
 315     float grid_y = std::max(0.001F - half, std::min(half - 0.001F, target[Y_AXIS] / AUTO_BED_LEVELING_GRID_Y));
 316     int floor_x = floorf(grid_x);
 317     int floor_y = floorf(grid_y);
 318     float ratio_x = grid_x - floor_x;
 319     float ratio_y = grid_y - floor_y;
 320     float z1 = grid[floor_x + half][floor_y + half];
 321     float z2 = grid[floor_x + half][floor_y + half + 1];
 322     float z3 = grid[floor_x + half + 1][floor_y + half];
 323     float z4 = grid[floor_x + half + 1][floor_y + half + 1];
 324     float left = (1 - ratio_y) * z1 + ratio_y * z2;
 325     float right = (1 - ratio_y) * z3 + ratio_y * z4;
 326     float offset = (1 - ratio_x) * left + ratio_x * right;
 327
 328     target[Z_AXIS] += offset;
 329
 330
 331 /*
 332     THEKERNEL->streams->printf("//DEBUG: TARGET: %f, %f, %f\n", target[0], target[1], target[2]);
 333     THEKERNEL->streams->printf("//DEBUG: grid_x= %f\n", grid_x);
 334     THEKERNEL->streams->printf("//DEBUG: grid_y= %f\n", grid_y);
 335     THEKERNEL->streams->printf("//DEBUG: floor_x= %d\n", floor_x);
 336     THEKERNEL->streams->printf("//DEBUG: floor_y= %d\n", floor_y);
 337     THEKERNEL->streams->printf("//DEBUG: ratio_x= %f\n", ratio_x);
 338     THEKERNEL->streams->printf("//DEBUG: ratio_y= %f\n", ratio_y);
 339     THEKERNEL->streams->printf("//DEBUG: z1= %f\n", z1);
 340     THEKERNEL->streams->printf("//DEBUG: z2= %f\n", z2);
 341     THEKERNEL->streams->printf("//DEBUG: z3= %f\n", z3);
 342     THEKERNEL->streams->printf("//DEBUG: z4= %f\n", z4);
 343     THEKERNEL->streams->printf("//DEBUG: left= %f\n", left);
 344     THEKERNEL->streams->printf("//DEBUG: right= %f\n", right);
 345     THEKERNEL->streams->printf("//DEBUG: offset= %f\n", offset);
 346 */
 347 }
 348
 349
 350 // Print calibration results for plotting or manual frame adjustment.
 351 void DeltaGridStrategy::print_bed_level(StreamOutput *stream)
 352 {
 353     for (int y = 0; y < AUTO_BED_LEVELING_GRID_POINTS; y++) {
 354         for (int x = 0; x < AUTO_BED_LEVELING_GRID_POINTS; x++) {
 355             stream->printf("%1.4f ", grid[x][y]);
 356         }
 357         stream->printf("\n");
 358     }
 359 }
 360
 361 // Reset calibration results to zero.
 362 void DeltaGridStrategy::reset_bed_level()
 363 {
 364     for (int y = 0; y < AUTO_BED_LEVELING_GRID_POINTS; y++) {
 365         for (int x = 0; x < AUTO_BED_LEVELING_GRID_POINTS; x++) {
 366             grid[x][y] = NAN;
 367         }
 368     }
 369 }