// Probes the 7 points on a delta can be used for off board calibration
bool DeltaCalibrationStrategy::probe_delta_points(Gcode *gcode)
{
float bedht= findBed();
if(isnan(bedht)) return false;
gcode->stream->printf("initial Bed ht is %f mm\n", bedht);
// check probe ht
float mm;
if(!zprobe->doProbeAt(mm, 0, 0)) return false;
float dz = zprobe->getProbeHeight() - mm;
gcode->stream->printf("center probe: %1.4f\n", dz);
// get probe points
float t1x, t1y, t2x, t2y, t3x, t3y;
std::tie(t1x, t1y, t2x, t2y, t3x, t3y) = getCoordinates(this->probe_radius);
// gather probe points
float pp[][2] {{t1x, t1y}, {t2x, t2y}, {t3x, t3y}, {0, 0}, {-t1x, -t1y}, {-t2x, -t2y}, {-t3x, -t3y}};
float max_delta= 0;
float last_z= NAN;
float start_z;
std::tie(std::ignore, std::ignore, start_z)= THEROBOT->get_axis_position();
for(auto& i : pp) {
float mm;
if(!zprobe->doProbeAt(mm, i[0], i[1])) return false;
float z = mm;
if(gcode->subcode == 0) {
gcode->stream->printf("X:%1.4f Y:%1.4f Z:%1.4f A:%1.4f B:%1.4f C:%1.4f\n",
i[0], i[1], z,
THEROBOT->actuators[0]->get_current_position()+z,
THEROBOT->actuators[1]->get_current_position()+z,
THEROBOT->actuators[2]->get_current_position()+z);
}else if(gcode->subcode == 1) {
// format that can be pasted here http://escher3d.com/pages/wizards/wizarddelta.php
gcode->stream->printf("X%1.4f Y%1.4f Z%1.4f\n", i[0], i[1], start_z - z);
}
if(isnan(last_z)) {
last_z= z;
}else{
max_delta= std::max(max_delta, fabsf(z-last_z));
}
}
gcode->stream->printf("max delta: %f\n", max_delta);
return true;
}
bool DeltaCalibrationStrategy::calibrate_delta_radius(Gcode *gcode)
{
float target = 0.03F;
if(gcode->has_letter('I')) target = gcode->get_value('I'); // override default target
if(gcode->has_letter('J')) this->probe_radius = gcode->get_value('J'); // override default probe radius
gcode->stream->printf("Calibrating delta radius: target %f, radius %f\n", target, this->probe_radius);
// get probe points
float t1x, t1y, t2x, t2y, t3x, t3y;
std::tie(t1x, t1y, t2x, t2y, t3x, t3y) = getCoordinates(this->probe_radius);
// find the bed, as we potentially have a temporary z probe we don't know how low under the nozzle it is
// so we need to find thr initial place that the probe triggers when it hits the bed
float bedht= findBed();
if(isnan(bedht)) return false;
gcode->stream->printf("initial Bed ht is %f mm\n", bedht);
zprobe->home();
zprobe->coordinated_move(NAN, NAN, -bedht, zprobe->getFastFeedrate(), true); // do a relative move from home to the point above the bed
// probe center to get reference point at this Z height
int dc;
if(!zprobe->doProbeAt(dc, 0, 0)) return false;
gcode->stream->printf("CT Z:%1.3f C:%d\n", zprobe->zsteps_to_mm(dc), dc);
float cmm = zprobe->zsteps_to_mm(dc);
// get current delta radius
float delta_radius = 0.0F;
BaseSolution::arm_options_t options;
if(THEKERNEL->robot->arm_solution->get_optional(options)) {
delta_radius = options['R'];
}
if(delta_radius == 0.0F) {
gcode->stream->printf("This appears to not be a delta arm solution\n");
return false;
}
options.clear();
float drinc = 2.5F; // approx
for (int i = 1; i <= 10; ++i) {
// probe t1, t2, t3 and get average, but use coordinated moves, probing center won't change
int dx, dy, dz;
if(!zprobe->doProbeAt(dx, t1x, t1y)) return false;
gcode->stream->printf("T1-%d Z:%1.3f C:%d\n", i, zprobe->zsteps_to_mm(dx), dx);
if(!zprobe->doProbeAt(dy, t2x, t2y)) return false;
gcode->stream->printf("T2-%d Z:%1.3f C:%d\n", i, zprobe->zsteps_to_mm(dy), dy);
if(!zprobe->doProbeAt(dz, t3x, t3y)) return false;
gcode->stream->printf("T3-%d Z:%1.3f C:%d\n", i, zprobe->zsteps_to_mm(dz), dz);
// now look at the difference and reduce it by adjusting delta radius
float m = zprobe->zsteps_to_mm((dx + dy + dz) / 3.0F);
float d = cmm - m;
gcode->stream->printf("C-%d Z-ave:%1.4f delta: %1.3f\n", i, m, d);
if(abs(d) <= target) break; // resolution of success
// increase delta radius to adjust for low center
// decrease delta radius to adjust for high center
delta_radius += (d * drinc);
// set the new delta radius
options['R'] = delta_radius;
THEKERNEL->robot->arm_solution->set_optional(options);
gcode->stream->printf("Setting delta radius to: %1.4f\n", delta_radius);
zprobe->home();
zprobe->coordinated_move(NAN, NAN, -bedht, zprobe->getFastFeedrate(), true); // needs to be a relative coordinated move
// flush the output
THEKERNEL->call_event(ON_IDLE);
}
return true;
}
bool DeltaCalibrationStrategy::calibrate_delta_endstops(Gcode *gcode)
{
float target = 0.03F;
if(gcode->has_letter('I')) target = gcode->get_value('I'); // override default target
if(gcode->has_letter('J')) this->probe_radius = gcode->get_value('J'); // override default probe radius
bool keep = false;
if(gcode->has_letter('K')) keep = true; // keep current settings
gcode->stream->printf("Calibrating Endstops: target %fmm, radius %fmm\n", target, this->probe_radius);
// get probe points
float t1x, t1y, t2x, t2y, t3x, t3y;
std::tie(t1x, t1y, t2x, t2y, t3x, t3y) = getCoordinates(this->probe_radius);
float trimx = 0.0F, trimy = 0.0F, trimz = 0.0F;
if(!keep) {
// zero trim values
if(!set_trim(0, 0, 0, gcode->stream)) return false;
} else {
// get current trim, and continue from that
if (get_trim(trimx, trimy, trimz)) {
gcode->stream->printf("Current Trim X: %f, Y: %f, Z: %f\r\n", trimx, trimy, trimz);
} else {
gcode->stream->printf("Could not get current trim, are endstops enabled?\n");
return false;
}
}
// find the bed, as we potentially have a temporary z probe we don't know how low under the nozzle it is
// so we need to find the initial place that the probe triggers when it hits the bed
float bedht= findBed();
if(isnan(bedht)) return false;
gcode->stream->printf("initial Bed ht is %f mm\n", bedht);
// move to start position
zprobe->home();
zprobe->coordinated_move(NAN, NAN, -bedht, zprobe->getFastFeedrate(), true); // do a relative move from home to the point above the bed
// get initial probes
// probe the base of the X tower
int s;
if(!zprobe->doProbeAt(s, t1x, t1y)) return false;
float t1z = zprobe->zsteps_to_mm(s);
gcode->stream->printf("T1-0 Z:%1.4f C:%d\n", t1z, s);
// probe the base of the Y tower
if(!zprobe->doProbeAt(s, t2x, t2y)) return false;
float t2z = zprobe->zsteps_to_mm(s);
gcode->stream->printf("T2-0 Z:%1.4f C:%d\n", t2z, s);
// probe the base of the Z tower
if(!zprobe->doProbeAt(s, t3x, t3y)) return false;
float t3z = zprobe->zsteps_to_mm(s);
gcode->stream->printf("T3-0 Z:%1.4f C:%d\n", t3z, s);
float trimscale = 1.2522F; // empirically determined
auto mm = std::minmax({t1z, t2z, t3z});
if((mm.second - mm.first) <= target) {
gcode->stream->printf("trim already set within required parameters: delta %f\n", mm.second - mm.first);
return true;
}
// set trims to worst case so we always have a negative trim
trimx += (mm.first - t1z) * trimscale;
trimy += (mm.first - t2z) * trimscale;
trimz += (mm.first - t3z) * trimscale;
for (int i = 1; i <= 10; ++i) {
// set trim
if(!set_trim(trimx, trimy, trimz, gcode->stream)) return false;
// home and move probe to start position just above the bed
zprobe->home();
zprobe->coordinated_move(NAN, NAN, -bedht, zprobe->getFastFeedrate(), true); // do a relative move from home to the point above the bed
// probe the base of the X tower
if(!zprobe->doProbeAt(s, t1x, t1y)) return false;
t1z = zprobe->zsteps_to_mm(s);
gcode->stream->printf("T1-%d Z:%1.4f C:%d\n", i, t1z, s);
// probe the base of the Y tower
if(!zprobe->doProbeAt(s, t2x, t2y)) return false;
t2z = zprobe->zsteps_to_mm(s);
gcode->stream->printf("T2-%d Z:%1.4f C:%d\n", i, t2z, s);
// probe the base of the Z tower
if(!zprobe->doProbeAt(s, t3x, t3y)) return false;
t3z = zprobe->zsteps_to_mm(s);
gcode->stream->printf("T3-%d Z:%1.4f C:%d\n", i, t3z, s);
mm = std::minmax({t1z, t2z, t3z});
if((mm.second - mm.first) <= target) {
gcode->stream->printf("trim set to within required parameters: delta %f\n", mm.second - mm.first);
break;
}
// set new trim values based on min difference
trimx += (mm.first - t1z) * trimscale;
trimy += (mm.first - t2z) * trimscale;
trimz += (mm.first - t3z) * trimscale;
// flush the output
THEKERNEL->call_event(ON_IDLE);
}
if((mm.second - mm.first) > target) {
gcode->stream->printf("WARNING: trim did not resolve to within required parameters: delta %f\n", mm.second - mm.first);
}
return true;
}
