src/modules/tools/zprobe/DeltaCalibrationStrategy.cpp

   1 #include "DeltaCalibrationStrategy.h"
   2 #include "Kernel.h"
   3 #include "Config.h"
   4 #include "Robot.h"
   5 #include "StreamOutputPool.h"
   6 #include "Gcode.h"
   7 #include "checksumm.h"
   8 #include "ConfigValue.h"
   9 #include "PublicDataRequest.h"
  10 #include "EndstopsPublicAccess.h"
  11 #include "PublicData.h"
  12 #include "Conveyor.h"
  13 #include "ZProbe.h"
  14 #include "BaseSolution.h"
  15 #include "StepperMotor.h"
  16
  17 #include <tuple>
  18 #include <algorithm>
  19
  20 #define radius_checksum         CHECKSUM("radius")
  21 #define initial_height_checksum CHECKSUM("initial_height")
  22
  23 // deprecated
  24 #define probe_radius_checksum CHECKSUM("probe_radius")
  25
  26 bool DeltaCalibrationStrategy::handleConfig()
  27 {
  28     // default is probably wrong
  29     float r= THEKERNEL->config->value(leveling_strategy_checksum, delta_calibration_strategy_checksum, radius_checksum)->by_default(-1)->as_number();
  30     if(r == -1) {
  31         // deprecated config syntax]
  32         r =  THEKERNEL->config->value(zprobe_checksum, probe_radius_checksum)->by_default(100.0F)->as_number();
  33     }
  34     this->probe_radius= r;
  35
  36     // the initial height above the bed we stop the intial move down after home to find the bed
  37     // 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)
  38     this->initial_height= THEKERNEL->config->value(leveling_strategy_checksum, delta_calibration_strategy_checksum, initial_height_checksum)->by_default(10)->as_number();
  39     return true;
  40 }
  41
  42 bool DeltaCalibrationStrategy::handleGcode(Gcode *gcode)
  43 {
  44     if( gcode->has_g) {
  45         // G code processing
  46         if( gcode->g == 32 ) { // auto calibration for delta, Z bed mapping for cartesian
  47             // first wait for an empty queue i.e. no moves left
  48             THEKERNEL->conveyor->wait_for_idle();
  49
  50             // turn off any compensation transform as it will be invalidated anyway by this
  51             THEROBOT->compensationTransform= nullptr;
  52
  53             if(!gcode->has_letter('R')) {
  54                 if(!calibrate_delta_endstops(gcode)) {
  55                     gcode->stream->printf("Calibration failed to complete, check the initial probe height and/or initial_height settings\n");
  56                     return true;
  57                 }
  58             }
  59             if(!gcode->has_letter('E')) {
  60                 if(!calibrate_delta_radius(gcode)) {
  61                     gcode->stream->printf("Calibration failed to complete, check the initial probe height and/or initial_height settings\n");
  62                     return true;
  63                 }
  64             }
  65             gcode->stream->printf("Calibration complete, save settings with M500\n");
  66             return true;
  67
  68         }else if (gcode->g == 29) {
  69             // probe the 7 points
  70             if(!probe_delta_points(gcode)) {
  71                 gcode->stream->printf("Calibration failed to complete, check the initial probe height and/or initial_height settings\n");
  72             }
  73             return true;
  74         }
  75
  76     } else if(gcode->has_m) {
  77         // handle mcodes
  78     }
  79
  80     return false;
  81 }
  82
  83 // calculate the X and Y positions for the three towers given the radius from the center
  84 static std::tuple<float, float, float, float, float, float> getCoordinates(float radius)
  85 {
  86     float px = 0.866F * radius; // ~sin(60)
  87     float py = 0.5F * radius; // cos(60)
  88     float t1x = -px, t1y = -py; // X Tower
  89     float t2x = px, t2y = -py; // Y Tower
  90     float t3x = 0.0F, t3y = radius; // Z Tower
  91     return std::make_tuple(t1x, t1y, t2x, t2y, t3x, t3y);
  92 }
  93
  94
  95 // Probes the 7 points on a delta can be used for off board calibration
  96 bool DeltaCalibrationStrategy::probe_delta_points(Gcode *gcode)
  97 {
  98     float bedht= findBed();
  99     if(isnan(bedht)) return false;
 100
 101     gcode->stream->printf("initial Bed ht is %f mm\n", bedht);
 102
 103     // check probe ht
 104     float mm;
 105     if(!zprobe->doProbeAt(mm, 0, 0)) return false;
 106     float dz = zprobe->getProbeHeight() - mm;
 107     gcode->stream->printf("center probe: %1.4f\n", dz);
 108
 109     // get probe points
 110     float t1x, t1y, t2x, t2y, t3x, t3y;
 111     std::tie(t1x, t1y, t2x, t2y, t3x, t3y) = getCoordinates(this->probe_radius);
 112
 113     // gather probe points
 114     float pp[][2] {{t1x, t1y}, {t2x, t2y}, {t3x, t3y}, {0, 0}, {-t1x, -t1y}, {-t2x, -t2y}, {-t3x, -t3y}};
 115
 116     float max_delta= 0;
 117     float last_z= NAN;
 118     float start_z= THEROBOT->actuators[2]->get_current_position();
 119
 120     for(auto& i : pp) {
 121         float mm;
 122         if(!zprobe->doProbeAt(mm, i[0], i[1])) return false;
 123         float z = mm;
 124         if(gcode->subcode == 0) {
 125             // prints the delta Z moved at the XY coordinates given
 126             gcode->stream->printf("X:%1.4f Y:%1.4f Z:%1.4f\n", i[0], i[1], z);
 127
 128         }else if(gcode->subcode == 1) {
 129             // format that can be pasted here http://escher3d.com/pages/wizards/wizarddelta.php
 130             gcode->stream->printf("X%1.4f Y%1.4f Z%1.4f\n", i[0], i[1], start_z - z); // actual Z of bed at probe point
 131         }
 132
 133         if(isnan(last_z)) {
 134             last_z= z;
 135         }else{
 136             max_delta= std::max(max_delta, fabsf(z-last_z));
 137         }
 138     }
 139
 140     gcode->stream->printf("max delta: %f\n", max_delta);
 141
 142     return true;
 143 }
 144
 145 float DeltaCalibrationStrategy::findBed()
 146 {
 147     // home
 148     zprobe->home();
 149
 150     // 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
 151     float deltaz= zprobe->getMaxZ() - initial_height;
 152     zprobe->coordinated_move(NAN, NAN, -deltaz, zprobe->getFastFeedrate(), true);
 153
 154     // find bed, run at slow rate so as to not hit bed hard
 155     float mm;
 156     if(!zprobe->run_probe(mm, false)) return NAN;
 157     zprobe->return_probe(mm);
 158
 159     // leave the probe zprobe->getProbeHeight() above bed
 160     float dz= zprobe->getProbeHeight() - mm;
 161     zprobe->coordinated_move(NAN, NAN, dz, zprobe->getFastFeedrate(), true); // relative move
 162
 163     return mm + deltaz - zprobe->getProbeHeight(); // distance to move from home to 5mm above bed
 164 }
 165
 166 /* Run a calibration routine for a delta
 167     1. Home
 168     2. probe for z bed
 169     3. probe initial tower positions
 170     4. set initial trims such that trims will be minimal negative values
 171     5. home, probe three towers again
 172     6. calculate trim offset and apply to all trims
 173     7. repeat 5, 6 until it converges on a solution
 174 */
 175
 176 bool DeltaCalibrationStrategy::calibrate_delta_endstops(Gcode *gcode)
 177 {
 178     float target = 0.03F;
 179     if(gcode->has_letter('I')) target = gcode->get_value('I'); // override default target
 180     if(gcode->has_letter('J')) this->probe_radius = gcode->get_value('J'); // override default probe radius
 181
 182     bool keep = false;
 183     if(gcode->has_letter('K')) keep = true; // keep current settings
 184
 185     gcode->stream->printf("Calibrating Endstops: target %fmm, radius %fmm\n", target, this->probe_radius);
 186
 187     // get probe points
 188     float t1x, t1y, t2x, t2y, t3x, t3y;
 189     std::tie(t1x, t1y, t2x, t2y, t3x, t3y) = getCoordinates(this->probe_radius);
 190
 191     float trimx = 0.0F, trimy = 0.0F, trimz = 0.0F;
 192     if(!keep) {
 193         // zero trim values
 194         if(!set_trim(0, 0, 0, gcode->stream)) return false;
 195
 196     } else {
 197         // get current trim, and continue from that
 198         if (get_trim(trimx, trimy, trimz)) {
 199             gcode->stream->printf("Current Trim X: %f, Y: %f, Z: %f\r\n", trimx, trimy, trimz);
 200
 201         } else {
 202             gcode->stream->printf("Could not get current trim, are endstops enabled?\n");
 203             return false;
 204         }
 205     }
 206
 207     // find the bed, as we potentially have a temporary z probe we don't know how low under the nozzle it is
 208     // so we need to find the initial place that the probe triggers when it hits the bed
 209     float bedht= findBed();
 210     if(isnan(bedht)) return false;
 211     gcode->stream->printf("initial Bed ht is %f mm\n", bedht);
 212
 213     // check probe ht
 214     float mm;
 215     if(!zprobe->doProbeAt(mm, 0, 0)) return false;
 216     float dz = zprobe->getProbeHeight() - mm;
 217     gcode->stream->printf("center probe: %1.4f\n", dz);
 218     if(fabsf(dz) > target) {
 219          gcode->stream->printf("Probe was not repeatable to %f mm, (%f)\n", target, dz);
 220          return false;
 221     }
 222
 223     // get initial probes
 224     // probe the base of the X tower
 225     if(!zprobe->doProbeAt(mm, t1x, t1y)) return false;
 226     float t1z = mm;
 227     gcode->stream->printf("T1-0 Z:%1.4f\n", t1z);
 228
 229     // probe the base of the Y tower
 230     if(!zprobe->doProbeAt(mm, t2x, t2y)) return false;
 231     float t2z = mm;
 232     gcode->stream->printf("T2-0 Z:%1.4f\n", t2z);
 233
 234     // probe the base of the Z tower
 235     if(!zprobe->doProbeAt(mm, t3x, t3y)) return false;
 236     float t3z = mm;
 237     gcode->stream->printf("T3-0 Z:%1.4f\n", t3z);
 238
 239     float trimscale = 1.2522F; // empirically determined
 240
 241     auto mmx = std::minmax({t1z, t2z, t3z});
 242     if((mmx.second - mmx.first) <= target) {
 243         gcode->stream->printf("trim already set within required parameters: delta %f\n", mmx.second - mmx.first);
 244         return true;
 245     }
 246
 247     // set trims to worst case so we always have a negative trim
 248     trimx += (mmx.first - t1z) * trimscale;
 249     trimy += (mmx.first - t2z) * trimscale;
 250     trimz += (mmx.first - t3z) * trimscale;
 251
 252     for (int i = 1; i <= 10; ++i) {
 253         // set trim
 254         if(!set_trim(trimx, trimy, trimz, gcode->stream)) return false;
 255
 256         // home and move probe to start position just above the bed
 257         zprobe->home();
 258         zprobe->coordinated_move(NAN, NAN, -bedht, zprobe->getFastFeedrate(), true); // do a relative move from home to the point above the bed
 259
 260         // probe the base of the X tower
 261         if(!zprobe->doProbeAt(mm, t1x, t1y)) return false;
 262         t1z = mm;
 263         gcode->stream->printf("T1-%d Z:%1.4f\n", i, t1z);
 264
 265         // probe the base of the Y tower
 266         if(!zprobe->doProbeAt(mm, t2x, t2y)) return false;
 267         t2z = mm;
 268         gcode->stream->printf("T2-%d Z:%1.4f\n", i, t2z);
 269
 270         // probe the base of the Z tower
 271         if(!zprobe->doProbeAt(mm, t3x, t3y)) return false;
 272         t3z = mm;
 273         gcode->stream->printf("T3-%d Z:%1.4f\n", i, t3z);
 274
 275         mmx = std::minmax({t1z, t2z, t3z});
 276         if((mmx.second - mmx.first) <= target) {
 277             gcode->stream->printf("trim set to within required parameters: delta %f\n", mmx.second - mmx.first);
 278             break;
 279         }
 280
 281         // set new trim values based on min difference
 282         trimx += (mmx.first - t1z) * trimscale;
 283         trimy += (mmx.first - t2z) * trimscale;
 284         trimz += (mmx.first - t3z) * trimscale;
 285
 286         // flush the output
 287         THEKERNEL->call_event(ON_IDLE);
 288     }
 289
 290     if((mmx.second - mmx.first) > target) {
 291         gcode->stream->printf("WARNING: trim did not resolve to within required parameters: delta %f\n", mmx.second - mmx.first);
 292     }
 293
 294     return true;
 295 }
 296
 297 /*
 298     probe edges to get outer positions, then probe center
 299     modify the delta radius until center and X converge
 300 */
 301
 302 bool DeltaCalibrationStrategy::calibrate_delta_radius(Gcode *gcode)
 303 {
 304     float target = 0.03F;
 305     if(gcode->has_letter('I')) target = gcode->get_value('I'); // override default target
 306     if(gcode->has_letter('J')) this->probe_radius = gcode->get_value('J'); // override default probe radius
 307
 308     gcode->stream->printf("Calibrating delta radius: target %f, radius %f\n", target, this->probe_radius);
 309
 310     // get probe points
 311     float t1x, t1y, t2x, t2y, t3x, t3y;
 312     std::tie(t1x, t1y, t2x, t2y, t3x, t3y) = getCoordinates(this->probe_radius);
 313
 314     // find the bed, as we potentially have a temporary z probe we don't know how low under the nozzle it is
 315     // so we need to find thr initial place that the probe triggers when it hits the bed
 316     float bedht= findBed();
 317     if(isnan(bedht)) return false;
 318     gcode->stream->printf("initial Bed ht is %f mm\n", bedht);
 319
 320     // check probe ht
 321     float mm;
 322     if(!zprobe->doProbeAt(mm, 0, 0)) return false;
 323     float dz = zprobe->getProbeHeight() - mm;
 324     gcode->stream->printf("center probe: %1.4f\n", dz);
 325     if(fabsf(dz) > target) {
 326          gcode->stream->printf("Probe was not repeatable to %f mm, (%f)\n", target, dz);
 327          return false;
 328     }
 329
 330     // probe center to get reference point at this Z height
 331     float dc;
 332     if(!zprobe->doProbeAt(dc, 0, 0)) return false;
 333     gcode->stream->printf("CT Z:%1.3f\n", dc);
 334     float cmm = dc;
 335
 336     // get current delta radius
 337     float delta_radius = 0.0F;
 338     BaseSolution::arm_options_t options;
 339     if(THEROBOT->arm_solution->get_optional(options)) {
 340         delta_radius = options['R'];
 341     }
 342     if(delta_radius == 0.0F) {
 343         gcode->stream->printf("This appears to not be a delta arm solution\n");
 344         return false;
 345     }
 346     options.clear();
 347
 348     bool good= false;
 349     float drinc = 2.5F; // approx
 350     for (int i = 1; i <= 10; ++i) {
 351         // probe t1, t2, t3 and get average, but use coordinated moves, probing center won't change
 352         float dx, dy, dz;
 353         if(!zprobe->doProbeAt(dx, t1x, t1y)) return false;
 354         gcode->stream->printf("T1-%d Z:%1.3f\n", i, dx);
 355         if(!zprobe->doProbeAt(dy, t2x, t2y)) return false;
 356         gcode->stream->printf("T2-%d Z:%1.3f\n", i, dy);
 357         if(!zprobe->doProbeAt(dz, t3x, t3y)) return false;
 358         gcode->stream->printf("T3-%d Z:%1.3f\n", i, dz);
 359
 360         // now look at the difference and reduce it by adjusting delta radius
 361         float m = (dx + dy + dz) / 3.0F;
 362         float d = cmm - m;
 363         gcode->stream->printf("C-%d Z-ave:%1.4f delta: %1.3f\n", i, m, d);
 364
 365         if(fabsf(d) <= target){
 366             good= true;
 367             break; // resolution of success
 368         }
 369
 370         // increase delta radius to adjust for low center
 371         // decrease delta radius to adjust for high center
 372         delta_radius += (d * drinc);
 373
 374         // set the new delta radius
 375         options['R'] = delta_radius;
 376         THEROBOT->arm_solution->set_optional(options);
 377         gcode->stream->printf("Setting delta radius to: %1.4f\n", delta_radius);
 378
 379         zprobe->home();
 380         zprobe->coordinated_move(NAN, NAN, -bedht, zprobe->getFastFeedrate(), true); // needs to be a relative coordinated move
 381
 382         // flush the output
 383         THEKERNEL->call_event(ON_IDLE);
 384     }
 385
 386     if(!good) {
 387         gcode->stream->printf("WARNING: delta radius did not resolve to within required parameters: %f\n", target);
 388     }
 389
 390     return true;
 391 }
 392
 393 bool DeltaCalibrationStrategy::set_trim(float x, float y, float z, StreamOutput *stream)
 394 {
 395     float t[3] {x, y, z};
 396     bool ok = PublicData::set_value( endstops_checksum, trim_checksum, t);
 397
 398     if (ok) {
 399         stream->printf("set trim to X:%f Y:%f Z:%f\n", x, y, z);
 400     } else {
 401         stream->printf("unable to set trim, is endstops enabled?\n");
 402     }
 403
 404     return ok;
 405 }
 406
 407 bool DeltaCalibrationStrategy::get_trim(float &x, float &y, float &z)
 408 {
 409     void *returned_data;
 410     bool ok = PublicData::get_value( endstops_checksum, trim_checksum, &returned_data );
 411
 412     if (ok) {
 413         float *trim = static_cast<float *>(returned_data);
 414         x = trim[0];
 415         y = trim[1];
 416         z = trim[2];
 417         return true;
 418     }
 419     return false;
 420 }