bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 Wiki Activity

Add custom types for position (#15204)

Browse Source
This commit is contained in:
Scott Lahteine
2019-09-29 04:25:39 -05:00
committed by GitHub
parent 43d6e9fa43
commit 50e4545255
227 changed files with 3147 additions and 3264 deletions
Download Patch File Download Diff File Expand all files Collapse all files

50
Marlin/src/gcode/calibrate/G28.cpp
View File

@ -59,7 +59,7 @@
static void quick_home_xy() {
// Pretend the current position is 0,0
current_position[X_AXIS] = current_position[Y_AXIS] = 0.0;
current_position.set(0.0, 0.0);
sync_plan_position();
const int x_axis_home_dir =
@ -95,7 +95,7 @@
endstops.validate_homing_move();
current_position[X_AXIS] = current_position[Y_AXIS] = 0.0;
current_position.set(0.0, 0.0);
#if ENABLED(SENSORLESS_HOMING)
tmc_disable_stallguard(stepperX, stealth_states.x);
@ -128,17 +128,15 @@
/**
* Move the Z probe (or just the nozzle) to the safe homing point
* (Z is already at the right height)
*/
destination[X_AXIS] = Z_SAFE_HOMING_X_POINT;
destination[Y_AXIS] = Z_SAFE_HOMING_Y_POINT;
destination[Z_AXIS] = current_position[Z_AXIS]; // Z is already at the right height
destination.set(safe_homing_xy, current_position.z);
#if HOMING_Z_WITH_PROBE
destination[X_AXIS] -= probe_offset[X_AXIS];
destination[Y_AXIS] -= probe_offset[Y_AXIS];
destination -= probe_offset;
#endif
if (position_is_reachable(destination[X_AXIS], destination[Y_AXIS])) {
if (position_is_reachable(destination)) {
if (DEBUGGING(LEVELING)) DEBUG_POS("home_z_safely", destination);
@ -151,7 +149,7 @@
safe_delay(500); // Short delay needed to settle
#endif
do_blocking_move_to_xy(destination[X_AXIS], destination[Y_AXIS]);
do_blocking_move_to_xy(destination);
homeaxis(Z_AXIS);
}
else {
@ -232,16 +230,14 @@ void GcodeSuite::G28(const bool always_home_all) {
#endif
#if ENABLED(IMPROVE_HOMING_RELIABILITY)
slow_homing_t slow_homing { 0 };
slow_homing.acceleration.x = planner.settings.max_acceleration_mm_per_s2[X_AXIS];
slow_homing.acceleration.y = planner.settings.max_acceleration_mm_per_s2[Y_AXIS];
slow_homing_t slow_homing{0};
slow_homing.acceleration.set(planner.settings.max_acceleration_mm_per_s2[X_AXIS];
planner.settings.max_acceleration_mm_per_s2[Y_AXIS]);
planner.settings.max_acceleration_mm_per_s2[X_AXIS] = 100;
planner.settings.max_acceleration_mm_per_s2[Y_AXIS] = 100;
#if HAS_CLASSIC_JERK
slow_homing.jerk.x = planner.max_jerk[X_AXIS];
slow_homing.jerk.y = planner.max_jerk[Y_AXIS];
planner.max_jerk[X_AXIS] = 0;
planner.max_jerk[Y_AXIS] = 0;
slow_homing.jerk_xy = planner.max_jerk;
planner.max_jerk.set(0, 0);
#endif
planner.reset_acceleration_rates();
@ -274,7 +270,7 @@ void GcodeSuite::G28(const bool always_home_all) {
home_all = always_home_all || (homeX == homeY && homeX == homeZ),
doX = home_all || homeX, doY = home_all || homeY, doZ = home_all || homeZ;
set_destination_from_current();
destination = current_position;
#if Z_HOME_DIR > 0 // If homing away from BED do Z first
@ -291,10 +287,10 @@ void GcodeSuite::G28(const bool always_home_all) {
if (z_homing_height && (doX || doY)) {
// Raise Z before homing any other axes and z is not already high enough (never lower z)
destination[Z_AXIS] = z_homing_height;
if (destination[Z_AXIS] > current_position[Z_AXIS]) {
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Raise Z (before homing) to ", destination[Z_AXIS]);
do_blocking_move_to_z(destination[Z_AXIS]);
destination.z = z_homing_height;
if (destination.z > current_position.z) {
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Raise Z (before homing) to ", destination.z);
do_blocking_move_to_z(destination.z);
}
}
@ -329,14 +325,14 @@ void GcodeSuite::G28(const bool always_home_all) {
homeaxis(X_AXIS);
// Remember this extruder's position for later tool change
inactive_extruder_x_pos = current_position[X_AXIS];
inactive_extruder_x_pos = current_position.x;
// Home the 1st (left) extruder
active_extruder = 0;
homeaxis(X_AXIS);
// Consider the active extruder to be parked
COPY(raised_parked_position, current_position);
raised_parked_position = current_position;
delayed_move_time = 0;
active_extruder_parked = true;
@ -390,14 +386,14 @@ void GcodeSuite::G28(const bool always_home_all) {
homeaxis(X_AXIS);
// Remember this extruder's position for later tool change
inactive_extruder_x_pos = current_position[X_AXIS];
inactive_extruder_x_pos = current_position.x;
// Home the 1st (left) extruder
active_extruder = 0;
homeaxis(X_AXIS);
// Consider the active extruder to be parked
COPY(raised_parked_position, current_position);
raised_parked_position = current_position;
delayed_move_time = 0;
active_extruder_parked = true;
extruder_duplication_enabled = IDEX_saved_duplication_state;
@ -441,10 +437,8 @@ void GcodeSuite::G28(const bool always_home_all) {
planner.settings.max_acceleration_mm_per_s2[X_AXIS] = slow_homing.acceleration.x;
planner.settings.max_acceleration_mm_per_s2[Y_AXIS] = slow_homing.acceleration.y;
#if HAS_CLASSIC_JERK
planner.max_jerk[X_AXIS] = slow_homing.jerk.x;
planner.max_jerk[Y_AXIS] = slow_homing.jerk.y;
planner.max_jerk = slow_homing.jerk_xy;
#endif
planner.reset_acceleration_rates();
#endif

162
Marlin/src/gcode/calibrate/G33.cpp
View File

@ -70,7 +70,7 @@ enum CalEnum : char { // the 7 main calibration points -
#define AC_CLEANUP() ac_cleanup()
#endif
float lcd_probe_pt(const float &rx, const float &ry);
float lcd_probe_pt(const xy_pos_t &xy);
void ac_home() {
endstops.enable(true);
@ -122,9 +122,9 @@ void print_signed_float(PGM_P const prefix, const float &f) {
static void print_calibration_settings(const bool end_stops, const bool tower_angles) {
SERIAL_ECHOPAIR(".Height:", delta_height);
if (end_stops) {
print_signed_float(PSTR("Ex"), delta_endstop_adj[A_AXIS]);
print_signed_float(PSTR("Ey"), delta_endstop_adj[B_AXIS]);
print_signed_float(PSTR("Ez"), delta_endstop_adj[C_AXIS]);
print_signed_float(PSTR("Ex"), delta_endstop_adj.a);
print_signed_float(PSTR("Ey"), delta_endstop_adj.b);
print_signed_float(PSTR("Ez"), delta_endstop_adj.c);
}
if (end_stops && tower_angles) {
SERIAL_ECHOPAIR(" Radius:", delta_radius);
@ -133,9 +133,9 @@ static void print_calibration_settings(const bool end_stops, const bool tower_an
SERIAL_ECHO_SP(13);
}
if (tower_angles) {
print_signed_float(PSTR("Tx"), delta_tower_angle_trim[A_AXIS]);
print_signed_float(PSTR("Ty"), delta_tower_angle_trim[B_AXIS]);
print_signed_float(PSTR("Tz"), delta_tower_angle_trim[C_AXIS]);
print_signed_float(PSTR("Tx"), delta_tower_angle_trim.a);
print_signed_float(PSTR("Ty"), delta_tower_angle_trim.b);
print_signed_float(PSTR("Tz"), delta_tower_angle_trim.c);
}
if ((!end_stops && tower_angles) || (end_stops && !tower_angles)) { // XOR
SERIAL_ECHOPAIR(" Radius:", delta_radius);
@ -188,12 +188,12 @@ static float std_dev_points(float z_pt[NPP + 1], const bool _0p_cal, const bool
/**
* - Probe a point
*/
static float calibration_probe(const float &nx, const float &ny, const bool stow) {
static float calibration_probe(const xy_pos_t &xy, const bool stow) {
#if HAS_BED_PROBE
return probe_at_point(nx, ny, stow ? PROBE_PT_STOW : PROBE_PT_RAISE, 0, false);
return probe_at_point(xy, stow ? PROBE_PT_STOW : PROBE_PT_RAISE, 0, false);
#else
UNUSED(stow);
return lcd_probe_pt(nx, ny);
return lcd_probe_pt(xy);
#endif
}
@ -223,7 +223,8 @@ static bool probe_calibration_points(float z_pt[NPP + 1], const int8_t probe_poi
if (!_0p_calibration) {
if (!_7p_no_intermediates && !_7p_4_intermediates && !_7p_11_intermediates) { // probe the center
z_pt[CEN] += calibration_probe(0, 0, stow_after_each);
const xy_pos_t center{0};
z_pt[CEN] += calibration_probe(center, stow_after_each);
if (isnan(z_pt[CEN])) return false;
}
@ -233,7 +234,8 @@ static bool probe_calibration_points(float z_pt[NPP + 1], const int8_t probe_poi
I_LOOP_CAL_PT(rad, start, steps) {
const float a = RADIANS(210 + (360 / NPP) * (rad - 1)),
r = delta_calibration_radius * 0.1;
z_pt[CEN] += calibration_probe(cos(a) * r, sin(a) * r, stow_after_each);
const xy_pos_t vec = { cos(a), sin(a) };
z_pt[CEN] += calibration_probe(vec * r, stow_after_each);
if (isnan(z_pt[CEN])) return false;
}
z_pt[CEN] /= float(_7p_2_intermediates ? 7 : probe_points);
@ -257,7 +259,8 @@ static bool probe_calibration_points(float z_pt[NPP + 1], const int8_t probe_poi
const float a = RADIANS(210 + (360 / NPP) * (rad - 1)),
r = delta_calibration_radius * (1 - 0.1 * (zig_zag ? offset - circle : circle)),
interpol = FMOD(rad, 1);
const float z_temp = calibration_probe(cos(a) * r, sin(a) * r, stow_after_each);
const xy_pos_t vec = { cos(a), sin(a) };
const float z_temp = calibration_probe(vec * r, stow_after_each);
if (isnan(z_temp)) return false;
// split probe point to neighbouring calibration points
z_pt[uint8_t(LROUND(rad - interpol + NPP - 1)) % NPP + 1] += z_temp * sq(cos(RADIANS(interpol * 90)));
@ -281,80 +284,69 @@ static bool probe_calibration_points(float z_pt[NPP + 1], const int8_t probe_poi
* - formulae for approximative forward kinematics in the end-stop displacement matrix
* - definition of the matrix scaling parameters
*/
static void reverse_kinematics_probe_points(float z_pt[NPP + 1], float mm_at_pt_axis[NPP + 1][ABC]) {
float pos[XYZ] = { 0.0 };
static void reverse_kinematics_probe_points(float z_pt[NPP + 1], abc_float_t mm_at_pt_axis[NPP + 1]) {
xyz_pos_t pos{0};
LOOP_CAL_ALL(rad) {
const float a = RADIANS(210 + (360 / NPP) * (rad - 1)),
r = (rad == CEN ? 0.0f : delta_calibration_radius);
pos[X_AXIS] = cos(a) * r;
pos[Y_AXIS] = sin(a) * r;
pos[Z_AXIS] = z_pt[rad];
pos.set(cos(a) * r, sin(a) * r, z_pt[rad]);
inverse_kinematics(pos);
LOOP_XYZ(axis) mm_at_pt_axis[rad][axis] = delta[axis];
mm_at_pt_axis[rad] = delta;
}
}
static void forward_kinematics_probe_points(float mm_at_pt_axis[NPP + 1][ABC], float z_pt[NPP + 1]) {
static void forward_kinematics_probe_points(abc_float_t mm_at_pt_axis[NPP + 1], float z_pt[NPP + 1]) {
const float r_quot = delta_calibration_radius / delta_radius;
#define ZPP(N,I,A) ((1 / 3.0f + r_quot * (N) / 3.0f ) * mm_at_pt_axis[I][A])
#define ZPP(N,I,A) (((1.0f + r_quot * (N)) / 3.0f) * mm_at_pt_axis[I].A)
#define Z00(I, A) ZPP( 0, I, A)
#define Zp1(I, A) ZPP(+1, I, A)
#define Zm1(I, A) ZPP(-1, I, A)
#define Zp2(I, A) ZPP(+2, I, A)
#define Zm2(I, A) ZPP(-2, I, A)
z_pt[CEN] = Z00(CEN, A_AXIS) + Z00(CEN, B_AXIS) + Z00(CEN, C_AXIS);
z_pt[__A] = Zp2(__A, A_AXIS) + Zm1(__A, B_AXIS) + Zm1(__A, C_AXIS);
z_pt[__B] = Zm1(__B, A_AXIS) + Zp2(__B, B_AXIS) + Zm1(__B, C_AXIS);
z_pt[__C] = Zm1(__C, A_AXIS) + Zm1(__C, B_AXIS) + Zp2(__C, C_AXIS);
z_pt[_BC] = Zm2(_BC, A_AXIS) + Zp1(_BC, B_AXIS) + Zp1(_BC, C_AXIS);
z_pt[_CA] = Zp1(_CA, A_AXIS) + Zm2(_CA, B_AXIS) + Zp1(_CA, C_AXIS);
z_pt[_AB] = Zp1(_AB, A_AXIS) + Zp1(_AB, B_AXIS) + Zm2(_AB, C_AXIS);
z_pt[CEN] = Z00(CEN, a) + Z00(CEN, b) + Z00(CEN, c);
z_pt[__A] = Zp2(__A, a) + Zm1(__A, b) + Zm1(__A, c);
z_pt[__B] = Zm1(__B, a) + Zp2(__B, b) + Zm1(__B, c);
z_pt[__C] = Zm1(__C, a) + Zm1(__C, b) + Zp2(__C, c);
z_pt[_BC] = Zm2(_BC, a) + Zp1(_BC, b) + Zp1(_BC, c);
z_pt[_CA] = Zp1(_CA, a) + Zm2(_CA, b) + Zp1(_CA, c);
z_pt[_AB] = Zp1(_AB, a) + Zp1(_AB, b) + Zm2(_AB, c);
}
static void calc_kinematics_diff_probe_points(float z_pt[NPP + 1], float delta_e[ABC], float delta_r, float delta_t[ABC]) {
static void calc_kinematics_diff_probe_points(float z_pt[NPP + 1], abc_float_t delta_e, const float delta_r, abc_float_t delta_t) {
const float z_center = z_pt[CEN];
float diff_mm_at_pt_axis[NPP + 1][ABC],
new_mm_at_pt_axis[NPP + 1][ABC];
abc_float_t diff_mm_at_pt_axis[NPP + 1], new_mm_at_pt_axis[NPP + 1];
reverse_kinematics_probe_points(z_pt, diff_mm_at_pt_axis);
delta_radius += delta_r;
LOOP_XYZ(axis) delta_tower_angle_trim[axis] += delta_t[axis];
delta_tower_angle_trim += delta_t;
recalc_delta_settings();
reverse_kinematics_probe_points(z_pt, new_mm_at_pt_axis);
LOOP_XYZ(axis) LOOP_CAL_ALL(rad) diff_mm_at_pt_axis[rad][axis] -= new_mm_at_pt_axis[rad][axis] + delta_e[axis];
LOOP_CAL_ALL(rad) diff_mm_at_pt_axis[rad] -= new_mm_at_pt_axis[rad] + delta_e;
forward_kinematics_probe_points(diff_mm_at_pt_axis, z_pt);
LOOP_CAL_RAD(rad) z_pt[rad] -= z_pt[CEN] - z_center;
z_pt[CEN] = z_center;
delta_radius -= delta_r;
LOOP_XYZ(axis) delta_tower_angle_trim[axis] -= delta_t[axis];
delta_tower_angle_trim -= delta_t;
recalc_delta_settings();
}
static float auto_tune_h() {
const float r_quot = delta_calibration_radius / delta_radius;
float h_fac = 0.0f;
h_fac = r_quot / (2.0f / 3.0f);
h_fac = 1.0f / h_fac; // (2/3)/CR
return h_fac;
return RECIPROCAL(r_quot / (2.0f / 3.0f)); // (2/3)/CR
}
static float auto_tune_r() {
const float diff = 0.01f;
float r_fac = 0.0f,
z_pt[NPP + 1] = { 0.0f },
delta_e[ABC] = { 0.0f },
delta_r = { 0.0f },
delta_t[ABC] = { 0.0f };
constexpr float diff = 0.01f, delta_r = diff;
float r_fac = 0.0f, z_pt[NPP + 1] = { 0.0f };
abc_float_t delta_e = { 0.0f }, delta_t = { 0.0f };
delta_r = diff;
calc_kinematics_diff_probe_points(z_pt, delta_e, delta_r, delta_t);
r_fac = -(z_pt[__A] + z_pt[__B] + z_pt[__C] + z_pt[_BC] + z_pt[_CA] + z_pt[_AB]) / 6.0f;
r_fac = diff / r_fac / 3.0f; // 1/(3*delta_Z)
@ -362,14 +354,11 @@ static float auto_tune_r() {
}
static float auto_tune_a() {
const float diff = 0.01f;
float a_fac = 0.0f,
z_pt[NPP + 1] = { 0.0f },
delta_e[ABC] = { 0.0f },
delta_r = { 0.0f },
delta_t[ABC] = { 0.0f };
constexpr float diff = 0.01f, delta_r = 0.0f;
float a_fac = 0.0f, z_pt[NPP + 1] = { 0.0f };
abc_float_t delta_e = { 0.0f }, delta_t = { 0.0f };
ZERO(delta_t);
delta_t.reset();
LOOP_XYZ(axis) {
delta_t[axis] = diff;
calc_kinematics_diff_probe_points(z_pt, delta_e, delta_r, delta_t);
@ -453,21 +442,11 @@ void GcodeSuite::G33() {
zero_std_dev = (verbose_level ? 999.0f : 0.0f), // 0.0 in dry-run mode : forced end
zero_std_dev_min = zero_std_dev,
zero_std_dev_old = zero_std_dev,
h_factor,
r_factor,
a_factor,
e_old[ABC] = {
delta_endstop_adj[A_AXIS],
delta_endstop_adj[B_AXIS],
delta_endstop_adj[C_AXIS]
},
h_factor, r_factor, a_factor,
r_old = delta_radius,
h_old = delta_height,
a_old[ABC] = {
delta_tower_angle_trim[A_AXIS],
delta_tower_angle_trim[B_AXIS],
delta_tower_angle_trim[C_AXIS]
};
h_old = delta_height;
abc_pos_t e_old = delta_endstop_adj, a_old = delta_tower_angle_trim;
SERIAL_ECHOLNPGM("G33 Auto Calibrate");
@ -520,15 +499,14 @@ void GcodeSuite::G33() {
if (zero_std_dev < zero_std_dev_min) {
// set roll-back point
COPY(e_old, delta_endstop_adj);
e_old = delta_endstop_adj;
r_old = delta_radius;
h_old = delta_height;
COPY(a_old, delta_tower_angle_trim);
a_old = delta_tower_angle_trim;
}
float e_delta[ABC] = { 0.0f },
r_delta = 0.0f,
t_delta[ABC] = { 0.0f };
abc_float_t e_delta = { 0.0f }, t_delta = { 0.0f };
float r_delta = 0.0f;
/**
* convergence matrices:
@ -563,42 +541,42 @@ void GcodeSuite::G33() {
case 2:
if (towers_set) { // see 4 point calibration (towers) matrix
e_delta[A_AXIS] = (+Z4(__A) -Z2(__B) -Z2(__C)) * h_factor +Z4(CEN);
e_delta[B_AXIS] = (-Z2(__A) +Z4(__B) -Z2(__C)) * h_factor +Z4(CEN);
e_delta[C_AXIS] = (-Z2(__A) -Z2(__B) +Z4(__C)) * h_factor +Z4(CEN);
r_delta = (+Z4(__A) +Z4(__B) +Z4(__C) -Z12(CEN)) * r_factor;
e_delta.set((+Z4(__A) -Z2(__B) -Z2(__C)) * h_factor +Z4(CEN),
(-Z2(__A) +Z4(__B) -Z2(__C)) * h_factor +Z4(CEN),
(-Z2(__A) -Z2(__B) +Z4(__C)) * h_factor +Z4(CEN));
r_delta = (+Z4(__A) +Z4(__B) +Z4(__C) -Z12(CEN)) * r_factor;
}
else { // see 4 point calibration (opposites) matrix
e_delta[A_AXIS] = (-Z4(_BC) +Z2(_CA) +Z2(_AB)) * h_factor +Z4(CEN);
e_delta[B_AXIS] = (+Z2(_BC) -Z4(_CA) +Z2(_AB)) * h_factor +Z4(CEN);
e_delta[C_AXIS] = (+Z2(_BC) +Z2(_CA) -Z4(_AB)) * h_factor +Z4(CEN);
r_delta = (+Z4(_BC) +Z4(_CA) +Z4(_AB) -Z12(CEN)) * r_factor;
e_delta.set((-Z4(_BC) +Z2(_CA) +Z2(_AB)) * h_factor +Z4(CEN),
(+Z2(_BC) -Z4(_CA) +Z2(_AB)) * h_factor +Z4(CEN),
(+Z2(_BC) +Z2(_CA) -Z4(_AB)) * h_factor +Z4(CEN));
r_delta = (+Z4(_BC) +Z4(_CA) +Z4(_AB) -Z12(CEN)) * r_factor;
}
break;
default: // see 7 point calibration (towers & opposites) matrix
e_delta[A_AXIS] = (+Z2(__A) -Z1(__B) -Z1(__C) -Z2(_BC) +Z1(_CA) +Z1(_AB)) * h_factor +Z4(CEN);
e_delta[B_AXIS] = (-Z1(__A) +Z2(__B) -Z1(__C) +Z1(_BC) -Z2(_CA) +Z1(_AB)) * h_factor +Z4(CEN);
e_delta[C_AXIS] = (-Z1(__A) -Z1(__B) +Z2(__C) +Z1(_BC) +Z1(_CA) -Z2(_AB)) * h_factor +Z4(CEN);
r_delta = (+Z2(__A) +Z2(__B) +Z2(__C) +Z2(_BC) +Z2(_CA) +Z2(_AB) -Z12(CEN)) * r_factor;
e_delta.set((+Z2(__A) -Z1(__B) -Z1(__C) -Z2(_BC) +Z1(_CA) +Z1(_AB)) * h_factor +Z4(CEN),
(-Z1(__A) +Z2(__B) -Z1(__C) +Z1(_BC) -Z2(_CA) +Z1(_AB)) * h_factor +Z4(CEN),
(-Z1(__A) -Z1(__B) +Z2(__C) +Z1(_BC) +Z1(_CA) -Z2(_AB)) * h_factor +Z4(CEN));
r_delta = (+Z2(__A) +Z2(__B) +Z2(__C) +Z2(_BC) +Z2(_CA) +Z2(_AB) -Z12(CEN)) * r_factor;
if (towers_set) { // see 7 point tower angle calibration (towers & opposites) matrix
t_delta[A_AXIS] = (+Z0(__A) -Z4(__B) +Z4(__C) +Z0(_BC) -Z4(_CA) +Z4(_AB) +Z0(CEN)) * a_factor;
t_delta[B_AXIS] = (+Z4(__A) +Z0(__B) -Z4(__C) +Z4(_BC) +Z0(_CA) -Z4(_AB) +Z0(CEN)) * a_factor;
t_delta[C_AXIS] = (-Z4(__A) +Z4(__B) +Z0(__C) -Z4(_BC) +Z4(_CA) +Z0(_AB) +Z0(CEN)) * a_factor;
t_delta.set((+Z0(__A) -Z4(__B) +Z4(__C) +Z0(_BC) -Z4(_CA) +Z4(_AB) +Z0(CEN)) * a_factor,
(+Z4(__A) +Z0(__B) -Z4(__C) +Z4(_BC) +Z0(_CA) -Z4(_AB) +Z0(CEN)) * a_factor,
(-Z4(__A) +Z4(__B) +Z0(__C) -Z4(_BC) +Z4(_CA) +Z0(_AB) +Z0(CEN)) * a_factor);
}
break;
}
LOOP_XYZ(axis) delta_endstop_adj[axis] += e_delta[axis];
delta_endstop_adj += e_delta;
delta_radius += r_delta;
LOOP_XYZ(axis) delta_tower_angle_trim[axis] += t_delta[axis];
delta_tower_angle_trim += t_delta;
}
else if (zero_std_dev >= test_precision) {
// roll back
COPY(delta_endstop_adj, e_old);
delta_endstop_adj = e_old;
delta_radius = r_old;
delta_height = h_old;
COPY(delta_tower_angle_trim, a_old);
delta_tower_angle_trim = a_old;
}
if (verbose_level != 0) { // !dry run
@ -611,7 +589,7 @@ void GcodeSuite::G33() {
}
// adjust delta_height and endstops by the max amount
const float z_temp = _MAX(delta_endstop_adj[A_AXIS], delta_endstop_adj[B_AXIS], delta_endstop_adj[C_AXIS]);
const float z_temp = _MAX(delta_endstop_adj.a, delta_endstop_adj.b, delta_endstop_adj.c);
delta_height -= z_temp;
LOOP_XYZ(axis) delta_endstop_adj[axis] -= z_temp;
}

41
Marlin/src/gcode/calibrate/G34_M422.cpp
View File

@ -45,8 +45,17 @@
#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
#include "../../core/debug_out.h"
float z_auto_align_xpos[Z_STEPPER_COUNT] = Z_STEPPER_ALIGN_X,
z_auto_align_ypos[Z_STEPPER_COUNT] = Z_STEPPER_ALIGN_Y;
// Sanity-check
constexpr xy_pos_t sanity_arr_z_align[] = Z_STEPPER_ALIGN_XY;
static_assert(COUNT(sanity_arr_z_align) == Z_STEPPER_COUNT,
#if ENABLED(Z_TRIPLE_STEPPER_DRIVERS)
"Z_STEPPER_ALIGN_XY requires three {X,Y} entries (Z, Z2, and Z3)."
#else
"Z_STEPPER_ALIGN_XY requires two {X,Y} entries (Z and Z2)."
#endif
);
xy_pos_t z_auto_align_pos[Z_STEPPER_COUNT] = Z_STEPPER_ALIGN_XY;
inline void set_all_z_lock(const bool lock) {
stepper.set_z_lock(lock);
@ -123,11 +132,11 @@ void GcodeSuite::G34() {
float z_probe = Z_BASIC_CLEARANCE + (G34_MAX_GRADE) * 0.01f * (
#if ENABLED(Z_TRIPLE_STEPPER_DRIVERS)
SQRT(_MAX(HYPOT2(z_auto_align_xpos[0] - z_auto_align_ypos[0], z_auto_align_xpos[1] - z_auto_align_ypos[1]),
HYPOT2(z_auto_align_xpos[1] - z_auto_align_ypos[1], z_auto_align_xpos[2] - z_auto_align_ypos[2]),
HYPOT2(z_auto_align_xpos[2] - z_auto_align_ypos[2], z_auto_align_xpos[0] - z_auto_align_ypos[0])))
SQRT(_MAX(HYPOT2(z_auto_align_pos[0].x - z_auto_align_pos[0].y, z_auto_align_pos[1].x - z_auto_align_pos[1].y),
HYPOT2(z_auto_align_pos[1].x - z_auto_align_pos[1].y, z_auto_align_pos[2].x - z_auto_align_pos[2].y),
HYPOT2(z_auto_align_pos[2].x - z_auto_align_pos[2].y, z_auto_align_pos[0].x - z_auto_align_pos[0].y)))
#else
HYPOT(z_auto_align_xpos[0] - z_auto_align_ypos[0], z_auto_align_xpos[1] - z_auto_align_ypos[1])
HYPOT(z_auto_align_pos[0].x - z_auto_align_pos[0].y, z_auto_align_pos[1].x - z_auto_align_pos[1].y)
#endif
);
@ -135,7 +144,7 @@ void GcodeSuite::G34() {
if (!all_axes_known()) home_all_axes();
// Move the Z coordinate realm towards the positive - dirty trick
current_position[Z_AXIS] -= z_probe * 0.5;
current_position.z -= z_probe * 0.5f;
float last_z_align_move[Z_STEPPER_COUNT] = ARRAY_N(Z_STEPPER_COUNT, 10000.0f, 10000.0f, 10000.0f),
z_measured[Z_STEPPER_COUNT] = { 0 },
@ -162,7 +171,7 @@ void GcodeSuite::G34() {
if (iteration == 0 || izstepper > 0) do_blocking_move_to_z(z_probe);
// Probe a Z height for each stepper.
const float z_probed_height = probe_at_point(z_auto_align_xpos[zstepper], z_auto_align_ypos[zstepper], raise_after, 0, true);
const float z_probed_height = probe_at_point(z_auto_align_pos[zstepper], raise_after, 0, true);
if (isnan(z_probed_height)) {
SERIAL_ECHOLNPGM("Probing failed.");
err_break = true;
@ -240,7 +249,7 @@ void GcodeSuite::G34() {
}
// Do a move to correct part of the misalignment for the current stepper
do_blocking_move_to_z(amplification * z_align_move + current_position[Z_AXIS]);
do_blocking_move_to_z(amplification * z_align_move + current_position.z);
} // for (zstepper)
// Back to normal stepper operations
@ -299,20 +308,22 @@ void GcodeSuite::M422() {
return;
}
const float x_pos = parser.floatval('X', z_auto_align_xpos[zstepper]);
if (!WITHIN(x_pos, X_MIN_POS, X_MAX_POS)) {
const xy_pos_t pos = {
parser.floatval('X', z_auto_align_pos[zstepper].x),
parser.floatval('Y', z_auto_align_pos[zstepper].y)
};
if (!WITHIN(pos.x, X_MIN_POS, X_MAX_POS)) {
SERIAL_ECHOLNPGM("?(X) out of bounds.");
return;
}
const float y_pos = parser.floatval('Y', z_auto_align_ypos[zstepper]);
if (!WITHIN(y_pos, Y_MIN_POS, Y_MAX_POS)) {
if (!WITHIN(pos.y, Y_MIN_POS, Y_MAX_POS)) {
SERIAL_ECHOLNPGM("?(Y) out of bounds.");
return;
}
z_auto_align_xpos[zstepper] = x_pos;
z_auto_align_ypos[zstepper] = y_pos;
z_auto_align_pos[zstepper] = pos;
}
#endif // Z_STEPPER_AUTO_ALIGN

154
Marlin/src/gcode/calibrate/G425.cpp
View File

@ -61,17 +61,17 @@
enum side_t : uint8_t { TOP, RIGHT, FRONT, LEFT, BACK, NUM_SIDES };
static constexpr xyz_pos_t true_center CALIBRATION_OBJECT_CENTER;
static constexpr xyz_float_t dimensions CALIBRATION_OBJECT_DIMENSIONS;
static constexpr xy_float_t nod = { CALIBRATION_NOZZLE_OUTER_DIAMETER, CALIBRATION_NOZZLE_OUTER_DIAMETER };
struct measurements_t {
static constexpr float dimensions[XYZ] = CALIBRATION_OBJECT_DIMENSIONS;
static constexpr float true_center[XYZ] = CALIBRATION_OBJECT_CENTER;
xyz_pos_t obj_center = true_center; // Non-static must be assigned from xyz_pos_t
float obj_center[XYZ] = CALIBRATION_OBJECT_CENTER;
float obj_side[NUM_SIDES];
float obj_side[NUM_SIDES], backlash[NUM_SIDES];
xyz_float_t pos_error;
float backlash[NUM_SIDES];
float pos_error[XYZ];
float nozzle_outer_dimension[2] = {CALIBRATION_NOZZLE_OUTER_DIAMETER, CALIBRATION_NOZZLE_OUTER_DIAMETER};
xy_float_t nozzle_outer_dimension = nod;
};
#define TEMPORARY_SOFT_ENDSTOP_STATE(enable) REMEMBER(tes, soft_endstops_enabled, enable);
@ -88,29 +88,8 @@ struct measurements_t {
#define TEMPORARY_BACKLASH_SMOOTHING(value)
#endif
/**
* Move to a particular location. Up to three individual axes
* and their destinations can be specified, in any order.
*/
inline void move_to(
const AxisEnum a1 = NO_AXIS, const float p1 = 0,
const AxisEnum a2 = NO_AXIS, const float p2 = 0,
const AxisEnum a3 = NO_AXIS, const float p3 = 0
) {
set_destination_from_current();
// Note: The order of p1, p2, p3 may not correspond to X, Y, Z
if (a1 != NO_AXIS) destination[a1] = p1;
if (a2 != NO_AXIS) destination[a2] = p2;
if (a3 != NO_AXIS) destination[a3] = p3;
// Make sure coordinates are within bounds
destination[X_AXIS] = _MAX(_MIN(destination[X_AXIS], X_MAX_POS), X_MIN_POS);
destination[Y_AXIS] = _MAX(_MIN(destination[Y_AXIS], Y_MAX_POS), Y_MIN_POS);
destination[Z_AXIS] = _MAX(_MIN(destination[Z_AXIS], Z_MAX_POS), Z_MIN_POS);
// Move to position
do_blocking_move_to(destination, MMM_TO_MMS(CALIBRATION_FEEDRATE_TRAVEL));
inline void calibration_move() {
do_blocking_move_to(current_position, MMM_TO_MMS(CALIBRATION_FEEDRATE_TRAVEL));
}
/**
@ -121,10 +100,12 @@ inline void move_to(
*/
inline void park_above_object(measurements_t &m, const float uncertainty) {
// Move to safe distance above calibration object
move_to(Z_AXIS, m.obj_center[Z_AXIS] + m.dimensions[Z_AXIS] / 2 + uncertainty);
current_position.z = m.obj_center.z + dimensions.z / 2 + uncertainty;
calibration_move();
// Move to center of calibration object in XY
move_to(X_AXIS, m.obj_center[X_AXIS], Y_AXIS, m.obj_center[Y_AXIS]);
current_position = xy_pos_t(m.obj_center);
calibration_move();
}
#if HOTENDS > 1
@ -139,14 +120,9 @@ inline void park_above_object(measurements_t &m, const float uncertainty) {
#if HAS_HOTEND_OFFSET
inline void normalize_hotend_offsets() {
for (uint8_t e = 1; e < HOTENDS; e++) {
hotend_offset[X_AXIS][e] -= hotend_offset[X_AXIS][0];
hotend_offset[Y_AXIS][e] -= hotend_offset[Y_AXIS][0];
hotend_offset[Z_AXIS][e] -= hotend_offset[Z_AXIS][0];
}
hotend_offset[X_AXIS][0] = 0;
hotend_offset[Y_AXIS][0] = 0;
hotend_offset[Z_AXIS][0] = 0;
for (uint8_t e = 1; e < HOTENDS; e++)
hotend_offset[e] -= hotend_offset[0];
hotend_offset[0].reset();
}
#endif
@ -175,7 +151,7 @@ float measuring_movement(const AxisEnum axis, const int dir, const bool stop_sta
const feedRate_t mms = fast ? MMM_TO_MMS(CALIBRATION_FEEDRATE_FAST) : MMM_TO_MMS(CALIBRATION_FEEDRATE_SLOW);
const float limit = fast ? 50 : 5;
set_destination_from_current();
destination = current_position;
for (float travel = 0; travel < limit; travel += step) {
destination[axis] += dir * step;
do_blocking_move_to(destination, mms);
@ -199,7 +175,7 @@ inline float measure(const AxisEnum axis, const int dir, const bool stop_state,
const bool fast = uncertainty == CALIBRATION_MEASUREMENT_UNKNOWN;
// Save position
set_destination_from_current();
destination = current_position;
const float start_pos = destination[axis];
const float measured_pos = measuring_movement(axis, dir, stop_state, fast);
// Measure backlash
@ -223,7 +199,7 @@ inline float measure(const AxisEnum axis, const int dir, const bool stop_state,
* to find out height of edge
*/
inline void probe_side(measurements_t &m, const float uncertainty, const side_t side, const bool probe_top_at_edge=false) {
const float dimensions[] = CALIBRATION_OBJECT_DIMENSIONS;
const xyz_float_t dimensions = CALIBRATION_OBJECT_DIMENSIONS;
AxisEnum axis;
float dir;
@ -232,7 +208,7 @@ inline void probe_side(measurements_t &m, const float uncertainty, const side_t
switch (side) {
case TOP: {
const float measurement = measure(Z_AXIS, -1, true, &m.backlash[TOP], uncertainty);
m.obj_center[Z_AXIS] = measurement - dimensions[Z_AXIS] / 2;
m.obj_center.z = measurement - dimensions.z / 2;
m.obj_side[TOP] = measurement;
return;
}
@ -240,22 +216,24 @@ inline void probe_side(measurements_t &m, const float uncertainty, const side_t
case FRONT: axis = Y_AXIS; dir = 1; break;
case LEFT: axis = X_AXIS; dir = 1; break;
case BACK: axis = Y_AXIS; dir = -1; break;
default:
return;
default: return;
}
if (probe_top_at_edge) {
// Probe top nearest the side we are probing
move_to(axis, m.obj_center[axis] + (-dir) * (dimensions[axis] / 2 - m.nozzle_outer_dimension[axis]));
current_position[axis] = m.obj_center[axis] + (-dir) * (dimensions[axis] / 2 - m.nozzle_outer_dimension[axis]);
calibration_move();
m.obj_side[TOP] = measure(Z_AXIS, -1, true, &m.backlash[TOP], uncertainty);
m.obj_center[Z_AXIS] = m.obj_side[TOP] - dimensions[Z_AXIS] / 2;
m.obj_center.z = m.obj_side[TOP] - dimensions.z / 2;
}
// Move to safe distance to the side of the calibration object
move_to(axis, m.obj_center[axis] + (-dir) * (dimensions[axis] / 2 + m.nozzle_outer_dimension[axis] / 2 + uncertainty));
current_position[axis] = m.obj_center[axis] + (-dir) * (dimensions[axis] / 2 + m.nozzle_outer_dimension[axis] / 2 + uncertainty);
calibration_move();
// Plunge below the side of the calibration object and measure
move_to(Z_AXIS, m.obj_side[TOP] - CALIBRATION_NOZZLE_TIP_HEIGHT * 0.7);
current_position.z = m.obj_side[TOP] - CALIBRATION_NOZZLE_TIP_HEIGHT * 0.7;
calibration_move();
const float measurement = measure(axis, dir, true, &m.backlash[side], uncertainty);
m.obj_center[axis] = measurement + dir * (dimensions[axis] / 2 + m.nozzle_outer_dimension[axis] / 2);
m.obj_side[side] = measurement;
@ -294,36 +272,36 @@ inline void probe_sides(measurements_t &m, const float uncertainty) {
// Compute the measured center of the calibration object.
#if HAS_X_CENTER
m.obj_center[X_AXIS] = (m.obj_side[LEFT] + m.obj_side[RIGHT]) / 2;
m.obj_center.x = (m.obj_side[LEFT] + m.obj_side[RIGHT]) / 2;
#endif
#if HAS_Y_CENTER
m.obj_center[Y_AXIS] = (m.obj_side[FRONT] + m.obj_side[BACK]) / 2;
m.obj_center.y = (m.obj_side[FRONT] + m.obj_side[BACK]) / 2;
#endif
// Compute the outside diameter of the nozzle at the height
// at which it makes contact with the calibration object
#if HAS_X_CENTER
m.nozzle_outer_dimension[X_AXIS] = m.obj_side[RIGHT] - m.obj_side[LEFT] - m.dimensions[X_AXIS];
m.nozzle_outer_dimension.x = m.obj_side[RIGHT] - m.obj_side[LEFT] - dimensions.x;
#endif
#if HAS_Y_CENTER
m.nozzle_outer_dimension[Y_AXIS] = m.obj_side[BACK] - m.obj_side[FRONT] - m.dimensions[Y_AXIS];
m.nozzle_outer_dimension.y = m.obj_side[BACK] - m.obj_side[FRONT] - dimensions.y;
#endif
park_above_object(m, uncertainty);
// The difference between the known and the measured location
// of the calibration object is the positional error
m.pos_error[X_AXIS] = (0
m.pos_error.x = (0
#if HAS_X_CENTER
+ m.true_center[X_AXIS] - m.obj_center[X_AXIS]
+ true_center.x - m.obj_center.x
#endif
);
m.pos_error[Y_AXIS] = (0
m.pos_error.y = (0
#if HAS_Y_CENTER
+ m.true_center[Y_AXIS] - m.obj_center[Y_AXIS]
+ true_center.y - m.obj_center.y
#endif
);
m.pos_error[Z_AXIS] = m.true_center[Z_AXIS] - m.obj_center[Z_AXIS];
m.pos_error.z = true_center.z - m.obj_center.z;
}
#if ENABLED(CALIBRATION_REPORTING)
@ -348,12 +326,12 @@ inline void probe_sides(measurements_t &m, const float uncertainty) {
inline void report_measured_center(const measurements_t &m) {
SERIAL_ECHOLNPGM("Center:");
#if HAS_X_CENTER
SERIAL_ECHOLNPAIR(" X", m.obj_center[X_AXIS]);
SERIAL_ECHOLNPAIR(" X", m.obj_center.x);
#endif
#if HAS_Y_CENTER
SERIAL_ECHOLNPAIR(" Y", m.obj_center[Y_AXIS]);
SERIAL_ECHOLNPAIR(" Y", m.obj_center.y);
#endif
SERIAL_ECHOLNPAIR(" Z", m.obj_center[Z_AXIS]);
SERIAL_ECHOLNPAIR(" Z", m.obj_center.z);
SERIAL_EOL();
}
@ -380,12 +358,12 @@ inline void probe_sides(measurements_t &m, const float uncertainty) {
SERIAL_ECHO(int(active_extruder));
SERIAL_ECHOLNPGM(" Positional Error:");
#if HAS_X_CENTER
SERIAL_ECHOLNPAIR(" X", m.pos_error[X_AXIS]);
SERIAL_ECHOLNPAIR(" X", m.pos_error.x);
#endif
#if HAS_Y_CENTER
SERIAL_ECHOLNPAIR(" Y", m.pos_error[Y_AXIS]);
SERIAL_ECHOLNPAIR(" Y", m.pos_error.y);
#endif
SERIAL_ECHOLNPAIR(" Z", m.pos_error[Z_AXIS]);
SERIAL_ECHOLNPAIR(" Z", m.pos_error.z);
SERIAL_EOL();
}
@ -393,10 +371,10 @@ inline void probe_sides(measurements_t &m, const float uncertainty) {
SERIAL_ECHOLNPGM("Nozzle Tip Outer Dimensions:");
#if HAS_X_CENTER || HAS_Y_CENTER
#if HAS_X_CENTER
SERIAL_ECHOLNPAIR(" X", m.nozzle_outer_dimension[X_AXIS]);
SERIAL_ECHOLNPAIR(" X", m.nozzle_outer_dimension.x);
#endif
#if HAS_Y_CENTER
SERIAL_ECHOLNPAIR(" Y", m.nozzle_outer_dimension[Y_AXIS]);
SERIAL_ECHOLNPAIR(" Y", m.nozzle_outer_dimension.y);
#endif
#else
UNUSED(m);
@ -410,7 +388,7 @@ inline void probe_sides(measurements_t &m, const float uncertainty) {
//
inline void report_hotend_offsets() {
for (uint8_t e = 1; e < HOTENDS; e++)
SERIAL_ECHOLNPAIR("T", int(e), " Hotend Offset X", hotend_offset[X_AXIS][e], " Y", hotend_offset[Y_AXIS][e], " Z", hotend_offset[Z_AXIS][e]);
SERIAL_ECHOLNPAIR("T", int(e), " Hotend Offset X", hotend_offset[e].x, " Y", hotend_offset[e].y, " Z", hotend_offset[e].z);
}
#endif
@ -434,49 +412,40 @@ inline void calibrate_backlash(measurements_t &m, const float uncertainty) {
#if ENABLED(BACKLASH_GCODE)
#if HAS_X_CENTER
backlash.distance_mm[X_AXIS] = (m.backlash[LEFT] + m.backlash[RIGHT]) / 2;
backlash.distance_mm.x = (m.backlash[LEFT] + m.backlash[RIGHT]) / 2;
#elif ENABLED(CALIBRATION_MEASURE_LEFT)
backlash.distance_mm[X_AXIS] = m.backlash[LEFT];
backlash.distance_mm.x = m.backlash[LEFT];
#elif ENABLED(CALIBRATION_MEASURE_RIGHT)
backlash.distance_mm[X_AXIS] = m.backlash[RIGHT];
backlash.distance_mm.x = m.backlash[RIGHT];
#endif
#if HAS_Y_CENTER
backlash.distance_mm[Y_AXIS] = (m.backlash[FRONT] + m.backlash[BACK]) / 2;
backlash.distance_mm.y = (m.backlash[FRONT] + m.backlash[BACK]) / 2;
#elif ENABLED(CALIBRATION_MEASURE_FRONT)
backlash.distance_mm[Y_AXIS] = m.backlash[FRONT];
backlash.distance_mm.y = m.backlash[FRONT];
#elif ENABLED(CALIBRATION_MEASURE_BACK)
backlash.distance_mm[Y_AXIS] = m.backlash[BACK];
backlash.distance_mm.y = m.backlash[BACK];
#endif
backlash.distance_mm[Z_AXIS] = m.backlash[TOP];
backlash.distance_mm.z = m.backlash[TOP];
#endif
}
#if ENABLED(BACKLASH_GCODE)
// Turn on backlash compensation and move in all
// directions to take up any backlash
{
// New scope for TEMPORARY_BACKLASH_CORRECTION
TEMPORARY_BACKLASH_CORRECTION(all_on);
TEMPORARY_BACKLASH_SMOOTHING(0.0f);
move_to(
X_AXIS, current_position[X_AXIS] + 3,
Y_AXIS, current_position[Y_AXIS] + 3,
Z_AXIS, current_position[Z_AXIS] + 3
);
move_to(
X_AXIS, current_position[X_AXIS] - 3,
Y_AXIS, current_position[Y_AXIS] - 3,
Z_AXIS, current_position[Z_AXIS] - 3
);
const xyz_float_t move = { 3, 3, 3 };
current_position += move; calibration_move();
current_position -= move; calibration_move();
}
#endif
}
inline void update_measurements(measurements_t &m, const AxisEnum axis) {
const float true_center[XYZ] = CALIBRATION_OBJECT_CENTER;
current_position[axis] += m.pos_error[axis];
m.obj_center[axis] = true_center[axis];
m.pos_error[axis] = 0;
@ -508,12 +477,12 @@ inline void calibrate_toolhead(measurements_t &m, const float uncertainty, const
// Adjust the hotend offset
#if HAS_HOTEND_OFFSET
#if HAS_X_CENTER
hotend_offset[X_AXIS][extruder] += m.pos_error[X_AXIS];
hotend_offset[extruder].x += m.pos_error.x;
#endif
#if HAS_Y_CENTER
hotend_offset[Y_AXIS][extruder] += m.pos_error[Y_AXIS];
hotend_offset[extruder].y += m.pos_error.y;
#endif
hotend_offset[Z_AXIS][extruder] += m.pos_error[Z_AXIS];
hotend_offset[extruder].z += m.pos_error.z;
normalize_hotend_offsets();
#endif
@ -589,7 +558,8 @@ inline void calibrate_all() {
// Do a slow and precise calibration of the toolheads
calibrate_all_toolheads(m, CALIBRATION_MEASUREMENT_UNCERTAIN);
move_to(X_AXIS, 150); // Park nozzle away from calibration object
current_position.x = X_CENTER;
calibration_move(); // Park nozzle away from calibration object
}
/**

34
Marlin/src/gcode/calibrate/M48.cpp
View File

@ -74,13 +74,14 @@ void GcodeSuite::M48() {
const ProbePtRaise raise_after = parser.boolval('E') ? PROBE_PT_STOW : PROBE_PT_RAISE;
float X_current = current_position[X_AXIS],
Y_current = current_position[Y_AXIS];
xy_float_t next_pos = current_position;
const float X_probe_location = parser.linearval('X', X_current + probe_offset[X_AXIS]),
Y_probe_location = parser.linearval('Y', Y_current + probe_offset[Y_AXIS]);
const xy_pos_t probe_pos = {
parser.linearval('X', next_pos.x + probe_offset.x),
parser.linearval('Y', next_pos.y + probe_offset.y)
};
if (!position_is_reachable_by_probe(X_probe_location, Y_probe_location)) {
if (!position_is_reachable_by_probe(probe_pos)) {
SERIAL_ECHOLNPGM("? (X,Y) out of bounds.");
return;
}
@ -116,7 +117,7 @@ void GcodeSuite::M48() {
float mean = 0.0, sigma = 0.0, min = 99999.9, max = -99999.9, sample_set[n_samples];
// Move to the first point, deploy, and probe
const float t = probe_at_point(X_probe_location, Y_probe_location, raise_after, verbose_level);
const float t = probe_at_point(probe_pos, raise_after, verbose_level);
bool probing_good = !isnan(t);
if (probing_good) {
@ -165,32 +166,31 @@ void GcodeSuite::M48() {
while (angle < 0.0) angle += 360.0; // outside of this range. It looks like they behave correctly with
// numbers outside of the range, but just to be safe we clamp them.
X_current = X_probe_location - probe_offset[X_AXIS] + cos(RADIANS(angle)) * radius;
Y_current = Y_probe_location - probe_offset[Y_AXIS] + sin(RADIANS(angle)) * radius;
next_pos.set(probe_pos.x - probe_offset.x + cos(RADIANS(angle)) * radius,
probe_pos.y - probe_offset.y + sin(RADIANS(angle)) * radius);
#if DISABLED(DELTA)
LIMIT(X_current, X_MIN_POS, X_MAX_POS);
LIMIT(Y_current, Y_MIN_POS, Y_MAX_POS);
LIMIT(next_pos.x, X_MIN_POS, X_MAX_POS);
LIMIT(next_pos.y, Y_MIN_POS, Y_MAX_POS);
#else
// If we have gone out too far, we can do a simple fix and scale the numbers
// back in closer to the origin.
while (!position_is_reachable_by_probe(X_current, Y_current)) {
X_current *= 0.8;
Y_current *= 0.8;
while (!position_is_reachable_by_probe(next_pos)) {
next_pos *= 0.8;
if (verbose_level > 3)
SERIAL_ECHOLNPAIR("Moving inward: X", X_current, " Y", Y_current);
SERIAL_ECHOLNPAIR("Moving inward: X", next_pos.x, " Y", next_pos.y);
}
#endif
if (verbose_level > 3)
SERIAL_ECHOLNPAIR("Going to: X", X_current, " Y", Y_current, " Z", current_position[Z_AXIS]);
SERIAL_ECHOLNPAIR("Going to: X", next_pos.x, " Y", next_pos.y);
do_blocking_move_to_xy(X_current, Y_current);
do_blocking_move_to_xy(next_pos);
} // n_legs loop
} // n_legs
// Probe a single point
sample_set[n] = probe_at_point(X_probe_location, Y_probe_location, raise_after, 0);
sample_set[n] = probe_at_point(probe_pos, raise_after, 0);
// Break the loop if the probe fails
probing_good = !isnan(sample_set[n]);

22
Marlin/src/gcode/calibrate/M665.cpp
View File

@ -43,14 +43,14 @@
* Z = Gamma (Tower 3) angle trim
*/
void GcodeSuite::M665() {
if (parser.seen('H')) delta_height = parser.value_linear_units();
if (parser.seen('L')) delta_diagonal_rod = parser.value_linear_units();
if (parser.seen('R')) delta_radius = parser.value_linear_units();
if (parser.seen('S')) delta_segments_per_second = parser.value_float();
if (parser.seen('B')) delta_calibration_radius = parser.value_float();
if (parser.seen('X')) delta_tower_angle_trim[A_AXIS] = parser.value_float();
if (parser.seen('Y')) delta_tower_angle_trim[B_AXIS] = parser.value_float();
if (parser.seen('Z')) delta_tower_angle_trim[C_AXIS] = parser.value_float();
if (parser.seen('H')) delta_height = parser.value_linear_units();
if (parser.seen('L')) delta_diagonal_rod = parser.value_linear_units();
if (parser.seen('R')) delta_radius = parser.value_linear_units();
if (parser.seen('S')) delta_segments_per_second = parser.value_float();
if (parser.seen('B')) delta_calibration_radius = parser.value_float();
if (parser.seen('X')) delta_tower_angle_trim.a = parser.value_float();
if (parser.seen('Y')) delta_tower_angle_trim.b = parser.value_float();
if (parser.seen('Z')) delta_tower_angle_trim.c = parser.value_float();
recalc_delta_settings();
}
@ -76,13 +76,13 @@
#if HAS_SCARA_OFFSET
if (parser.seenval('Z')) scara_home_offset[Z_AXIS] = parser.value_linear_units();
if (parser.seenval('Z')) scara_home_offset.z = parser.value_linear_units();
const bool hasA = parser.seenval('A'), hasP = parser.seenval('P'), hasX = parser.seenval('X');
const uint8_t sumAPX = hasA + hasP + hasX;
if (sumAPX) {
if (sumAPX == 1)
scara_home_offset[A_AXIS] = parser.value_float();
scara_home_offset.a = parser.value_float();
else {
SERIAL_ERROR_MSG("Only one of A, P, or X is allowed.");
return;
@ -93,7 +93,7 @@
const uint8_t sumBTY = hasB + hasT + hasY;
if (sumBTY) {
if (sumBTY == 1)
scara_home_offset[B_AXIS] = parser.value_float();
scara_home_offset.b = parser.value_float();
else {
SERIAL_ERROR_MSG("Only one of B, T, or Y is allowed.");
return;