thebears/marlin_skr
1
0
Fork 0
Code Issues Pull Requests Projects Releases 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

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;
}