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_return(mm, zprobe->getSlowFeedrate())) return NAN;
 157
 158     // leave the probe zprobe->getProbeHeight() above bed
 159     float dz= zprobe->getProbeHeight() - mm;
 160     zprobe->coordinated_move(NAN, NAN, dz, zprobe->getFastFeedrate(), true); // relative move
 161
 162     return mm + deltaz - zprobe->getProbeHeight(); // distance to move from home to 5mm above bed
 163 }
 164
 165 /* Run a calibration routine for a delta
 166     1. Home
 167     2. probe for z bed
 168     3. probe initial tower positions
 169     4. set initial trims such that trims will be minimal negative values
 170     5. home, probe three towers again
 171     6. calculate trim offset and apply to all trims
 172     7. repeat 5, 6 until it converges on a solution
 173 */
 174
 175 bool DeltaCalibrationStrategy::calibrate_delta_endstops(Gcode *gcode)
 176 {
 177     float target = 0.03F;
 178     if(gcode->has_letter('I')) target = gcode->get_value('I'); // override default target
 179     if(gcode->has_letter('J')) this->probe_radius = gcode->get_value('J'); // override default probe radius
 180
 181     bool keep = false;
 182     if(gcode->has_letter('K')) keep = true; // keep current settings
 183
 184     gcode->stream->printf("Calibrating Endstops: target %fmm, radius %fmm\n", target, this->probe_radius);
 185
 186     // get probe points
 187     float t1x, t1y, t2x, t2y, t3x, t3y;
 188     std::tie(t1x, t1y, t2x, t2y, t3x, t3y) = getCoordinates(this->probe_radius);
 189
 190     float trimx = 0.0F, trimy = 0.0F, trimz = 0.0F;
 191     if(!keep) {
 192         // zero trim values
 193         if(!set_trim(0, 0, 0, gcode->stream)) return false;
 194
 195     } else {
 196         // get current trim, and continue from that
 197         if (get_trim(trimx, trimy, trimz)) {
 198             gcode->stream->printf("Current Trim X: %f, Y: %f, Z: %f\r\n", trimx, trimy, trimz);
 199
 200         } else {
 201             gcode->stream->printf("Could not get current trim, are endstops enabled?\n");
 202             return false;
 203         }
 204     }
 205
 206     // find the bed, as we potentially have a temporary z probe we don't know how low under the nozzle it is
 207     // so we need to find the initial place that the probe triggers when it hits the bed
 208     float bedht= findBed();
 209     if(isnan(bedht)) return false;
 210     gcode->stream->printf("initial Bed ht is %f mm\n", bedht);
 211
 212     // check probe ht
 213     float mm;
 214     if(!zprobe->doProbeAt(mm, 0, 0)) return false;
 215     float dz = zprobe->getProbeHeight() - mm;
 216     gcode->stream->printf("center probe: %1.4f\n", dz);
 217     if(fabsf(dz) > target) {
 218          gcode->stream->printf("Probe was not repeatable to %f mm, (%f)\n", target, dz);
 219          return false;
 220     }
 221
 222     // get initial probes
 223     // probe the base of the X tower
 224     if(!zprobe->doProbeAt(mm, t1x, t1y)) return false;
 225     float t1z = mm;
 226     gcode->stream->printf("T1-0 Z:%1.4f\n", t1z);
 227
 228     // probe the base of the Y tower
 229     if(!zprobe->doProbeAt(mm, t2x, t2y)) return false;
 230     float t2z = mm;
 231     gcode->stream->printf("T2-0 Z:%1.4f\n", t2z);
 232
 233     // probe the base of the Z tower
 234     if(!zprobe->doProbeAt(mm, t3x, t3y)) return false;
 235     float t3z = mm;
 236     gcode->stream->printf("T3-0 Z:%1.4f\n", t3z);
 237
 238     float trimscale = 1.2522F; // empirically determined
 239
 240     auto mmx = std::minmax({t1z, t2z, t3z});
 241     if((mmx.second - mmx.first) <= target) {
 242         gcode->stream->printf("trim already set within required parameters: delta %f\n", mmx.second - mmx.first);
 243         return true;
 244     }
 245
 246     // set trims to worst case so we always have a negative trim
 247     trimx += (mmx.first - t1z) * trimscale;
 248     trimy += (mmx.first - t2z) * trimscale;
 249     trimz += (mmx.first - t3z) * trimscale;
 250
 251     for (int i = 1; i <= 10; ++i) {
 252         // set trim
 253         if(!set_trim(trimx, trimy, trimz, gcode->stream)) return false;
 254
 255         // home and move probe to start position just above the bed
 256         zprobe->home();
 257         zprobe->coordinated_move(NAN, NAN, -bedht, zprobe->getFastFeedrate(), true); // do a relative move from home to the point above the bed
 258
 259         // probe the base of the X tower
 260         if(!zprobe->doProbeAt(mm, t1x, t1y)) return false;
 261         t1z = mm;
 262         gcode->stream->printf("T1-%d Z:%1.4f\n", i, t1z);
 263
 264         // probe the base of the Y tower
 265         if(!zprobe->doProbeAt(mm, t2x, t2y)) return false;
 266         t2z = mm;
 267         gcode->stream->printf("T2-%d Z:%1.4f\n", i, t2z);
 268
 269         // probe the base of the Z tower
 270         if(!zprobe->doProbeAt(mm, t3x, t3y)) return false;
 271         t3z = mm;
 272         gcode->stream->printf("T3-%d Z:%1.4f\n", i, t3z);
 273
 274         mmx = std::minmax({t1z, t2z, t3z});
 275         if((mmx.second - mmx.first) <= target) {
 276             gcode->stream->printf("trim set to within required parameters: delta %f\n", mmx.second - mmx.first);
 277             break;
 278         }
 279
 280         // set new trim values based on min difference
 281         trimx += (mmx.first - t1z) * trimscale;
 282         trimy += (mmx.first - t2z) * trimscale;
 283         trimz += (mmx.first - t3z) * trimscale;
 284
 285         // flush the output
 286         THEKERNEL->call_event(ON_IDLE);
 287     }
 288
 289     if((mmx.second - mmx.first) > target) {
 290         gcode->stream->printf("WARNING: trim did not resolve to within required parameters: delta %f\n", mmx.second - mmx.first);
 291     }
 292
 293     return true;
 294 }
 295
 296 /*
 297     probe edges to get outer positions, then probe center
 298     modify the delta radius until center and X converge
 299 */
 300
 301 bool DeltaCalibrationStrategy::calibrate_delta_radius(Gcode *gcode)
 302 {
 303     float target = 0.03F;
 304     if(gcode->has_letter('I')) target = gcode->get_value('I'); // override default target
 305     if(gcode->has_letter('J')) this->probe_radius = gcode->get_value('J'); // override default probe radius
 306
 307     gcode->stream->printf("Calibrating delta radius: target %f, radius %f\n", target, this->probe_radius);
 308
 309     // get probe points
 310     float t1x, t1y, t2x, t2y, t3x, t3y;
 311     std::tie(t1x, t1y, t2x, t2y, t3x, t3y) = getCoordinates(this->probe_radius);
 312
 313     // find the bed, as we potentially have a temporary z probe we don't know how low under the nozzle it is
 314     // so we need to find thr initial place that the probe triggers when it hits the bed
 315     float bedht= findBed();
 316     if(isnan(bedht)) return false;
 317     gcode->stream->printf("initial Bed ht is %f mm\n", bedht);
 318
 319     // check probe ht
 320     float mm;
 321     if(!zprobe->doProbeAt(mm, 0, 0)) return false;
 322     float dz = zprobe->getProbeHeight() - mm;
 323     gcode->stream->printf("center probe: %1.4f\n", dz);
 324     if(fabsf(dz) > target) {
 325          gcode->stream->printf("Probe was not repeatable to %f mm, (%f)\n", target, dz);
 326          return false;
 327     }
 328
 329     // probe center to get reference point at this Z height
 330     float dc;
 331     if(!zprobe->doProbeAt(dc, 0, 0)) return false;
 332     gcode->stream->printf("CT Z:%1.3f\n", dc);
 333     float cmm = dc;
 334
 335     // get current delta radius
 336     float delta_radius = 0.0F;
 337     BaseSolution::arm_options_t options;
 338     if(THEROBOT->arm_solution->get_optional(options)) {
 339         delta_radius = options['R'];
 340     }
 341     if(delta_radius == 0.0F) {
 342         gcode->stream->printf("This appears to not be a delta arm solution\n");
 343         return false;
 344     }
 345     options.clear();
 346
 347     bool good= false;
 348     float drinc = 2.5F; // approx
 349     for (int i = 1; i <= 10; ++i) {
 350         // probe t1, t2, t3 and get average, but use coordinated moves, probing center won't change
 351         float dx, dy, dz;
 352         if(!zprobe->doProbeAt(dx, t1x, t1y)) return false;
 353         gcode->stream->printf("T1-%d Z:%1.3f\n", i, dx);
 354         if(!zprobe->doProbeAt(dy, t2x, t2y)) return false;
 355         gcode->stream->printf("T2-%d Z:%1.3f\n", i, dy);
 356         if(!zprobe->doProbeAt(dz, t3x, t3y)) return false;
 357         gcode->stream->printf("T3-%d Z:%1.3f\n", i, dz);
 358
 359         // now look at the difference and reduce it by adjusting delta radius
 360         float m = (dx + dy + dz) / 3.0F;
 361         float d = cmm - m;
 362         gcode->stream->printf("C-%d Z-ave:%1.4f delta: %1.3f\n", i, m, d);
 363
 364         if(fabsf(d) <= target){
 365             good= true;
 366             break; // resolution of success
 367         }
 368
 369         // increase delta radius to adjust for low center
 370         // decrease delta radius to adjust for high center
 371         delta_radius += (d * drinc);
 372
 373         // set the new delta radius
 374         options['R'] = delta_radius;
 375         THEROBOT->arm_solution->set_optional(options);
 376         gcode->stream->printf("Setting delta radius to: %1.4f\n", delta_radius);
 377
 378         zprobe->home();
 379         zprobe->coordinated_move(NAN, NAN, -bedht, zprobe->getFastFeedrate(), true); // needs to be a relative coordinated move
 380
 381         // flush the output
 382         THEKERNEL->call_event(ON_IDLE);
 383     }
 384
 385     if(!good) {
 386         gcode->stream->printf("WARNING: delta radius did not resolve to within required parameters: %f\n", target);
 387     }
 388
 389     return true;
 390 }
 391
 392 bool DeltaCalibrationStrategy::set_trim(float x, float y, float z, StreamOutput *stream)
 393 {
 394     float t[3] {x, y, z};
 395     bool ok = PublicData::set_value( endstops_checksum, trim_checksum, t);
 396
 397     if (ok) {
 398         stream->printf("set trim to X:%f Y:%f Z:%f\n", x, y, z);
 399     } else {
 400         stream->printf("unable to set trim, is endstops enabled?\n");
 401     }
 402
 403     return ok;
 404 }
 405
 406 bool DeltaCalibrationStrategy::get_trim(float &x, float &y, float &z)
 407 {
 408     void *returned_data;
 409     bool ok = PublicData::get_value( endstops_checksum, trim_checksum, &returned_data );
 410
 411     if (ok) {
 412         float *trim = static_cast<float *>(returned_data);
 413         x = trim[0];
 414         y = trim[1];
 415         z = trim[2];
 416         return true;
 417     }
 418     return false;
 419 }