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