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