Skip to content

M
MarlinKimbra
Commit fd791097 authored by MagoKimbra's avatar MagoKimbra
Browse files
Options

Merge remote-tracking branch 'refs/remotes/origin/master' into dev

parents bde64a97 088e7558
Pipeline #83 skipped
Hide whitespace changes
Inline Side-by-side
Showing with 648 additions and 644 deletions
MK/module/MK_Main.cpp
View file @ fd791097
...@@ -1870,772 +1870,776 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio ...@@ -1870,772 +1870,776 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio
delta_tower3_y = (delta_radius + tower_adj[5]) * sin((90 + tower_adj[2]) * M_PI/180); delta_tower3_y = (delta_radius + tower_adj[5]) * sin((90 + tower_adj[2]) * M_PI/180);
} }
bool Equal_AB(const float A, const float B, const float prec = ac_prec) { #if ENABLED(Z_PROBE_ENDSTOP)
if (abs(A - B) <= prec) return true;
return false;
}
static void extrapolate_one_point(int x, int y, int xdir, int ydir) { bool Equal_AB(const float A, const float B, const float prec = ac_prec) {
if (bed_level[x][y] != 0.0) { if (abs(A - B) <= prec) return true;
return; // Don't overwrite good values. return false;
}
float a = 2 * bed_level[x + xdir][y] - bed_level[x + xdir * 2][y]; // Left to right.
float b = 2 * bed_level[x][y + ydir] - bed_level[x][y + ydir * 2]; // Front to back.
float c = 2 * bed_level[x + xdir][y + ydir] - bed_level[x + xdir * 2][y + ydir * 2]; // Diagonal.
float median = c; // Median is robust (ignores outliers).
if (a < b) {
if (b < c) median = b;
if (c < a) median = a;
} else { // b <= a
if (c < b) median = b;
if (a < c) median = a;
} }
bed_level[x][y] = median;
}
// Fill in the unprobed points (corners of circular print surface) static void extrapolate_one_point(int x, int y, int xdir, int ydir) {
// using linear extrapolation, away from the center. if (bed_level[x][y] != 0.0) {
static void extrapolate_unprobed_bed_level() { return; // Don't overwrite good values.
int half = (AUTO_BED_LEVELING_GRID_POINTS - 1) / 2; }
for (int y = 0; y <= half; y++) { float a = 2 * bed_level[x + xdir][y] - bed_level[x + xdir * 2][y]; // Left to right.
for (int x = 0; x <= half; x++) { float b = 2 * bed_level[x][y + ydir] - bed_level[x][y + ydir * 2]; // Front to back.
if (x + y < 3) continue; float c = 2 * bed_level[x + xdir][y + ydir] - bed_level[x + xdir * 2][y + ydir * 2]; // Diagonal.
extrapolate_one_point(half - x, half - y, x > 1 ? +1:0, y > 1 ? +1:0); float median = c; // Median is robust (ignores outliers).
extrapolate_one_point(half + x, half - y, x > 1 ? -1:0, y > 1 ? +1:0); if (a < b) {
extrapolate_one_point(half - x, half + y, x > 1 ? +1:0, y > 1 ? -1:0); if (b < c) median = b;
extrapolate_one_point(half + x, half + y, x > 1 ? -1:0, y > 1 ? -1:0); if (c < a) median = a;
} else { // b <= a
if (c < b) median = b;
if (a < c) median = a;
}
bed_level[x][y] = median;
}
// Fill in the unprobed points (corners of circular print surface)
// using linear extrapolation, away from the center.
static void extrapolate_unprobed_bed_level() {
int half = (AUTO_BED_LEVELING_GRID_POINTS - 1) / 2;
for (int y = 0; y <= half; y++) {
for (int x = 0; x <= half; x++) {
if (x + y < 3) continue;
extrapolate_one_point(half - x, half - y, x > 1 ? +1:0, y > 1 ? +1:0);
extrapolate_one_point(half + x, half - y, x > 1 ? -1:0, y > 1 ? +1:0);
extrapolate_one_point(half - x, half + y, x > 1 ? +1:0, y > 1 ? -1:0);
extrapolate_one_point(half + x, half + y, x > 1 ? -1:0, y > 1 ? -1:0);
}
} }
} }
}
// Print calibration results for plotting or manual frame adjustment. // Print calibration results for plotting or manual frame adjustment.
static void print_bed_level() { static void print_bed_level() {
for (int y = 0; y < AUTO_BED_LEVELING_GRID_POINTS; y++) { for (int y = 0; y < AUTO_BED_LEVELING_GRID_POINTS; y++) {
ECHO_S(DB); ECHO_S(DB);
for (int x = 0; x < AUTO_BED_LEVELING_GRID_POINTS; x++) { for (int x = 0; x < AUTO_BED_LEVELING_GRID_POINTS; x++) {
if(bed_level[x][y] >= 0) ECHO_M(" "); if(bed_level[x][y] >= 0) ECHO_M(" ");
ECHO_VM(bed_level[x][y], " ", 3); ECHO_VM(bed_level[x][y], " ", 3);
}
ECHO_E;
} }
ECHO_E;
} }
}
// Reset calibration results to zero. static void deploy_z_probe() {
static void reset_bed_level() {
if (DEBUGGING(INFO)) ECHO_LM(INFO, "reset_bed_level");
for (int y = 0; y < AUTO_BED_LEVELING_GRID_POINTS; y++) {
for (int x = 0; x < AUTO_BED_LEVELING_GRID_POINTS; x++) {
bed_level[x][y] = 0.0;
}
}
}
static void deploy_z_probe() { if (DEBUGGING(INFO)) DEBUG_POS("deploy_z_probe", current_position);
if (DEBUGGING(INFO)) DEBUG_POS("deploy_z_probe", current_position); if (endstops.z_probe_enabled) return;
if (endstops.z_probe_enabled) return; #if HAS(SERVO_ENDSTOPS)
feedrate = homing_feedrate[Z_AXIS];
do_blocking_move_to_z( z_probe_deploy_start_location[Z_AXIS]);
do_blocking_move_to_xy( z_probe_deploy_start_location[X_AXIS],
z_probe_deploy_start_location[Y_AXIS]);
#if HAS(SERVO_ENDSTOPS) // Engage Z Servo endstop if enabled
feedrate = homing_feedrate[Z_AXIS]; if (servo_endstop_id[Z_AXIS] >= 0)
do_blocking_move_to_z( z_probe_deploy_start_location[Z_AXIS]); servo[servo_endstop_id[Z_AXIS]].move(servo_endstop_angle[Z_AXIS][0]);
do_blocking_move_to_xy( z_probe_deploy_start_location[X_AXIS], #else
z_probe_deploy_start_location[Y_AXIS]); feedrate = homing_feedrate[Z_AXIS];
do_blocking_move_to_z( z_probe_deploy_start_location[Z_AXIS]);
do_blocking_move_to_xy( z_probe_deploy_start_location[X_AXIS],
z_probe_deploy_start_location[Y_AXIS]);
// Engage Z Servo endstop if enabled feedrate = homing_feedrate[Z_AXIS]/10;
if (servo_endstop_id[Z_AXIS] >= 0) do_blocking_move_to(z_probe_deploy_end_location[X_AXIS],
servo[servo_endstop_id[Z_AXIS]].move(servo_endstop_angle[Z_AXIS][0]); z_probe_deploy_end_location[Y_AXIS],
#else z_probe_deploy_end_location[Z_AXIS]);
feedrate = homing_feedrate[Z_AXIS];
do_blocking_move_to_z( z_probe_deploy_start_location[Z_AXIS]);
do_blocking_move_to_xy( z_probe_deploy_start_location[X_AXIS],
z_probe_deploy_start_location[Y_AXIS]);
feedrate = homing_feedrate[Z_AXIS]/10; feedrate = homing_feedrate[Z_AXIS];
do_blocking_move_to(z_probe_deploy_end_location[X_AXIS],
z_probe_deploy_end_location[Y_AXIS], do_blocking_move_to(z_probe_deploy_start_location[X_AXIS],
z_probe_deploy_end_location[Z_AXIS]); z_probe_deploy_start_location[Y_AXIS],
z_probe_deploy_start_location[Z_AXIS]);
#endif
feedrate = homing_feedrate[Z_AXIS]; endstops.enable_z_probe();
sync_plan_position_delta();
do_blocking_move_to(z_probe_deploy_start_location[X_AXIS], }
z_probe_deploy_start_location[Y_AXIS],
z_probe_deploy_start_location[Z_AXIS]);
#endif
endstops.enable_z_probe(); static void retract_z_probe() {
sync_plan_position_delta();
}
static void retract_z_probe() { if (DEBUGGING(INFO)) DEBUG_POS("retract_z_probe", current_position);
if (DEBUGGING(INFO)) DEBUG_POS("retract_z_probe", current_position); if (!endstops.z_probe_enabled) return;
if (!endstops.z_probe_enabled) return; #if HAS(SERVO_ENDSTOPS)
// Retract Z Servo endstop if enabled
if (servo_endstop_id[Z_AXIS] >= 0)
servo[servo_endstop_id[Z_AXIS]].move(servo_endstop_angle[Z_AXIS][1]);
#if HAS(SERVO_ENDSTOPS) feedrate = homing_feedrate[Z_AXIS];
// Retract Z Servo endstop if enabled do_blocking_move_to(z_probe_retract_start_location[X_AXIS],
if (servo_endstop_id[Z_AXIS] >= 0) z_probe_retract_start_location[Y_AXIS],
servo[servo_endstop_id[Z_AXIS]].move(servo_endstop_angle[Z_AXIS][1]); z_probe_retract_start_location[Z_AXIS]);
#else
feedrate = homing_feedrate[Z_AXIS];
do_blocking_move_to(z_probe_retract_start_location[X_AXIS],
z_probe_retract_start_location[Y_AXIS],
z_probe_retract_start_location[Z_AXIS]);
feedrate = homing_feedrate[Z_AXIS]; // Move the nozzle below the print surface to push the probe up.
do_blocking_move_to(z_probe_retract_start_location[X_AXIS], feedrate = homing_feedrate[Z_AXIS]/10;
z_probe_retract_start_location[Y_AXIS], do_blocking_move_to(z_probe_retract_end_location[X_AXIS],
z_probe_retract_start_location[Z_AXIS]); z_probe_retract_end_location[Y_AXIS],
#else z_probe_retract_end_location[Z_AXIS]);
feedrate = homing_feedrate[Z_AXIS];
do_blocking_move_to(z_probe_retract_start_location[X_AXIS],
z_probe_retract_start_location[Y_AXIS],
z_probe_retract_start_location[Z_AXIS]);
// Move the nozzle below the print surface to push the probe up. feedrate = homing_feedrate[Z_AXIS];
feedrate = homing_feedrate[Z_AXIS]/10; do_blocking_move_to(z_probe_retract_start_location[X_AXIS],
do_blocking_move_to(z_probe_retract_end_location[X_AXIS], z_probe_retract_start_location[Y_AXIS],
z_probe_retract_end_location[Y_AXIS], z_probe_retract_start_location[Z_AXIS]);
z_probe_retract_end_location[Z_AXIS]); #endif
feedrate = homing_feedrate[Z_AXIS]; endstops.enable_z_probe(false);
do_blocking_move_to(z_probe_retract_start_location[X_AXIS], sync_plan_position_delta();
z_probe_retract_start_location[Y_AXIS], }
z_probe_retract_start_location[Z_AXIS]);
#endif
endstops.enable_z_probe(false); static void run_z_probe() {
sync_plan_position_delta(); refresh_cmd_timeout();
}
static void run_z_probe() { endstops.enable();
refresh_cmd_timeout(); float start_z = current_position[Z_AXIS];
long start_steps = st_get_position(Z_AXIS);
endstops.enable(); feedrate = AUTOCAL_PROBERATE * 60;
float start_z = current_position[Z_AXIS]; destination[Z_AXIS] = -20;
long start_steps = st_get_position(Z_AXIS); prepare_move_raw();
st_synchronize();
endstops.hit_on_purpose(); // clear endstop hit flags
feedrate = AUTOCAL_PROBERATE * 60; endstops.enable(false);
destination[Z_AXIS] = -20; long stop_steps = st_get_position(Z_AXIS);
prepare_move_raw(); float mm = start_z - float(start_steps - stop_steps) / axis_steps_per_unit[Z_AXIS];
st_synchronize(); current_position[Z_AXIS] = mm;
endstops.hit_on_purpose(); // clear endstop hit flags sync_plan_position_delta();
}
endstops.enable(false); // Probe bed height at position (x,y), returns the measured z value
long stop_steps = st_get_position(Z_AXIS); static float probe_bed(float x, float y) {
float mm = start_z - float(start_steps - stop_steps) / axis_steps_per_unit[Z_AXIS];
current_position[Z_AXIS] = mm;
sync_plan_position_delta();
}
// Probe bed height at position (x,y), returns the measured z value // Move Z up to the bed_safe_z
static float probe_bed(float x, float y) { do_blocking_move_to_z(bed_safe_z);
// Move Z up to the bed_safe_z float Dx = x - z_probe_offset[X_AXIS];
do_blocking_move_to_z(bed_safe_z); NOLESS(Dx, X_MIN_POS);
NOMORE(Dx, X_MAX_POS);
float Dy = y - z_probe_offset[Y_AXIS];
NOLESS(Dy, Y_MIN_POS);
NOMORE(Dy, Y_MAX_POS);
float Dx = x - z_probe_offset[X_AXIS]; if (DEBUGGING(INFO)) {
NOLESS(Dx, X_MIN_POS); ECHO_LM(INFO, "probe_bed >>>");
NOMORE(Dx, X_MAX_POS); DEBUG_POS("", current_position);
float Dy = y - z_probe_offset[Y_AXIS]; ECHO_SMV(INFO, " > do_blocking_move_to_xy ", Dx);
NOLESS(Dy, Y_MIN_POS); ECHO_EMV(", ", Dy);
NOMORE(Dy, Y_MAX_POS); }
if (DEBUGGING(INFO)) { // this also updates current_position
ECHO_LM(INFO, "probe_bed >>>"); do_blocking_move_to_xy(Dx, Dy);
DEBUG_POS("", current_position);
ECHO_SMV(INFO, " > do_blocking_move_to_xy ", Dx);
ECHO_EMV(", ", Dy);
}
// this also updates current_position run_z_probe();
do_blocking_move_to_xy(Dx, Dy); float probe_z = current_position[Z_AXIS] + z_probe_offset[Z_AXIS];
run_z_probe(); if (DEBUGGING(INFO)) {
float probe_z = current_position[Z_AXIS] + z_probe_offset[Z_AXIS]; ECHO_SM(INFO, "Bed probe heights: ");
if (probe_z >= 0) ECHO_M(" ");
ECHO_EV(probe_z, 4);
}
if (DEBUGGING(INFO)) { bed_safe_z = current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS;
ECHO_SM(INFO, "Bed probe heights: "); return probe_z;
if (probe_z >= 0) ECHO_M(" ");
ECHO_EV(probe_z, 4);
} }
bed_safe_z = current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS; static void bed_probe_all() {
return probe_z; // Initial throwaway probe.. used to stabilize probe
} bed_level_c = probe_bed(0.0, 0.0);
static void bed_probe_all() {
// Initial throwaway probe.. used to stabilize probe
bed_level_c = probe_bed(0.0, 0.0);
// Probe all bed positions & store carriage positions // Probe all bed positions & store carriage positions
bed_level_z = probe_bed(0.0, bed_radius); bed_level_z = probe_bed(0.0, bed_radius);
bed_level_oy = probe_bed(-SIN_60 * bed_radius, COS_60 * bed_radius); bed_level_oy = probe_bed(-SIN_60 * bed_radius, COS_60 * bed_radius);
bed_level_x = probe_bed(-SIN_60 * bed_radius, -COS_60 * bed_radius); bed_level_x = probe_bed(-SIN_60 * bed_radius, -COS_60 * bed_radius);
bed_level_oz = probe_bed(0.0, -bed_radius); bed_level_oz = probe_bed(0.0, -bed_radius);
bed_level_y = probe_bed(SIN_60 * bed_radius, -COS_60 * bed_radius); bed_level_y = probe_bed(SIN_60 * bed_radius, -COS_60 * bed_radius);
bed_level_ox = probe_bed(SIN_60 * bed_radius, COS_60 * bed_radius); bed_level_ox = probe_bed(SIN_60 * bed_radius, COS_60 * bed_radius);
bed_level_c = probe_bed(0.0, 0.0); bed_level_c = probe_bed(0.0, 0.0);
} }
static void apply_endstop_adjustment(float x_endstop, float y_endstop, float z_endstop) { static void apply_endstop_adjustment(float x_endstop, float y_endstop, float z_endstop) {
memcpy(saved_endstop_adj, endstop_adj, sizeof(saved_endstop_adj)); memcpy(saved_endstop_adj, endstop_adj, sizeof(saved_endstop_adj));
endstop_adj[X_AXIS] += x_endstop; endstop_adj[X_AXIS] += x_endstop;
endstop_adj[Y_AXIS] += y_endstop; endstop_adj[Y_AXIS] += y_endstop;
endstop_adj[Z_AXIS] += z_endstop; endstop_adj[Z_AXIS] += z_endstop;
calculate_delta(current_position); calculate_delta(current_position);
plan_set_position(delta[TOWER_1] - (endstop_adj[X_AXIS] - saved_endstop_adj[X_AXIS]) , delta[TOWER_2] - (endstop_adj[Y_AXIS] - saved_endstop_adj[Y_AXIS]), delta[TOWER_3] - (endstop_adj[Z_AXIS] - saved_endstop_adj[Z_AXIS]), current_position[E_AXIS]); plan_set_position(delta[TOWER_1] - (endstop_adj[X_AXIS] - saved_endstop_adj[X_AXIS]) , delta[TOWER_2] - (endstop_adj[Y_AXIS] - saved_endstop_adj[Y_AXIS]), delta[TOWER_3] - (endstop_adj[Z_AXIS] - saved_endstop_adj[Z_AXIS]), current_position[E_AXIS]);
st_synchronize(); st_synchronize();
} }
static void adj_endstops() { static void adj_endstops() {
boolean x_done = false; boolean x_done = false;
boolean y_done = false; boolean y_done = false;
boolean z_done = false; boolean z_done = false;
float prv_bed_level_x, prv_bed_level_y, prv_bed_level_z; float prv_bed_level_x, prv_bed_level_y, prv_bed_level_z;
do { do {
bed_level_z = probe_bed(0.0, bed_radius); bed_level_z = probe_bed(0.0, bed_radius);
bed_level_x = probe_bed(-SIN_60 * bed_radius, -COS_60 * bed_radius); bed_level_x = probe_bed(-SIN_60 * bed_radius, -COS_60 * bed_radius);
bed_level_y = probe_bed(SIN_60 * bed_radius, -COS_60 * bed_radius); bed_level_y = probe_bed(SIN_60 * bed_radius, -COS_60 * bed_radius);
apply_endstop_adjustment(bed_level_x, bed_level_y, bed_level_z);
ECHO_SMV(DB, "x:", bed_level_x, 4);
ECHO_MV(" (adj:", endstop_adj[0], 4);
ECHO_MV(") y:", bed_level_y, 4);
ECHO_MV(" (adj:", endstop_adj[1], 4);
ECHO_MV(") z:", bed_level_z, 4);
ECHO_MV(" (adj:", endstop_adj[2], 4);
ECHO_EM(")");
if ((bed_level_x >= -ac_prec) and (bed_level_x <= ac_prec)) {
x_done = true;
ECHO_SM(DB, "X=OK ");
}
else {
x_done = false;
ECHO_SM(DB, "X=ERROR ");
}
apply_endstop_adjustment(bed_level_x, bed_level_y, bed_level_z); if ((bed_level_y >= -ac_prec) and (bed_level_y <= ac_prec)) {
y_done = true;
ECHO_M("Y=OK ");
}
else {
y_done = false;
ECHO_M("Y=ERROR ");
}
ECHO_SMV(DB, "x:", bed_level_x, 4); if ((bed_level_z >= -ac_prec) and (bed_level_z <= ac_prec)) {
ECHO_MV(" (adj:", endstop_adj[0], 4); z_done = true;
ECHO_MV(") y:", bed_level_y, 4); ECHO_EM("Z=OK");
ECHO_MV(" (adj:", endstop_adj[1], 4); }
ECHO_MV(") z:", bed_level_z, 4); else {
ECHO_MV(" (adj:", endstop_adj[2], 4); z_done = false;
ECHO_EM(")"); ECHO_EM("Z=ERROR");
}
} while (((x_done == false) or (y_done == false) or (z_done == false)));
if ((bed_level_x >= -ac_prec) and (bed_level_x <= ac_prec)) { float high_endstop = max(max(endstop_adj[0], endstop_adj[1]), endstop_adj[2]);
x_done = true;
ECHO_SM(DB, "X=OK ");
}
else {
x_done = false;
ECHO_SM(DB, "X=ERROR ");
}
if ((bed_level_y >= -ac_prec) and (bed_level_y <= ac_prec)) { if (DEBUGGING(INFO)) {
y_done = true; ECHO_LMV(INFO, "High endstop: ", high_endstop, 4);
ECHO_M("Y=OK ");
}
else {
y_done = false;
ECHO_M("Y=ERROR ");
} }
if ((bed_level_z >= -ac_prec) and (bed_level_z <= ac_prec)) { if (high_endstop > 0) {
z_done = true; ECHO_LMV(DB, "Reducing Build height by ", high_endstop);
ECHO_EM("Z=OK"); for(uint8_t i = 0; i < 3; i++) {
endstop_adj[i] -= high_endstop;
}
sw_endstop_max[Z_AXIS] -= high_endstop;
} }
else { else if (high_endstop < 0) {
z_done = false; ECHO_LMV(DB, "Increment Build height by ", abs(high_endstop));
ECHO_EM("Z=ERROR"); for(uint8_t i = 0; i < 3; i++) {
endstop_adj[i] -= high_endstop;
}
sw_endstop_max[Z_AXIS] -= high_endstop;
} }
} while (((x_done == false) or (y_done == false) or (z_done == false)));
float high_endstop = max(max(endstop_adj[0], endstop_adj[1]), endstop_adj[2]); set_delta_constants();
if (DEBUGGING(INFO)) {
ECHO_LMV(INFO, "High endstop: ", high_endstop, 4);
} }
if (high_endstop > 0) { int fix_tower_errors() {
ECHO_LMV(DB, "Reducing Build height by ", high_endstop); boolean t1_err, t2_err, t3_err;
for(uint8_t i = 0; i < 3; i++) { boolean xy_equal, xz_equal, yz_equal;
endstop_adj[i] -= high_endstop; float saved_tower_adj[6];
} int err_tower;
sw_endstop_max[Z_AXIS] -= high_endstop; float low_diff, high_diff;
} float x_diff, y_diff, z_diff;
else if (high_endstop < 0) { float xy_diff, yz_diff, xz_diff;
ECHO_LMV(DB, "Increment Build height by ", abs(high_endstop)); float low_opp, high_opp;
for(uint8_t i = 0; i < 3; i++) {
endstop_adj[i] -= high_endstop; for (uint8_t i = 0; i < 6; i++) saved_tower_adj[i] = tower_adj[i];
}
sw_endstop_max[Z_AXIS] -= high_endstop; err_tower = 0;
}
x_diff = abs(bed_level_x - bed_level_ox);
set_delta_constants(); high_diff = x_diff;
} y_diff = abs(bed_level_y - bed_level_oy);
if (y_diff > high_diff) high_diff = y_diff;
int fix_tower_errors() { z_diff = abs(bed_level_z - bed_level_oz);
boolean t1_err, t2_err, t3_err; if (z_diff > high_diff) high_diff = z_diff;
boolean xy_equal, xz_equal, yz_equal;
float saved_tower_adj[6]; if (x_diff <= ac_prec) t1_err = false; else t1_err = true;
int err_tower; if (y_diff <= ac_prec) t2_err = false; else t2_err = true;
float low_diff, high_diff; if (z_diff <= ac_prec) t3_err = false; else t3_err = true;
float x_diff, y_diff, z_diff;
float xy_diff, yz_diff, xz_diff; ECHO_LMV(DB, "x_diff = ", x_diff, 5);
float low_opp, high_opp; ECHO_LMV(DB, "y_diff = ", y_diff, 5);
ECHO_LMV(DB, "z_diff = ", z_diff, 5);
for (uint8_t i = 0; i < 6; i++) saved_tower_adj[i] = tower_adj[i]; ECHO_LMV(DB, "high_diff = ", high_diff, 5);
err_tower = 0; // Are all errors equal? (within defined precision)
xy_equal = false;
x_diff = abs(bed_level_x - bed_level_ox); xz_equal = false;
high_diff = x_diff; yz_equal = false;
y_diff = abs(bed_level_y - bed_level_oy); if (Equal_AB(x_diff, y_diff)) xy_equal = true;
if (y_diff > high_diff) high_diff = y_diff; if (Equal_AB(x_diff, z_diff)) xz_equal = true;
z_diff = abs(bed_level_z - bed_level_oz); if (Equal_AB(y_diff, z_diff)) yz_equal = true;
if (z_diff > high_diff) high_diff = z_diff;
ECHO_SM(DB, "xy_equal = ");
if (x_diff <= ac_prec) t1_err = false; else t1_err = true; if (xy_equal == true) ECHO_EM("true"); else ECHO_EM("false");
if (y_diff <= ac_prec) t2_err = false; else t2_err = true; ECHO_SM(DB, "xz_equal = ");
if (z_diff <= ac_prec) t3_err = false; else t3_err = true; if (xz_equal == true) ECHO_EM("true"); else ECHO_EM("false");
ECHO_SM(DB, "yz_equal = ");
ECHO_LMV(DB, "x_diff = ", x_diff, 5); if (yz_equal == true) ECHO_EM("true"); else ECHO_EM("false");
ECHO_LMV(DB, "y_diff = ", y_diff, 5);
ECHO_LMV(DB, "z_diff = ", z_diff, 5); low_opp = bed_level_ox;
ECHO_LMV(DB, "high_diff = ", high_diff, 5); high_opp = low_opp;
if (bed_level_oy < low_opp) low_opp = bed_level_oy;
// Are all errors equal? (within defined precision) if (bed_level_oy > high_opp) high_opp = bed_level_oy;
xy_equal = false; if (bed_level_oz < low_opp) low_opp = bed_level_oz;
xz_equal = false; if (bed_level_oz > high_opp) high_opp = bed_level_oz;
yz_equal = false;
if (Equal_AB(x_diff, y_diff)) xy_equal = true; ECHO_LMV(DB, "Opp Range = ", high_opp - low_opp, 5);
if (Equal_AB(x_diff, z_diff)) xz_equal = true;
if (Equal_AB(y_diff, z_diff)) yz_equal = true; if (Equal_AB(high_opp, low_opp)) {
ECHO_LM(DB, "Opposite Points within Limits - Adjustment not required");
ECHO_SM(DB, "xy_equal = ");
if (xy_equal == true) ECHO_EM("true"); else ECHO_EM("false");
ECHO_SM(DB, "xz_equal = ");
if (xz_equal == true) ECHO_EM("true"); else ECHO_EM("false");
ECHO_SM(DB, "yz_equal = ");
if (yz_equal == true) ECHO_EM("true"); else ECHO_EM("false");
low_opp = bed_level_ox;
high_opp = low_opp;
if (bed_level_oy < low_opp) low_opp = bed_level_oy;
if (bed_level_oy > high_opp) high_opp = bed_level_oy;
if (bed_level_oz < low_opp) low_opp = bed_level_oz;
if (bed_level_oz > high_opp) high_opp = bed_level_oz;
ECHO_LMV(DB, "Opp Range = ", high_opp - low_opp, 5);
if (Equal_AB(high_opp, low_opp)) {
ECHO_LM(DB, "Opposite Points within Limits - Adjustment not required");
t1_err = false;
t2_err = false;
t3_err = false;
}
// All Towers have errors
if ((t1_err == true) and (t2_err == true) and (t3_err == true)) {
if ((xy_equal == false) or (xz_equal == false) or (yz_equal == false)) {
// Errors not equal .. select the tower that needs to be adjusted
if (Equal_AB(high_diff, x_diff, 0.00001)) err_tower = 1;
if (Equal_AB(high_diff, y_diff, 0.00001)) err_tower = 2;
if (Equal_AB(high_diff, z_diff, 0.00001)) err_tower = 3;
ECHO_SMV(DB, "Tower ", err_tower);
ECHO_EM(" has largest error");
}
if ((xy_equal == true) and (xz_equal == true) and (yz_equal == true)) {
ECHO_LM(DB, "All Towers Errors Equal");
t1_err = false; t1_err = false;
t2_err = false; t2_err = false;
t3_err = false; t3_err = false;
} }
}
/*
// Two tower errors
if ((t1_err == true) and (t2_err == true) and (t3_err == false)) err_tower = 3;
if ((t1_err == true) and (t2_err == false) and (t3_err == true)) err_tower = 2;
if ((t1_err == false) and (t2_err == true) and (t3_err == true)) err_tower = 1;
*/
// Single tower error // All Towers have errors
if ((t1_err == true) and (t2_err == false) and (t3_err == false)) err_tower = 1; if ((t1_err == true) and (t2_err == true) and (t3_err == true)) {
if ((t1_err == false) and (t2_err == true) and (t3_err == false)) err_tower = 2; if ((xy_equal == false) or (xz_equal == false) or (yz_equal == false)) {
if ((t1_err == false) and (t2_err == false) and (t3_err == true)) err_tower = 3; // Errors not equal .. select the tower that needs to be adjusted
if (Equal_AB(high_diff, x_diff, 0.00001)) err_tower = 1;
ECHO_SM(DB, "t1:"); if (Equal_AB(high_diff, y_diff, 0.00001)) err_tower = 2;
if (t1_err == true) ECHO_M("Err"); else ECHO_M("OK"); if (Equal_AB(high_diff, z_diff, 0.00001)) err_tower = 3;
ECHO_M(" t2:"); ECHO_SMV(DB, "Tower ", err_tower);
if (t2_err == true) ECHO_M("Err"); else ECHO_M("OK"); ECHO_EM(" has largest error");
ECHO_M(" t3:"); }
if (t3_err == true) ECHO_M("Err"); else ECHO_M("OK"); if ((xy_equal == true) and (xz_equal == true) and (yz_equal == true)) {
ECHO_E; ECHO_LM(DB, "All Towers Errors Equal");
t1_err = false;
if (err_tower == 0) { t2_err = false;
ECHO_LM(DB, "Tower geometry OK"); t3_err = false;
} }
else { }
ECHO_SMV(DB, "Tower", int(err_tower));
ECHO_EM(" Error: Adjusting");
adj_tower_radius(err_tower);
}
//Set return value to indicate if anything has been changed (0 = no change)
int retval = 0;
for (uint8_t i = 0; i < 6; i++) if (saved_tower_adj[i] != tower_adj[i]) retval++;
return retval;
}
bool adj_deltaradius() { /*
float adj_r; // Two tower errors
uint8_t c_nochange_count = 0; if ((t1_err == true) and (t2_err == true) and (t3_err == false)) err_tower = 3;
float nochange_r; if ((t1_err == true) and (t2_err == false) and (t3_err == true)) err_tower = 2;
if ((t1_err == false) and (t2_err == true) and (t3_err == true)) err_tower = 1;
*/
// Single tower error
if ((t1_err == true) and (t2_err == false) and (t3_err == false)) err_tower = 1;
if ((t1_err == false) and (t2_err == true) and (t3_err == false)) err_tower = 2;
if ((t1_err == false) and (t2_err == false) and (t3_err == true)) err_tower = 3;
ECHO_SM(DB, "t1:");
if (t1_err == true) ECHO_M("Err"); else ECHO_M("OK");
ECHO_M(" t2:");
if (t2_err == true) ECHO_M("Err"); else ECHO_M("OK");
ECHO_M(" t3:");
if (t3_err == true) ECHO_M("Err"); else ECHO_M("OK");
ECHO_E;
bed_level_c = probe_bed(0.0, 0.0); if (err_tower == 0) {
ECHO_LM(DB, "Tower geometry OK");
}
else {
ECHO_SMV(DB, "Tower", int(err_tower));
ECHO_EM(" Error: Adjusting");
adj_tower_radius(err_tower);
}
if ((bed_level_c >= -ac_prec) and (bed_level_c <= ac_prec)) { //Set return value to indicate if anything has been changed (0 = no change)
ECHO_LM(DB, "Delta Radius OK"); int retval = 0;
return false; for (uint8_t i = 0; i < 6; i++) if (saved_tower_adj[i] != tower_adj[i]) retval++;
return retval;
} }
else {
ECHO_LM(DB, "Adjusting Delta Radius");
ECHO_LMV(DB, "Bed level center = ", bed_level_c);
// set initial direction and magnitude for delta radius adjustment bool adj_deltaradius() {
adj_r = 0.2; float adj_r;
if (bed_level_c > 0) adj_r = -0.2; uint8_t c_nochange_count = 0;
float nochange_r;
do { bed_level_c = probe_bed(0.0, 0.0);
delta_radius += adj_r;
set_delta_constants();
bed_level_c = probe_bed(0.0, 0.0); if ((bed_level_c >= -ac_prec) and (bed_level_c <= ac_prec)) {
ECHO_LM(DB, "Delta Radius OK");
return false;
}
else {
ECHO_LM(DB, "Adjusting Delta Radius");
ECHO_LMV(DB, "Bed level center = ", bed_level_c);
//Show progress // set initial direction and magnitude for delta radius adjustment
ECHO_SMV(DB, "r:", delta_radius, 4); adj_r = 0.2;
ECHO_MV(" (adj:", adj_r, 6); if (bed_level_c > 0) adj_r = -0.2;
ECHO_EMV(") c:", bed_level_c, 4);
//Adjust delta radius do {
if (bed_level_c < 0) adj_r = (abs(adj_r) / 2); delta_radius += adj_r;
if (bed_level_c > 0) adj_r = -(abs(adj_r) / 2); set_delta_constants();
} while(bed_level_c < -ac_prec or bed_level_c > ac_prec); bed_level_c = probe_bed(0.0, 0.0);
return true; //Show progress
} ECHO_SMV(DB, "r:", delta_radius, 4);
} ECHO_MV(" (adj:", adj_r, 6);
ECHO_EMV(") c:", bed_level_c, 4);
static void adj_tower_radius(int tower) { //Adjust delta radius
boolean done,t1_done,t2_done,t3_done; if (bed_level_c < 0) adj_r = (abs(adj_r) / 2);
int nochange_count; if (bed_level_c > 0) adj_r = -(abs(adj_r) / 2);
float target, prev_target, prev_bed_level;
float temp, adj_target;
//Set inital tower adjustment values } while(bed_level_c < -ac_prec or bed_level_c > ac_prec);
adj_t1_Radius = 0;
adj_t2_Radius = 0;
adj_t3_Radius = 0;
nochange_count = 0;
if ((tower == 1) and (adj_t1_Radius == 0)) { return true;
target = (bed_level_oy + bed_level_oz) / 2; }
temp = (bed_level_ox - target) / 2;
adj_target = target + temp;
if (bed_level_ox < adj_target) adj_t1_Radius = -0.4;
if (bed_level_ox > adj_target) adj_t1_Radius = 0.4;
}
if ((tower == 2) and (adj_t2_Radius == 0)) {
target = (bed_level_ox + bed_level_oz) / 2;
temp = (bed_level_oy - target) / 2;
adj_target = target + temp;
if (bed_level_oy < adj_target) adj_t2_Radius = -0.4;
if (bed_level_oy > adj_target) adj_t2_Radius = 0.4;
}
if ((tower == 3) and (adj_t3_Radius == 0)) {
target = (bed_level_oy + bed_level_ox) / 2;
temp = (bed_level_oz - target) / 2;
adj_target = target + temp;
if (bed_level_oz < adj_target) adj_t3_Radius = -0.4; //0.4;
if (bed_level_oz > adj_target) adj_t3_Radius = 0.4; //-0.4;
} }
do { static void adj_tower_radius(int tower) {
tower_adj[3] += adj_t1_Radius; boolean done,t1_done,t2_done,t3_done;
tower_adj[4] += adj_t2_Radius; int nochange_count;
tower_adj[5] += adj_t3_Radius; float target, prev_target, prev_bed_level;
set_delta_constants(); float temp, adj_target;
//done = false;
t1_done = false;
t2_done = false;
t3_done = false;
if (tower == 1) {
t2_done = true;
t3_done = true;
prev_target = adj_target;
prev_bed_level = bed_level_ox;
bed_level_ox = probe_bed(SIN_60 * bed_radius, COS_60 * bed_radius); //Set inital tower adjustment values
bed_level_oy = probe_bed(-SIN_60 * bed_radius, COS_60 * bed_radius); adj_t1_Radius = 0;
bed_level_oz = probe_bed(0.0, -bed_radius); adj_t2_Radius = 0;
adj_t3_Radius = 0;
nochange_count = 0;
if ((tower == 1) and (adj_t1_Radius == 0)) {
target = (bed_level_oy + bed_level_oz) / 2; target = (bed_level_oy + bed_level_oz) / 2;
temp = (bed_level_ox - target) / 2; temp = (bed_level_ox - target) / 2;
adj_target = target + temp; adj_target = target + temp;
if (((bed_level_ox < adj_target) and (adj_t1_Radius > 0)) or ((bed_level_ox > adj_target) and (adj_t1_Radius < 0))) adj_t1_Radius = -(adj_t1_Radius / 2); if (bed_level_ox < adj_target) adj_t1_Radius = -0.4;
if (Equal_AB(bed_level_ox, adj_target, ac_prec / 2)) t1_done = true; if (bed_level_ox > adj_target) adj_t1_Radius = 0.4;
if (Equal_AB(bed_level_ox, prev_bed_level, ac_prec / 2) and Equal_AB(adj_target, prev_target, ac_prec / 2)) nochange_count ++; }
if (nochange_count > 1) { if ((tower == 2) and (adj_t2_Radius == 0)) {
ECHO_LM(DB, "Stuck in Loop.. Exiting"); target = (bed_level_ox + bed_level_oz) / 2;
temp = (bed_level_oy - target) / 2;
adj_target = target + temp;
if (bed_level_oy < adj_target) adj_t2_Radius = -0.4;
if (bed_level_oy > adj_target) adj_t2_Radius = 0.4;
}
if ((tower == 3) and (adj_t3_Radius == 0)) {
target = (bed_level_oy + bed_level_ox) / 2;
temp = (bed_level_oz - target) / 2;
adj_target = target + temp;
if (bed_level_oz < adj_target) adj_t3_Radius = -0.4; //0.4;
if (bed_level_oz > adj_target) adj_t3_Radius = 0.4; //-0.4;
}
do {
tower_adj[3] += adj_t1_Radius;
tower_adj[4] += adj_t2_Radius;
tower_adj[5] += adj_t3_Radius;
set_delta_constants();
//done = false;
t1_done = false;
t2_done = false;
t3_done = false;
if (tower == 1) {
t2_done = true;
t3_done = true;
prev_target = adj_target;
prev_bed_level = bed_level_ox;
bed_level_ox = probe_bed(SIN_60 * bed_radius, COS_60 * bed_radius);
bed_level_oy = probe_bed(-SIN_60 * bed_radius, COS_60 * bed_radius);
bed_level_oz = probe_bed(0.0, -bed_radius);
target = (bed_level_oy + bed_level_oz) / 2;
temp = (bed_level_ox - target) / 2;
adj_target = target + temp;
if (((bed_level_ox < adj_target) and (adj_t1_Radius > 0)) or ((bed_level_ox > adj_target) and (adj_t1_Radius < 0))) adj_t1_Radius = -(adj_t1_Radius / 2);
if (Equal_AB(bed_level_ox, adj_target, ac_prec / 2)) t1_done = true;
if (Equal_AB(bed_level_ox, prev_bed_level, ac_prec / 2) and Equal_AB(adj_target, prev_target, ac_prec / 2)) nochange_count ++;
if (nochange_count > 1) {
ECHO_LM(DB, "Stuck in Loop.. Exiting");
t1_done = true;
}
ECHO_SMV(DB, "target:", adj_target, 6);
ECHO_MV(" ox:", bed_level_ox, 6);
ECHO_MV(" tower radius adj:", tower_adj[3], 6);
if (t1_done == true) ECHO_EM(" done:true"); else ECHO_EM(" done:false");
}
if (tower == 2) {
t1_done = true; t1_done = true;
t3_done = true;
prev_target = adj_target;
prev_bed_level = bed_level_oy;
bed_level_ox = probe_bed(SIN_60 * bed_radius, COS_60 * bed_radius);
bed_level_oy = probe_bed(-SIN_60 * bed_radius, COS_60 * bed_radius);
bed_level_oz = probe_bed(0.0, -bed_radius);
target = (bed_level_ox + bed_level_oz) /2;
temp = (bed_level_oy - target) / 2;
adj_target = target + temp;
if (((bed_level_oy < adj_target) and (adj_t2_Radius > 0)) or ((bed_level_oy > adj_target) and (adj_t2_Radius < 0))) adj_t2_Radius = -(adj_t2_Radius / 2);
if (Equal_AB(bed_level_oy, adj_target, ac_prec / 2)) t2_done = true;
if (Equal_AB(bed_level_oy, prev_bed_level, ac_prec / 2) and Equal_AB(adj_target, prev_target, ac_prec / 2)) nochange_count ++;
if (nochange_count > 1) {
ECHO_LM(DB, "Stuck in Loop.. Exiting");
t2_done = true;
}
ECHO_SMV(DB, "target:", adj_target, 6);
ECHO_MV(" oy:", bed_level_oy, 6);
ECHO_MV(" tower radius adj:", tower_adj[4], 6);
if (t2_done == true) ECHO_EM(" done:true"); else ECHO_EM(" done:false");
} }
ECHO_SMV(DB, "target:", adj_target, 6); if (tower == 3) {
ECHO_MV(" ox:", bed_level_ox, 6); t1_done = true;
ECHO_MV(" tower radius adj:", tower_adj[3], 6); t2_done = true;
if (t1_done == true) ECHO_EM(" done:true"); else ECHO_EM(" done:false"); prev_target = adj_target;
} prev_bed_level = bed_level_oz;
bed_level_ox = probe_bed(SIN_60 * bed_radius, COS_60 * bed_radius);
bed_level_oy = probe_bed(-SIN_60 * bed_radius, COS_60 * bed_radius);
bed_level_oz = probe_bed(0.0, -bed_radius);
target = (bed_level_oy + bed_level_ox) / 2;
temp = (bed_level_oz - target) / 2;
adj_target = target + temp;
if (((bed_level_oz < adj_target) and (adj_t3_Radius > 0)) or ((bed_level_oz > adj_target) and (adj_t3_Radius < 0))) adj_t3_Radius = -(adj_t3_Radius / 2);
if (Equal_AB(bed_level_oz, adj_target, ac_prec / 2)) t3_done = true;
if (Equal_AB(bed_level_oz, prev_bed_level, ac_prec / 2) and Equal_AB(adj_target, prev_target, ac_prec / 2)) nochange_count ++;
if (nochange_count > 1) {
ECHO_LM(DB, "Stuck in Loop.. Exiting");
t3_done = true;
}
ECHO_SMV(DB, "target:", adj_target, 6);
ECHO_MV(" oz:", bed_level_oz, 6);
ECHO_MV(" tower radius adj:", tower_adj[5], 6);
if (t3_done == true) ECHO_EM(" done:true"); else ECHO_EM(" done:false");
}
} while ((t1_done == false) or (t2_done == false) or (t3_done == false));
}
if (tower == 2) { static void adj_tower_delta(int tower) {
t1_done = true; float adj_val = 0;
t3_done = true; float adj_mag = 0.2;
prev_target = adj_target; float adj_prv;
prev_bed_level = bed_level_oy;
bed_level_ox = probe_bed(SIN_60 * bed_radius, COS_60 * bed_radius); do {
bed_level_oy = probe_bed(-SIN_60 * bed_radius, COS_60 * bed_radius); tower_adj[tower - 1] += adj_val;
bed_level_oz = probe_bed(0.0, -bed_radius); set_delta_constants();
target = (bed_level_ox + bed_level_oz) /2; if ((tower == 1) or (tower == 3)) bed_level_oy = probe_bed(-SIN_60 * bed_radius, COS_60 * bed_radius);
temp = (bed_level_oy - target) / 2; if ((tower == 1) or (tower == 2)) bed_level_oz = probe_bed(0.0, -bed_radius);
adj_target = target + temp; if ((tower == 2) or (tower == 3)) bed_level_ox = probe_bed(SIN_60 * bed_radius, COS_60 * bed_radius);
if (((bed_level_oy < adj_target) and (adj_t2_Radius > 0)) or ((bed_level_oy > adj_target) and (adj_t2_Radius < 0))) adj_t2_Radius = -(adj_t2_Radius / 2);
if (Equal_AB(bed_level_oy, adj_target, ac_prec / 2)) t2_done = true; adj_prv = adj_val;
if (Equal_AB(bed_level_oy, prev_bed_level, ac_prec / 2) and Equal_AB(adj_target, prev_target, ac_prec / 2)) nochange_count ++; adj_val = 0;
if (nochange_count > 1) {
ECHO_LM(DB, "Stuck in Loop.. Exiting"); if (tower == 1) {
t2_done = true; if (bed_level_oy < bed_level_oz) adj_val = adj_mag;
if (bed_level_oy > bed_level_oz) adj_val = -adj_mag;
} }
ECHO_SMV(DB, "target:", adj_target, 6); if (tower == 2) {
ECHO_MV(" oy:", bed_level_oy, 6); if (bed_level_oz < bed_level_ox) adj_val = adj_mag;
ECHO_MV(" tower radius adj:", tower_adj[4], 6); if (bed_level_oz > bed_level_ox) adj_val = -adj_mag;
if (t2_done == true) ECHO_EM(" done:true"); else ECHO_EM(" done:false"); }
}
if (tower == 3) { if (tower == 3) {
t1_done = true; if (bed_level_ox < bed_level_oy) adj_val = adj_mag;
t2_done = true; if (bed_level_ox > bed_level_oy) adj_val = -adj_mag;
prev_target = adj_target; }
prev_bed_level = bed_level_oz;
if ((adj_val > 0) and (adj_prv < 0)) {
adj_mag = adj_mag / 2;
adj_val = adj_mag;
}
bed_level_ox = probe_bed(SIN_60 * bed_radius, COS_60 * bed_radius); if ((adj_val < 0) and (adj_prv > 0)) {
bed_level_oy = probe_bed(-SIN_60 * bed_radius, COS_60 * bed_radius); adj_mag = adj_mag / 2;
bed_level_oz = probe_bed(0.0, -bed_radius); adj_val = -adj_mag;
}
target = (bed_level_oy + bed_level_ox) / 2; // Show Adjustments made
temp = (bed_level_oz - target) / 2; if (tower == 1) {
adj_target = target + temp; ECHO_SMV(DB, "oy:", bed_level_oy, 4);
if (((bed_level_oz < adj_target) and (adj_t3_Radius > 0)) or ((bed_level_oz > adj_target) and (adj_t3_Radius < 0))) adj_t3_Radius = -(adj_t3_Radius / 2); ECHO_MV(" oz:", bed_level_oz, 4);
if (Equal_AB(bed_level_oz, adj_target, ac_prec / 2)) t3_done = true;
if (Equal_AB(bed_level_oz, prev_bed_level, ac_prec / 2) and Equal_AB(adj_target, prev_target, ac_prec / 2)) nochange_count ++;
if (nochange_count > 1) {
ECHO_LM(DB, "Stuck in Loop.. Exiting");
t3_done = true;
} }
ECHO_SMV(DB, "target:", adj_target, 6);
ECHO_MV(" oz:", bed_level_oz, 6);
ECHO_MV(" tower radius adj:", tower_adj[5], 6);
if (t3_done == true) ECHO_EM(" done:true"); else ECHO_EM(" done:false");
}
} while ((t1_done == false) or (t2_done == false) or (t3_done == false));
}
static void adj_tower_delta(int tower) { if (tower == 2) {
float adj_val = 0; ECHO_SMV(DB, "ox:", bed_level_ox, 4);
float adj_mag = 0.2; ECHO_MV(" oz:", bed_level_oz, 4);
float adj_prv; }
do { if (tower == 3) {
tower_adj[tower - 1] += adj_val; ECHO_SMV(DB, "ox:", bed_level_ox, 4);
set_delta_constants(); ECHO_MV(" oy:", bed_level_oy, 4);
}
if ((tower == 1) or (tower == 3)) bed_level_oy = probe_bed(-SIN_60 * bed_radius, COS_60 * bed_radius); ECHO_EMV(" tower delta adj:", adj_val, 5);
if ((tower == 1) or (tower == 2)) bed_level_oz = probe_bed(0.0, -bed_radius); } while(adj_val != 0);
if ((tower == 2) or (tower == 3)) bed_level_ox = probe_bed(SIN_60 * bed_radius, COS_60 * bed_radius); }
adj_prv = adj_val; float adj_diagrod_length() {
adj_val = 0; float adj_val = 0;
float adj_mag = 0.2;
float adj_prv, target;
float prev_diag_rod = delta_diagonal_rod;
if (tower == 1) { do {
if (bed_level_oy < bed_level_oz) adj_val = adj_mag; delta_diagonal_rod += adj_val;
if (bed_level_oy > bed_level_oz) adj_val = -adj_mag; set_delta_constants();
}
if (tower == 2) { bed_level_oy = probe_bed(-SIN_60 * bed_radius, COS_60 * bed_radius);
if (bed_level_oz < bed_level_ox) adj_val = adj_mag; bed_level_oz = probe_bed(0.0, -bed_radius);
if (bed_level_oz > bed_level_ox) adj_val = -adj_mag; bed_level_ox = probe_bed(SIN_60 * bed_radius, COS_60 * bed_radius);
} bed_level_c = probe_bed(0.0, 0.0);
if (tower == 3) { target = (bed_level_ox + bed_level_oy + bed_level_oz) / 3;
if (bed_level_ox < bed_level_oy) adj_val = adj_mag; adj_prv = adj_val;
if (bed_level_ox > bed_level_oy) adj_val = -adj_mag; adj_val = 0;
}
if ((adj_val > 0) and (adj_prv < 0)) {
adj_mag = adj_mag / 2;
adj_val = adj_mag;
}
if ((adj_val < 0) and (adj_prv > 0)) { if (bed_level_c - 0.005 < target) adj_val = -adj_mag;
adj_mag = adj_mag / 2; if (bed_level_c + 0.005 > target) adj_val = adj_mag;
adj_val = -adj_mag;
}
// Show Adjustments made if (((adj_val > 0) and (adj_prv < 0)) or ((adj_val < 0) and (adj_prv > 0))) {
if (tower == 1) { adj_val = adj_val / 2;
ECHO_SMV(DB, "oy:", bed_level_oy, 4); adj_mag = adj_mag / 2;
ECHO_MV(" oz:", bed_level_oz, 4); }
}
if (tower == 2) { if ((bed_level_c - 0.005 < target) and (bed_level_c + 0.005 > target)) adj_val = 0;
ECHO_SMV(DB, "ox:", bed_level_ox, 4);
ECHO_MV(" oz:", bed_level_oz, 4);
}
if (tower == 3) { // If adj magnatude is very small.. quit adjusting
ECHO_SMV(DB, "ox:", bed_level_ox, 4); if ((abs(adj_val) < 0.001) and (adj_val != 0)) adj_val = 0;
ECHO_MV(" oy:", bed_level_oy, 4);
}
ECHO_EMV(" tower delta adj:", adj_val, 5); ECHO_SMV(DB, "target:", target, 4);
} while(adj_val != 0); ECHO_MV(" c:", bed_level_c, 4);
} ECHO_EMV(" adj:", adj_val, 5);
} while(adj_val != 0);
return (delta_diagonal_rod - prev_diag_rod);
}
float adj_diagrod_length() { static void calibrate_print_surface() {
float adj_val = 0; float probe_bed_z, probe_z, probe_h, probe_l;
float adj_mag = 0.2; int probe_count, auto_bed_leveling_grid_points = AUTO_BED_LEVELING_GRID_POINTS;
float adj_prv, target;
float prev_diag_rod = delta_diagonal_rod;
do { int left_probe_bed_position = LEFT_PROBE_BED_POSITION,
delta_diagonal_rod += adj_val; right_probe_bed_position = RIGHT_PROBE_BED_POSITION,
set_delta_constants(); front_probe_bed_position = FRONT_PROBE_BED_POSITION,
back_probe_bed_position = BACK_PROBE_BED_POSITION;
bed_level_oy = probe_bed(-SIN_60 * bed_radius, COS_60 * bed_radius); // probe at the points of a lattice grid
bed_level_oz = probe_bed(0.0, -bed_radius); const int xGridSpacing = (right_probe_bed_position - left_probe_bed_position) / (auto_bed_leveling_grid_points - 1),
bed_level_ox = probe_bed(SIN_60 * bed_radius, COS_60 * bed_radius); yGridSpacing = (back_probe_bed_position - front_probe_bed_position) / (auto_bed_leveling_grid_points - 1);
bed_level_c = probe_bed(0.0, 0.0);
target = (bed_level_ox + bed_level_oy + bed_level_oz) / 3; delta_grid_spacing[0] = xGridSpacing;
adj_prv = adj_val; delta_grid_spacing[1] = yGridSpacing;
adj_val = 0;
if (bed_level_c - 0.005 < target) adj_val = -adj_mag; // First point
if (bed_level_c + 0.005 > target) adj_val = adj_mag; bed_level_c = probe_bed(0.0, 0.0);
if (((adj_val > 0) and (adj_prv < 0)) or ((adj_val < 0) and (adj_prv > 0))) { bool zig = true;
adj_val = adj_val / 2;
adj_mag = adj_mag / 2;
}
if ((bed_level_c - 0.005 < target) and (bed_level_c + 0.005 > target)) adj_val = 0; for (int yCount = 0; yCount < auto_bed_leveling_grid_points; yCount++) {
double yProbe = front_probe_bed_position + yGridSpacing * yCount;
int xStart, xStop, xInc;
// If adj magnatude is very small.. quit adjusting if (zig) {
if ((abs(adj_val) < 0.001) and (adj_val != 0)) adj_val = 0; xStart = 0;
xStop = auto_bed_leveling_grid_points;
xInc = 1;
}
else {
xStart = auto_bed_leveling_grid_points - 1;
xStop = -1;
xInc = -1;
}
ECHO_SMV(DB, "target:", target, 4); zig = !zig;
ECHO_MV(" c:", bed_level_c, 4);
ECHO_EMV(" adj:", adj_val, 5);
} while(adj_val != 0);
return (delta_diagonal_rod - prev_diag_rod);
}
static void calibrate_print_surface() { for (int xCount = xStart; xCount != xStop; xCount += xInc) {
float probe_bed_z, probe_z, probe_h, probe_l; double xProbe = left_probe_bed_position + xGridSpacing * xCount;
int probe_count, auto_bed_leveling_grid_points = AUTO_BED_LEVELING_GRID_POINTS;
int left_probe_bed_position = LEFT_PROBE_BED_POSITION, // Avoid probing the corners (outside the round or hexagon print surface) on a delta printer.
right_probe_bed_position = RIGHT_PROBE_BED_POSITION, float distance_from_center = sqrt(xProbe * xProbe + yProbe * yProbe);
front_probe_bed_position = FRONT_PROBE_BED_POSITION, if (distance_from_center > DELTA_PROBABLE_RADIUS) continue;
back_probe_bed_position = BACK_PROBE_BED_POSITION;
// probe at the points of a lattice grid bed_level[xCount][yCount] = probe_bed(xProbe, yProbe);
const int xGridSpacing = (right_probe_bed_position - left_probe_bed_position) / (auto_bed_leveling_grid_points - 1),
yGridSpacing = (back_probe_bed_position - front_probe_bed_position) / (auto_bed_leveling_grid_points - 1);
delta_grid_spacing[0] = xGridSpacing; idle();
delta_grid_spacing[1] = yGridSpacing; } // xProbe
} // yProbe
// First point extrapolate_unprobed_bed_level();
bed_level_c = probe_bed(0.0, 0.0); print_bed_level();
}
bool zig = true; static void calibration_report() {
// Display Report
ECHO_LM(DB, "| \tZ-Tower\t\t\tEndstop Offsets");
ECHO_SM(DB, "| \t");
if (bed_level_z >= 0) ECHO_M(" ");
ECHO_MV("", bed_level_z, 4);
ECHO_MV("\t\t\tX:", endstop_adj[0], 4);
ECHO_MV(" Y:", endstop_adj[1], 4);
ECHO_EMV(" Z:", endstop_adj[2], 4);
ECHO_SM(DB, "| ");
if (bed_level_ox >= 0) ECHO_M(" ");
ECHO_MV("", bed_level_ox, 4);
ECHO_M("\t");
if (bed_level_oy >= 0) ECHO_M(" ");
ECHO_MV("", bed_level_oy, 4);
ECHO_EM("\t\tTower Offsets");
ECHO_SM(DB, "| \t");
if (bed_level_c >= 0) ECHO_M(" ");
ECHO_MV("", bed_level_c, 4);
ECHO_MV("\t\t\tA:",tower_adj[0]);
ECHO_MV(" B:",tower_adj[1]);
ECHO_EMV(" C:",tower_adj[2]);
ECHO_SM(DB, "| ");
if (bed_level_x >= 0) ECHO_M(" ");
ECHO_MV("", bed_level_x, 4);
ECHO_M("\t");
if (bed_level_y >= 0) ECHO_M(" ");
ECHO_MV("", bed_level_y, 4);
ECHO_MV("\t\tI:",tower_adj[3]);
ECHO_MV(" J:",tower_adj[4]);
ECHO_EMV(" K:",tower_adj[5]);
ECHO_SM(DB, "| \t");
if (bed_level_oz >= 0) ECHO_M(" ");
ECHO_MV("", bed_level_oz, 4);
ECHO_EMV("\t\t\tDelta Radius: ", delta_radius, 4);
ECHO_LMV(DB, "| X-Tower\tY-Tower\t\tDiagonal Rod: ", delta_diagonal_rod, 4);
ECHO_E;
}
for (int yCount = 0; yCount < auto_bed_leveling_grid_points; yCount++) { #endif
double yProbe = front_probe_bed_position + yGridSpacing * yCount;
int xStart, xStop, xInc;
if (zig) { // Reset calibration results to zero.
xStart = 0; static void reset_bed_level() {
xStop = auto_bed_leveling_grid_points; if (DEBUGGING(INFO)) ECHO_LM(INFO, "reset_bed_level");
xInc = 1; for (int y = 0; y < AUTO_BED_LEVELING_GRID_POINTS; y++) {
for (int x = 0; x < AUTO_BED_LEVELING_GRID_POINTS; x++) {
bed_level[x][y] = 0.0;
} }
else { }
xStart = auto_bed_leveling_grid_points - 1;
xStop = -1;
xInc = -1;
}
zig = !zig;
for (int xCount = xStart; xCount != xStop; xCount += xInc) {
double xProbe = left_probe_bed_position + xGridSpacing * xCount;
// Avoid probing the corners (outside the round or hexagon print surface) on a delta printer.
float distance_from_center = sqrt(xProbe * xProbe + yProbe * yProbe);
if (distance_from_center > DELTA_PROBABLE_RADIUS) continue;
bed_level[xCount][yCount] = probe_bed(xProbe, yProbe);
idle();
} // xProbe
} // yProbe
extrapolate_unprobed_bed_level();
print_bed_level();
}
static void calibration_report() {
// Display Report
ECHO_LM(DB, "| \tZ-Tower\t\t\tEndstop Offsets");
ECHO_SM(DB, "| \t");
if (bed_level_z >= 0) ECHO_M(" ");
ECHO_MV("", bed_level_z, 4);
ECHO_MV("\t\t\tX:", endstop_adj[0], 4);
ECHO_MV(" Y:", endstop_adj[1], 4);
ECHO_EMV(" Z:", endstop_adj[2], 4);
ECHO_SM(DB, "| ");
if (bed_level_ox >= 0) ECHO_M(" ");
ECHO_MV("", bed_level_ox, 4);
ECHO_M("\t");
if (bed_level_oy >= 0) ECHO_M(" ");
ECHO_MV("", bed_level_oy, 4);
ECHO_EM("\t\tTower Offsets");
ECHO_SM(DB, "| \t");
if (bed_level_c >= 0) ECHO_M(" ");
ECHO_MV("", bed_level_c, 4);
ECHO_MV("\t\t\tA:",tower_adj[0]);
ECHO_MV(" B:",tower_adj[1]);
ECHO_EMV(" C:",tower_adj[2]);
ECHO_SM(DB, "| ");
if (bed_level_x >= 0) ECHO_M(" ");
ECHO_MV("", bed_level_x, 4);
ECHO_M("\t");
if (bed_level_y >= 0) ECHO_M(" ");
ECHO_MV("", bed_level_y, 4);
ECHO_MV("\t\tI:",tower_adj[3]);
ECHO_MV(" J:",tower_adj[4]);
ECHO_EMV(" K:",tower_adj[5]);
ECHO_SM(DB, "| \t");
if (bed_level_oz >= 0) ECHO_M(" ");
ECHO_MV("", bed_level_oz, 4);
ECHO_EMV("\t\t\tDelta Radius: ", delta_radius, 4);
ECHO_LMV(DB, "| X-Tower\tY-Tower\t\tDiagonal Rod: ", delta_diagonal_rod, 4);
ECHO_E;
} }
static void home_delta_axis() { static void home_delta_axis() {
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment