MPCNC/Firmware
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

matrix names update (#7697)

Browse Source 
* matrix names update

* symplified names

* new angle normalization

* ABC

* axis

* least squares

* recalc_delta_settings

* endstop_adj

* 0p

* bug
This commit is contained in:
Luc Van Daele 2017-09-24 08:45:31 +02:00 committed by Scott Lahteine
parent ca13f4c3dd
commit 74d430cb97
5 changed files with 135 additions and 132 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Marlin/Marlin.h
View File

@ -302,9 +302,9 @@ extern float soft_endstop_min[XYZ], soft_endstop_max[XYZ];
delta_diagonal_rod, delta_diagonal_rod,
delta_calibration_radius, delta_calibration_radius,
delta_segments_per_second, delta_segments_per_second,
delta_tower_angle_trim[2], delta_tower_angle_trim[ABC],
delta_clip_start_height; delta_clip_start_height;
void recalc_delta_settings(float radius, float diagonal_rod); void recalc_delta_settings(float radius, float diagonal_rod, float tower_angle_trim[ABC]);
#elif IS_SCARA #elif IS_SCARA
void forward_kinematics_SCARA(const float &a, const float &b); void forward_kinematics_SCARA(const float &a, const float &b);
#endif #endif

236
Marlin/Marlin_main.cpp
View File

@ -596,7 +596,7 @@ static uint8_t target_extruder;
// Initialized by settings.load() // Initialized by settings.load()
float delta_radius, float delta_radius,
delta_tower_angle_trim[2], delta_tower_angle_trim[ABC],
delta_tower[ABC][2], delta_tower[ABC][2],
delta_diagonal_rod, delta_diagonal_rod,
delta_calibration_radius, delta_calibration_radius,
@ -3093,7 +3093,7 @@ static void homeaxis(const AxisEnum axis) {
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("endstop_adj:"); if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("endstop_adj:");
#endif #endif
do_homing_move(axis, endstop_adj[axis] - 0.1); do_homing_move(axis, endstop_adj[axis] - 0.1 * Z_HOME_DIR);
} }
#else #else
@ -5333,6 +5333,7 @@ void home_all_axes() { gcode_G28(true); }
* *
* Pn Number of probe points: * Pn Number of probe points:
* *
* P0 No probe. Normalize only.
* P1 Probe center and set height only. * P1 Probe center and set height only.
* P2 Probe center and towers. Set height, endstops, and delta radius. * P2 Probe center and towers. Set height, endstops, and delta radius.
* P3 Probe all positions: center, towers and opposite towers. Set all. * P3 Probe all positions: center, towers and opposite towers. Set all.
@ -5361,7 +5362,7 @@ void home_all_axes() { gcode_G28(true); }
SERIAL_PROTOCOL_F(f, 2); SERIAL_PROTOCOL_F(f, 2);
} }
inline void print_G33_settings(const bool end_stops, const bool tower_angles){ // TODO echo these to LCD ??? inline void print_G33_settings(const bool end_stops, const bool tower_angles){
SERIAL_PROTOCOLPAIR(".Height:", DELTA_HEIGHT + home_offset[Z_AXIS]); SERIAL_PROTOCOLPAIR(".Height:", DELTA_HEIGHT + home_offset[Z_AXIS]);
if (end_stops) { if (end_stops) {
print_signed_float(PSTR(" Ex"), endstop_adj[A_AXIS]); print_signed_float(PSTR(" Ex"), endstop_adj[A_AXIS]);
@ -5374,7 +5375,8 @@ void home_all_axes() { gcode_G28(true); }
SERIAL_PROTOCOLPGM(".Tower angle : "); SERIAL_PROTOCOLPGM(".Tower angle : ");
print_signed_float(PSTR("Tx"), delta_tower_angle_trim[A_AXIS]); 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("Ty"), delta_tower_angle_trim[B_AXIS]);
SERIAL_PROTOCOLLNPGM(" Tz:+0.00"); print_signed_float(PSTR("Tz"), delta_tower_angle_trim[C_AXIS]);
SERIAL_EOL();
} }
} }
@ -5396,8 +5398,8 @@ void home_all_axes() { gcode_G28(true); }
inline void gcode_G33() { inline void gcode_G33() {
const int8_t probe_points = parser.intval('P', DELTA_CALIBRATION_DEFAULT_POINTS); const int8_t probe_points = parser.intval('P', DELTA_CALIBRATION_DEFAULT_POINTS);
if (!WITHIN(probe_points, 1, 7)) { if (!WITHIN(probe_points, 0, 7)) {
SERIAL_PROTOCOLLNPGM("?(P)oints is implausible (1-7)."); SERIAL_PROTOCOLLNPGM("?(P)oints is implausible (0-7).");
return; return;
} }
@ -5421,11 +5423,12 @@ void home_all_axes() { gcode_G28(true); }
const bool towers_set = parser.boolval('T', true), const bool towers_set = parser.boolval('T', true),
stow_after_each = parser.boolval('E'), stow_after_each = parser.boolval('E'),
_0p_calibration = probe_points == 0,
_1p_calibration = probe_points == 1, _1p_calibration = probe_points == 1,
_4p_calibration = probe_points == 2, _4p_calibration = probe_points == 2,
_4p_towers_points = _4p_calibration && towers_set, _4p_towers_points = _4p_calibration && towers_set,
_4p_opposite_points = _4p_calibration && !towers_set, _4p_opposite_points = _4p_calibration && !towers_set,
_7p_calibration = probe_points >= 3, _7p_calibration = probe_points >= 3 || _0p_calibration,
_7p_half_circle = probe_points == 3, _7p_half_circle = probe_points == 3,
_7p_double_circle = probe_points == 5, _7p_double_circle = probe_points == 5,
_7p_triple_circle = probe_points == 6, _7p_triple_circle = probe_points == 6,
@ -5440,17 +5443,20 @@ void home_all_axes() { gcode_G28(true); }
zero_std_dev = (verbose_level ? 999.0 : 0.0), // 0.0 in dry-run mode : forced end zero_std_dev = (verbose_level ? 999.0 : 0.0), // 0.0 in dry-run mode : forced end
zero_std_dev_old = zero_std_dev, zero_std_dev_old = zero_std_dev,
zero_std_dev_min = zero_std_dev, zero_std_dev_min = zero_std_dev,
e_old[XYZ] = { e_old[ABC] = {
endstop_adj[A_AXIS], endstop_adj[A_AXIS],
endstop_adj[B_AXIS], endstop_adj[B_AXIS],
endstop_adj[C_AXIS] endstop_adj[C_AXIS]
}, },
dr_old = delta_radius, dr_old = delta_radius,
zh_old = home_offset[Z_AXIS], zh_old = home_offset[Z_AXIS],
alpha_old = delta_tower_angle_trim[A_AXIS], ta_old[ABC] = {
beta_old = delta_tower_angle_trim[B_AXIS]; delta_tower_angle_trim[A_AXIS],
delta_tower_angle_trim[B_AXIS],
delta_tower_angle_trim[C_AXIS]
};
if (!_1p_calibration) { // test if the outer radius is reachable if (!_1p_calibration && !_0p_calibration) { // test if the outer radius is reachable
const float circles = (_7p_quadruple_circle ? 1.5 : const float circles = (_7p_quadruple_circle ? 1.5 :
_7p_triple_circle ? 1.0 : _7p_triple_circle ? 1.0 :
_7p_double_circle ? 0.5 : 0), _7p_double_circle ? 0.5 : 0),
@ -5480,9 +5486,11 @@ void home_all_axes() { gcode_G28(true); }
setup_for_endstop_or_probe_move(); setup_for_endstop_or_probe_move();
endstops.enable(true); endstops.enable(true);
if (!home_delta()) if (!_0p_calibration) {
return; if (!home_delta())
endstops.not_homing(); return;
endstops.not_homing();
}
// print settings // print settings
@ -5495,9 +5503,11 @@ void home_all_axes() { gcode_G28(true); }
print_G33_settings(!_1p_calibration, _7p_calibration && towers_set); print_G33_settings(!_1p_calibration, _7p_calibration && towers_set);
#if DISABLED(PROBE_MANUALLY) #if DISABLED(PROBE_MANUALLY)
const float measured_z = probe_pt(dx, dy, stow_after_each, 1, false); // 1st probe to set height if (!_0p_calibration) {
if (isnan(measured_z)) return G33_CLEANUP(); const float measured_z = probe_pt(dx, dy, stow_after_each, 1, false); // 1st probe to set height
home_offset[Z_AXIS] -= measured_z; if (isnan(measured_z)) return G33_CLEANUP();
home_offset[Z_AXIS] -= measured_z;
}
#endif #endif
do { do {
@ -5505,58 +5515,60 @@ void home_all_axes() { gcode_G28(true); }
float z_at_pt[13] = { 0.0 }; float z_at_pt[13] = { 0.0 };
test_precision = zero_std_dev_old != 999.0 ? (zero_std_dev + zero_std_dev_old) / 2 : zero_std_dev; test_precision = zero_std_dev_old != 999.0 ? (zero_std_dev + zero_std_dev_old) / 2 : zero_std_dev;
if (_0p_calibration) test_precision = 0.00;
iterations++; iterations++;
// Probe the points // Probe the points
if (!_7p_half_circle && !_7p_triple_circle) { // probe the center if (!_0p_calibration){
#if ENABLED(PROBE_MANUALLY) if (!_7p_half_circle && !_7p_triple_circle) { // probe the center
z_at_pt[0] += lcd_probe_pt(0, 0);
#else
z_at_pt[0] += probe_pt(dx, dy, stow_after_each, 1, false);
if (isnan(z_at_pt[0])) return G33_CLEANUP();
#endif
}
if (_7p_calibration) { // probe extra center points
for (int8_t axis = _7p_multi_circle ? 11 : 9; axis > 0; axis -= _7p_multi_circle ? 2 : 4) {
const float a = RADIANS(180 + 30 * axis), r = delta_calibration_radius * 0.1;
#if ENABLED(PROBE_MANUALLY) #if ENABLED(PROBE_MANUALLY)
z_at_pt[0] += lcd_probe_pt(cos(a) * r, sin(a) * r); z_at_pt[0] += lcd_probe_pt(0, 0);
#else #else
z_at_pt[0] += probe_pt(cos(a) * r + dx, sin(a) * r + dy, stow_after_each, 1); z_at_pt[0] += probe_pt(dx, dy, stow_after_each, 1, false);
if (isnan(z_at_pt[0])) return G33_CLEANUP(); if (isnan(z_at_pt[0])) return G33_CLEANUP();
#endif #endif
} }
z_at_pt[0] /= float(_7p_double_circle ? 7 : probe_points); if (_7p_calibration) { // probe extra center points
} for (int8_t axis = _7p_multi_circle ? 11 : 9; axis > 0; axis -= _7p_multi_circle ? 2 : 4) {
if (!_1p_calibration) { // probe the radius const float a = RADIANS(180 + 30 * axis), r = delta_calibration_radius * 0.1;
bool zig_zag = true;
const uint8_t start = _4p_opposite_points ? 3 : 1,
step = _4p_calibration ? 4 : _7p_half_circle ? 2 : 1;
for (uint8_t axis = start; axis < 13; axis += step) {
const float zigadd = (zig_zag ? 0.5 : 0.0),
offset_circles = _7p_quadruple_circle ? zigadd + 1.0 :
_7p_triple_circle ? zigadd + 0.5 :
_7p_double_circle ? zigadd : 0;
for (float circles = -offset_circles ; circles <= offset_circles; circles++) {
const float a = RADIANS(180 + 30 * axis),
r = delta_calibration_radius * (1 + circles * (zig_zag ? 0.1 : -0.1));
#if ENABLED(PROBE_MANUALLY) #if ENABLED(PROBE_MANUALLY)
z_at_pt[axis] += lcd_probe_pt(cos(a) * r, sin(a) * r); z_at_pt[0] += lcd_probe_pt(cos(a) * r, sin(a) * r);
#else #else
z_at_pt[axis] += probe_pt(cos(a) * r + dx, sin(a) * r + dy, stow_after_each, 1); z_at_pt[0] += probe_pt(cos(a) * r + dx, sin(a) * r + dy, stow_after_each, 1);
if (isnan(z_at_pt[axis])) return G33_CLEANUP(); if (isnan(z_at_pt[0])) return G33_CLEANUP();
#endif #endif
} }
zig_zag = !zig_zag; z_at_pt[0] /= float(_7p_double_circle ? 7 : probe_points);
z_at_pt[axis] /= (2 * offset_circles + 1);
} }
} if (!_1p_calibration) { // probe the radius
if (_7p_intermed_points) // average intermediates to tower and opposites bool zig_zag = true;
for (uint8_t axis = 1; axis < 13; axis += 2) const uint8_t start = _4p_opposite_points ? 3 : 1,
z_at_pt[axis] = (z_at_pt[axis] + (z_at_pt[axis + 1] + z_at_pt[(axis + 10) % 12 + 1]) / 2.0) / 2.0; step = _4p_calibration ? 4 : _7p_half_circle ? 2 : 1;
for (uint8_t axis = start; axis < 13; axis += step) {
const float zigadd = (zig_zag ? 0.5 : 0.0),
offset_circles = _7p_quadruple_circle ? zigadd + 1.0 :
_7p_triple_circle ? zigadd + 0.5 :
_7p_double_circle ? zigadd : 0;
for (float circles = -offset_circles ; circles <= offset_circles; circles++) {
const float a = RADIANS(180 + 30 * axis),
r = delta_calibration_radius * (1 + circles * (zig_zag ? 0.1 : -0.1));
#if ENABLED(PROBE_MANUALLY)
z_at_pt[axis] += lcd_probe_pt(cos(a) * r, sin(a) * r);
#else
z_at_pt[axis] += probe_pt(cos(a) * r + dx, sin(a) * r + dy, stow_after_each, 1);
if (isnan(z_at_pt[axis])) return G33_CLEANUP();
#endif
}
zig_zag = !zig_zag;
z_at_pt[axis] /= (2 * offset_circles + 1);
}
}
if (_7p_intermed_points) // average intermediates to tower and opposites
for (uint8_t axis = 1; axis < 13; axis += 2)
z_at_pt[axis] = (z_at_pt[axis] + (z_at_pt[axis + 1] + z_at_pt[(axis + 10) % 12 + 1]) / 2.0) / 2.0;
}
float S1 = z_at_pt[0], float S1 = z_at_pt[0],
S2 = sq(z_at_pt[0]); S2 = sq(z_at_pt[0]);
int16_t N = 1; int16_t N = 1;
@ -5576,27 +5588,22 @@ void home_all_axes() { gcode_G28(true); }
COPY(e_old, endstop_adj); COPY(e_old, endstop_adj);
dr_old = delta_radius; dr_old = delta_radius;
zh_old = home_offset[Z_AXIS]; zh_old = home_offset[Z_AXIS];
alpha_old = delta_tower_angle_trim[A_AXIS]; COPY(ta_old, delta_tower_angle_trim);
beta_old = delta_tower_angle_trim[B_AXIS];
} }
float e_delta[XYZ] = { 0.0 }, r_delta = 0.0, t_alpha = 0.0, t_beta = 0.0; float e_delta[ABC] = { 0.0 }, r_delta = 0.0, t_delta[ABC] = { 0.0 };
const float r_diff = delta_radius - delta_calibration_radius, float r_diff = delta_radius - delta_calibration_radius,
h_factor = 1.00 + r_diff * 0.001, //1.02 for r_diff = 20mm h_factor = 1.00 + r_diff * 0.001, //1.02 for r_diff = 20mm
r_factor = -(1.75 + 0.005 * r_diff + 0.001 * sq(r_diff)), //2.25 for r_diff = 20mm r_factor = -(1.75 + 0.005 * r_diff + 0.001 * sq(r_diff)), //2.25 for r_diff = 20mm
a_factor = 100.0 / delta_calibration_radius; //1.25 for cal_rd = 80mm a_factor = 66.66 / delta_calibration_radius; //0.83 for cal_rd = 80mm
#define ZP(N,I) ((N) * z_at_pt[I]) #define ZP(N,I) ((N) * z_at_pt[I])
#define Z1000(I) ZP(1.00, I) #define Z6(I) ZP(6, I)
#define Z1050(I) ZP(h_factor, I) #define Z4(I) ZP(4, I)
#define Z0700(I) ZP(h_factor * 2.0 / 3.00, I) #define Z2(I) ZP(2, I)
#define Z0350(I) ZP(h_factor / 3.00, I) #define Z1(I) ZP(1, I)
#define Z0175(I) ZP(h_factor / 6.00, I) h_factor /= 6.00;
#define Z2250(I) ZP(r_factor, I) r_factor /= 6.00;
#define Z0750(I) ZP(r_factor / 3.00, I)
#define Z0375(I) ZP(r_factor / 6.00, I)
#define Z0444(I) ZP(a_factor * 4.0 / 9.0, I)
#define Z0888(I) ZP(a_factor * 8.0 / 9.0, I)
#if ENABLED(PROBE_MANUALLY) #if ENABLED(PROBE_MANUALLY)
test_precision = 0.00; // forced end test_precision = 0.00; // forced end
@ -5605,58 +5612,61 @@ void home_all_axes() { gcode_G28(true); }
switch (probe_points) { switch (probe_points) {
case 1: case 1:
test_precision = 0.00; // forced end test_precision = 0.00; // forced end
LOOP_XYZ(i) e_delta[i] = Z1000(0); LOOP_XYZ(axis) e_delta[axis] = Z1(0);
break; break;
case 2: case 2:
if (towers_set) { if (towers_set) {
e_delta[X_AXIS] = Z1050(0) + Z0700(1) - Z0350(5) - Z0350(9); e_delta[A_AXIS] = (Z6(0) + Z4(1) - Z2(5) - Z2(9)) * h_factor;
e_delta[Y_AXIS] = Z1050(0) - Z0350(1) + Z0700(5) - Z0350(9); e_delta[B_AXIS] = (Z6(0) - Z2(1) + Z4(5) - Z2(9)) * h_factor;
e_delta[Z_AXIS] = Z1050(0) - Z0350(1) - Z0350(5) + Z0700(9); e_delta[C_AXIS] = (Z6(0) - Z2(1) - Z2(5) + Z4(9)) * h_factor;
r_delta = Z2250(0) - Z0750(1) - Z0750(5) - Z0750(9); r_delta = (Z6(0) - Z2(1) - Z2(5) - Z2(9)) * r_factor;
} }
else { else {
e_delta[X_AXIS] = Z1050(0) - Z0700(7) + Z0350(11) + Z0350(3); e_delta[A_AXIS] = (Z6(0) - Z4(7) + Z2(11) + Z2(3)) * h_factor;
e_delta[Y_AXIS] = Z1050(0) + Z0350(7) - Z0700(11) + Z0350(3); e_delta[B_AXIS] = (Z6(0) + Z2(7) - Z4(11) + Z2(3)) * h_factor;
e_delta[Z_AXIS] = Z1050(0) + Z0350(7) + Z0350(11) - Z0700(3); e_delta[C_AXIS] = (Z6(0) + Z2(7) + Z2(11) - Z4(3)) * h_factor;
r_delta = Z2250(0) - Z0750(7) - Z0750(11) - Z0750(3); r_delta = (Z6(0) - Z2(7) - Z2(11) - Z2(3)) * r_factor;
} }
break; break;
default: default:
e_delta[X_AXIS] = Z1050(0) + Z0350(1) - Z0175(5) - Z0175(9) - Z0350(7) + Z0175(11) + Z0175(3); e_delta[A_AXIS] = (Z6(0) + Z2(1) - Z1(5) - Z1(9) - Z2(7) + Z1(11) + Z1(3)) * h_factor;
e_delta[Y_AXIS] = Z1050(0) - Z0175(1) + Z0350(5) - Z0175(9) + Z0175(7) - Z0350(11) + Z0175(3); e_delta[B_AXIS] = (Z6(0) - Z1(1) + Z2(5) - Z1(9) + Z1(7) - Z2(11) + Z1(3)) * h_factor;
e_delta[Z_AXIS] = Z1050(0) - Z0175(1) - Z0175(5) + Z0350(9) + Z0175(7) + Z0175(11) - Z0350(3); e_delta[C_AXIS] = (Z6(0) - Z1(1) - Z1(5) + Z2(9) + Z1(7) + Z1(11) - Z2(3)) * h_factor;
r_delta = Z2250(0) - Z0375(1) - Z0375(5) - Z0375(9) - Z0375(7) - Z0375(11) - Z0375(3); r_delta = (Z6(0) - Z1(1) - Z1(5) - Z1(9) - Z1(7) - Z1(11) - Z1(3)) * r_factor;
if (towers_set) { if (towers_set) {
t_alpha = Z0444(1) - Z0888(5) + Z0444(9) + Z0444(7) - Z0888(11) + Z0444(3); t_delta[A_AXIS] = ( - Z2(5) + Z1(9) - Z2(11) + Z1(3)) * a_factor;
t_beta = Z0888(1) - Z0444(5) - Z0444(9) + Z0888(7) - Z0444(11) - Z0444(3); t_delta[B_AXIS] = ( Z2(1) - Z1(9) + Z2(7) - Z1(3)) * a_factor;
t_delta[C_AXIS] = (-Z2(1) + Z1(5) - Z2(7) + Z1(11) ) * a_factor;
} }
break; break;
} }
LOOP_XYZ(axis) endstop_adj[axis] += e_delta[axis]; LOOP_XYZ(axis) endstop_adj[axis] += e_delta[axis];
delta_radius += r_delta; delta_radius += r_delta;
delta_tower_angle_trim[A_AXIS] += t_alpha; LOOP_XYZ(axis) delta_tower_angle_trim[axis] += t_delta[axis];
delta_tower_angle_trim[B_AXIS] += t_beta;
// adjust delta_height and endstops by the max amount
const float z_temp = MAX3(endstop_adj[A_AXIS], endstop_adj[B_AXIS], endstop_adj[C_AXIS]);
home_offset[Z_AXIS] -= z_temp;
LOOP_XYZ(i) endstop_adj[i] -= z_temp;
recalc_delta_settings(delta_radius, delta_diagonal_rod);
} }
else if (zero_std_dev >= test_precision) { // step one back else if (zero_std_dev >= test_precision) { // step one back
COPY(endstop_adj, e_old); COPY(endstop_adj, e_old);
delta_radius = dr_old; delta_radius = dr_old;
home_offset[Z_AXIS] = zh_old; home_offset[Z_AXIS] = zh_old;
delta_tower_angle_trim[A_AXIS] = alpha_old; COPY(delta_tower_angle_trim, ta_old);
delta_tower_angle_trim[B_AXIS] = beta_old;
recalc_delta_settings(delta_radius, delta_diagonal_rod);
} }
if (verbose_level != 0) { // !dry run
// normalise angles to least squares
float a_sum = 0.0;
LOOP_XYZ(axis) a_sum += delta_tower_angle_trim[axis];
LOOP_XYZ(axis) delta_tower_angle_trim[axis] -= a_sum / 3.0;
// adjust delta_height and endstops by the max amount
const float z_temp = MAX3(endstop_adj[A_AXIS], endstop_adj[B_AXIS], endstop_adj[C_AXIS]);
home_offset[Z_AXIS] -= z_temp;
LOOP_XYZ(axis) endstop_adj[axis] -= z_temp;
}
recalc_delta_settings(delta_radius, delta_diagonal_rod, delta_tower_angle_trim);
NOMORE(zero_std_dev_min, zero_std_dev); NOMORE(zero_std_dev_min, zero_std_dev);
// print report // print report
@ -8538,11 +8548,8 @@ inline void gcode_M205() {
if (parser.seen('B')) delta_calibration_radius = 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('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('Y')) delta_tower_angle_trim[B_AXIS] = parser.value_float();
if (parser.seen('Z')) { // rotate all 3 axis for Z = 0 if (parser.seen('Z')) delta_tower_angle_trim[C_AXIS] = parser.value_float();
delta_tower_angle_trim[A_AXIS] -= parser.value_float(); recalc_delta_settings(delta_radius, delta_diagonal_rod, delta_tower_angle_trim);
delta_tower_angle_trim[B_AXIS] -= parser.value_float();
}
recalc_delta_settings(delta_radius, delta_diagonal_rod);
} }
/** /**
* M666: Set delta endstop adjustment * M666: Set delta endstop adjustment
@ -8555,7 +8562,8 @@ inline void gcode_M205() {
#endif #endif
LOOP_XYZ(i) { LOOP_XYZ(i) {
if (parser.seen(axis_codes[i])) { if (parser.seen(axis_codes[i])) {
endstop_adj[i] = parser.value_linear_units(); if (parser.value_linear_units() * Z_HOME_DIR <= 0)
endstop_adj[i] = parser.value_linear_units();
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) { if (DEBUGGING(LEVELING)) {
SERIAL_ECHOPAIR("endstop_adj[", axis_codes[i]); SERIAL_ECHOPAIR("endstop_adj[", axis_codes[i]);
@ -8569,10 +8577,6 @@ inline void gcode_M205() {
SERIAL_ECHOLNPGM("<<< gcode_M666"); SERIAL_ECHOLNPGM("<<< gcode_M666");
} }
#endif #endif
// normalize endstops so all are <=0; set the residue to delta height
const float z_temp = MAX3(endstop_adj[A_AXIS], endstop_adj[B_AXIS], endstop_adj[C_AXIS]);
home_offset[Z_AXIS] -= z_temp;
LOOP_XYZ(i) endstop_adj[i] -= z_temp;
} }
#elif IS_SCARA #elif IS_SCARA
@ -11830,15 +11834,15 @@ void ok_to_send() {
* Recalculate factors used for delta kinematics whenever * Recalculate factors used for delta kinematics whenever
* settings have been changed (e.g., by M665). * settings have been changed (e.g., by M665).
*/ */
void recalc_delta_settings(float radius, float diagonal_rod) { void recalc_delta_settings(float radius, float diagonal_rod, float tower_angle_trim[ABC]) {
const float trt[ABC] = DELTA_RADIUS_TRIM_TOWER, const float trt[ABC] = DELTA_RADIUS_TRIM_TOWER,
drt[ABC] = DELTA_DIAGONAL_ROD_TRIM_TOWER; drt[ABC] = DELTA_DIAGONAL_ROD_TRIM_TOWER;
delta_tower[A_AXIS][X_AXIS] = cos(RADIANS(210 + delta_tower_angle_trim[A_AXIS])) * (radius + trt[A_AXIS]); // front left tower delta_tower[A_AXIS][X_AXIS] = cos(RADIANS(210 + tower_angle_trim[A_AXIS])) * (radius + trt[A_AXIS]); // front left tower
delta_tower[A_AXIS][Y_AXIS] = sin(RADIANS(210 + delta_tower_angle_trim[A_AXIS])) * (radius + trt[A_AXIS]); delta_tower[A_AXIS][Y_AXIS] = sin(RADIANS(210 + tower_angle_trim[A_AXIS])) * (radius + trt[A_AXIS]);
delta_tower[B_AXIS][X_AXIS] = cos(RADIANS(330 + delta_tower_angle_trim[B_AXIS])) * (radius + trt[B_AXIS]); // front right tower delta_tower[B_AXIS][X_AXIS] = cos(RADIANS(330 + tower_angle_trim[B_AXIS])) * (radius + trt[B_AXIS]); // front right tower
delta_tower[B_AXIS][Y_AXIS] = sin(RADIANS(330 + delta_tower_angle_trim[B_AXIS])) * (radius + trt[B_AXIS]); delta_tower[B_AXIS][Y_AXIS] = sin(RADIANS(330 + tower_angle_trim[B_AXIS])) * (radius + trt[B_AXIS]);
delta_tower[C_AXIS][X_AXIS] = 0.0; // back middle tower delta_tower[C_AXIS][X_AXIS] = cos(RADIANS( 90 + tower_angle_trim[C_AXIS])) * (radius + trt[C_AXIS]); // back middle tower
delta_tower[C_AXIS][Y_AXIS] = (radius + trt[C_AXIS]); delta_tower[C_AXIS][Y_AXIS] = sin(RADIANS( 90 + tower_angle_trim[C_AXIS])) * (radius + trt[C_AXIS]);
delta_diagonal_rod_2_tower[A_AXIS] = sq(diagonal_rod + drt[A_AXIS]); delta_diagonal_rod_2_tower[A_AXIS] = sq(diagonal_rod + drt[A_AXIS]);
delta_diagonal_rod_2_tower[B_AXIS] = sq(diagonal_rod + drt[B_AXIS]); delta_diagonal_rod_2_tower[B_AXIS] = sq(diagonal_rod + drt[B_AXIS]);
delta_diagonal_rod_2_tower[C_AXIS] = sq(diagonal_rod + drt[C_AXIS]); delta_diagonal_rod_2_tower[C_AXIS] = sq(diagonal_rod + drt[C_AXIS]);

23
Marlin/configuration_store.cpp
View File

@ -36,13 +36,13 @@
* *
*/ */
#define EEPROM_VERSION "V40" #define EEPROM_VERSION "V41"
// Change EEPROM version if these are changed: // Change EEPROM version if these are changed:
#define EEPROM_OFFSET 100 #define EEPROM_OFFSET 100
/** /**
* V39 EEPROM Layout: * V41 EEPROM Layout:
* *
* 100 Version (char x4) * 100 Version (char x4)
* 104 EEPROM CRC16 (uint16_t) * 104 EEPROM CRC16 (uint16_t)
@ -100,7 +100,7 @@
* 372 M665 B delta_calibration_radius (float) * 372 M665 B delta_calibration_radius (float)
* 376 M665 X delta_tower_angle_trim[A] (float) * 376 M665 X delta_tower_angle_trim[A] (float)
* 380 M665 Y delta_tower_angle_trim[B] (float) * 380 M665 Y delta_tower_angle_trim[B] (float)
* --- M665 Z delta_tower_angle_trim[C] (float) is always 0.0 * 384 M665 Z delta_tower_angle_trim[C] (float)
* *
* Z_DUAL_ENDSTOPS: 48 bytes * Z_DUAL_ENDSTOPS: 48 bytes
* 348 M666 Z z_endstop_adj (float) * 348 M666 Z z_endstop_adj (float)
@ -215,7 +215,7 @@ void MarlinSettings::postprocess() {
// Make sure delta kinematics are updated before refreshing the // Make sure delta kinematics are updated before refreshing the
// planner position so the stepper counts will be set correctly. // planner position so the stepper counts will be set correctly.
#if ENABLED(DELTA) #if ENABLED(DELTA)
recalc_delta_settings(delta_radius, delta_diagonal_rod); recalc_delta_settings(delta_radius, delta_diagonal_rod, delta_tower_angle_trim);
#endif #endif
// Refresh steps_to_mm with the reciprocal of axis_steps_per_mm // Refresh steps_to_mm with the reciprocal of axis_steps_per_mm
@ -448,16 +448,16 @@ void MarlinSettings::postprocess() {
EEPROM_WRITE(storage_slot); EEPROM_WRITE(storage_slot);
#endif // AUTO_BED_LEVELING_UBL #endif // AUTO_BED_LEVELING_UBL
// 9 floats for DELTA / Z_DUAL_ENDSTOPS // 10 floats for DELTA / Z_DUAL_ENDSTOPS
#if ENABLED(DELTA) #if ENABLED(DELTA)
EEPROM_WRITE(endstop_adj); // 3 floats EEPROM_WRITE(endstop_adj); // 3 floats
EEPROM_WRITE(delta_radius); // 1 float EEPROM_WRITE(delta_radius); // 1 float
EEPROM_WRITE(delta_diagonal_rod); // 1 float EEPROM_WRITE(delta_diagonal_rod); // 1 float
EEPROM_WRITE(delta_segments_per_second); // 1 float EEPROM_WRITE(delta_segments_per_second); // 1 float
EEPROM_WRITE(delta_calibration_radius); // 1 float EEPROM_WRITE(delta_calibration_radius); // 1 float
EEPROM_WRITE(delta_tower_angle_trim); // 2 floats EEPROM_WRITE(delta_tower_angle_trim); // 3 floats
dummy = 0.0f; dummy = 0.0f;
for (uint8_t q = 3; q--;) EEPROM_WRITE(dummy); for (uint8_t q = 2; q--;) EEPROM_WRITE(dummy);
#elif ENABLED(Z_DUAL_ENDSTOPS) #elif ENABLED(Z_DUAL_ENDSTOPS)
EEPROM_WRITE(z_endstop_adj); // 1 float EEPROM_WRITE(z_endstop_adj); // 1 float
dummy = 0.0f; dummy = 0.0f;
@ -844,9 +844,9 @@ void MarlinSettings::postprocess() {
EEPROM_READ(delta_diagonal_rod); // 1 float EEPROM_READ(delta_diagonal_rod); // 1 float
EEPROM_READ(delta_segments_per_second); // 1 float EEPROM_READ(delta_segments_per_second); // 1 float
EEPROM_READ(delta_calibration_radius); // 1 float EEPROM_READ(delta_calibration_radius); // 1 float
EEPROM_READ(delta_tower_angle_trim); // 2 floats EEPROM_READ(delta_tower_angle_trim); // 3 floats
dummy = 0.0f; dummy = 0.0f;
for (uint8_t q=3; q--;) EEPROM_READ(dummy); for (uint8_t q=2; q--;) EEPROM_READ(dummy);
#elif ENABLED(Z_DUAL_ENDSTOPS) #elif ENABLED(Z_DUAL_ENDSTOPS)
EEPROM_READ(z_endstop_adj); EEPROM_READ(z_endstop_adj);
dummy = 0.0f; dummy = 0.0f;
@ -1233,8 +1233,7 @@ void MarlinSettings::reset() {
delta_diagonal_rod = DELTA_DIAGONAL_ROD; delta_diagonal_rod = DELTA_DIAGONAL_ROD;
delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND; delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND;
delta_calibration_radius = DELTA_CALIBRATION_RADIUS; delta_calibration_radius = DELTA_CALIBRATION_RADIUS;
delta_tower_angle_trim[A_AXIS] = dta[A_AXIS] - dta[C_AXIS]; COPY(delta_tower_angle_trim, dta);
delta_tower_angle_trim[B_AXIS] = dta[B_AXIS] - dta[C_AXIS];
home_offset[Z_AXIS] = 0; home_offset[Z_AXIS] = 0;
#elif ENABLED(Z_DUAL_ENDSTOPS) #elif ENABLED(Z_DUAL_ENDSTOPS)
@ -1657,7 +1656,7 @@ void MarlinSettings::reset() {
SERIAL_ECHOPAIR(" B", LINEAR_UNIT(delta_calibration_radius)); SERIAL_ECHOPAIR(" B", LINEAR_UNIT(delta_calibration_radius));
SERIAL_ECHOPAIR(" X", LINEAR_UNIT(delta_tower_angle_trim[A_AXIS])); SERIAL_ECHOPAIR(" X", LINEAR_UNIT(delta_tower_angle_trim[A_AXIS]));
SERIAL_ECHOPAIR(" Y", LINEAR_UNIT(delta_tower_angle_trim[B_AXIS])); SERIAL_ECHOPAIR(" Y", LINEAR_UNIT(delta_tower_angle_trim[B_AXIS]));
SERIAL_ECHOPAIR(" Z", 0.00); SERIAL_ECHOPAIR(" Z", LINEAR_UNIT(delta_tower_angle_trim[C_AXIS]));
SERIAL_EOL(); SERIAL_EOL();
#elif ENABLED(Z_DUAL_ENDSTOPS) #elif ENABLED(Z_DUAL_ENDSTOPS)
if (!forReplay) { if (!forReplay) {

2
Marlin/ubl_motion.cpp
View File

@ -44,7 +44,7 @@
endstop_adj[ABC]; endstop_adj[ABC];
extern float delta_radius, extern float delta_radius,
delta_tower_angle_trim[2], delta_tower_angle_trim[ABC],
delta_tower[ABC][2], delta_tower[ABC][2],
delta_diagonal_rod, delta_diagonal_rod,
delta_calibration_radius, delta_calibration_radius,

2
Marlin/ultralcd.cpp
View File

@ -2703,7 +2703,7 @@ void kill_screen(const char* lcd_msg) {
MENU_ITEM_EDIT(float52, MSG_DELTA_RADIUS, &delta_radius, DELTA_RADIUS - 5.0, DELTA_RADIUS + 5.0); MENU_ITEM_EDIT(float52, MSG_DELTA_RADIUS, &delta_radius, DELTA_RADIUS - 5.0, DELTA_RADIUS + 5.0);
MENU_ITEM_EDIT(float43, "Tx", &delta_tower_angle_trim[A_AXIS], -5.0, 5.0); MENU_ITEM_EDIT(float43, "Tx", &delta_tower_angle_trim[A_AXIS], -5.0, 5.0);
MENU_ITEM_EDIT(float43, "Ty", &delta_tower_angle_trim[B_AXIS], -5.0, 5.0); MENU_ITEM_EDIT(float43, "Ty", &delta_tower_angle_trim[B_AXIS], -5.0, 5.0);
MENU_ITEM_EDIT(float43, "Tz", &Tz, -5.0, 5.0); MENU_ITEM_EDIT(float43, "Tz", &delta_tower_angle_trim[C_AXIS], -5.0, 5.0);
END_MENU(); END_MENU();
} }