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