Merge pull request #3446 from thinkyhead/rc_fixup_M206_and_mesh

Fix home_offset handling and account for it in G29
This commit is contained in:
Scott Lahteine 2016-04-11 16:49:01 -07:00
commit c5a2ce4366
2 changed files with 45 additions and 35 deletions

View file

@ -2847,7 +2847,7 @@ inline void gcode_G28() {
}
if (probe_point == 0) {
// Set Z to a positive value before recording the first Z.
current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
current_position[Z_AXIS] = MESH_HOME_SEARCH_Z + home_offset[Z_AXIS];
sync_plan_position();
}
else {
@ -2856,7 +2856,7 @@ inline void gcode_G28() {
iy = (probe_point - 1) / (MESH_NUM_X_POINTS);
if (iy & 1) ix = (MESH_NUM_X_POINTS - 1) - ix; // zig-zag
mbl.set_z(ix, iy, current_position[Z_AXIS]);
current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
current_position[Z_AXIS] = MESH_HOME_SEARCH_Z + home_offset[Z_AXIS];
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[X_AXIS] / 60, active_extruder);
st_synchronize();
}
@ -2865,8 +2865,8 @@ inline void gcode_G28() {
ix = probe_point % (MESH_NUM_X_POINTS);
iy = probe_point / (MESH_NUM_X_POINTS);
if (iy & 1) ix = (MESH_NUM_X_POINTS - 1) - ix; // zig-zag
current_position[X_AXIS] = mbl.get_x(ix);
current_position[Y_AXIS] = mbl.get_y(iy);
current_position[X_AXIS] = mbl.get_x(ix) + home_offset[X_AXIS];
current_position[Y_AXIS] = mbl.get_y(iy) + home_offset[Y_AXIS];
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[X_AXIS] / 60, active_extruder);
st_synchronize();
probe_point++;
@ -3155,7 +3155,7 @@ inline void gcode_G28() {
// raise extruder
float measured_z,
z_before = probePointCounter ? Z_RAISE_BETWEEN_PROBINGS + current_position[Z_AXIS] : Z_RAISE_BEFORE_PROBING;
z_before = probePointCounter ? Z_RAISE_BETWEEN_PROBINGS + current_position[Z_AXIS] : Z_RAISE_BEFORE_PROBING + home_offset[Z_AXIS];
if (probePointCounter) {
#if ENABLED(DEBUG_LEVELING_FEATURE)
@ -3168,7 +3168,7 @@ inline void gcode_G28() {
else {
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) {
SERIAL_ECHOPAIR("z_before = (before) ", Z_RAISE_BEFORE_PROBING);
SERIAL_ECHOPAIR("z_before = (before) ", Z_RAISE_BEFORE_PROBING + home_offset[Z_AXIS]);
SERIAL_EOL;
}
#endif
@ -3329,9 +3329,18 @@ inline void gcode_G28() {
p1 = ProbeDeploy, p2 = ProbeStay, p3 = ProbeStow;
// Probe at 3 arbitrary points
float z_at_pt_1 = probe_pt(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, Z_RAISE_BEFORE_PROBING, p1, verbose_level),
z_at_pt_2 = probe_pt(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS, p2, verbose_level),
z_at_pt_3 = probe_pt(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS, p3, verbose_level);
float z_at_pt_1 = probe_pt( ABL_PROBE_PT_1_X + home_offset[X_AXIS],
ABL_PROBE_PT_1_Y + home_offset[Y_AXIS],
Z_RAISE_BEFORE_PROBING + home_offset[Z_AXIS],
p1, verbose_level),
z_at_pt_2 = probe_pt( ABL_PROBE_PT_2_X + home_offset[X_AXIS],
ABL_PROBE_PT_2_Y + home_offset[Y_AXIS],
current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS,
p2, verbose_level),
z_at_pt_3 = probe_pt( ABL_PROBE_PT_3_X + home_offset[X_AXIS],
ABL_PROBE_PT_3_Y + home_offset[Y_AXIS],
current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS,
p3, verbose_level);
clean_up_after_endstop_move();
if (!dryrun) set_bed_level_equation_3pts(z_at_pt_1, z_at_pt_2, z_at_pt_3);
@ -4986,19 +4995,26 @@ inline void gcode_M205() {
if (code_seen('E')) max_e_jerk = code_value();
}
static void set_home_offset(AxisEnum axis, float v) {
min_pos[axis] = base_min_pos(axis) + v;
max_pos[axis] = base_max_pos(axis) + v;
current_position[axis] += v - home_offset[axis];
home_offset[axis] = v;
}
/**
* M206: Set Additional Homing Offset (X Y Z). SCARA aliases T=X, P=Y
*/
inline void gcode_M206() {
for (int8_t i = X_AXIS; i <= Z_AXIS; i++) {
if (code_seen(axis_codes[i])) {
home_offset[i] = code_value();
}
}
for (int8_t i = X_AXIS; i <= Z_AXIS; i++)
if (code_seen(axis_codes[i]))
set_home_offset((AxisEnum)i, code_value());
#if ENABLED(SCARA)
if (code_seen('T')) home_offset[X_AXIS] = code_value(); // Theta
if (code_seen('P')) home_offset[Y_AXIS] = code_value(); // Psi
if (code_seen('T')) set_home_offset(X_AXIS, code_value()); // Theta
if (code_seen('P')) set_home_offset(Y_AXIS, code_value()); // Psi
#endif
sync_plan_position();
}
#if ENABLED(DELTA)
@ -5685,16 +5701,12 @@ inline void gcode_M410() { quickStop(); }
*/
inline void gcode_M428() {
bool err = false;
float new_offs[3], new_pos[3];
memcpy(new_pos, current_position, sizeof(new_pos));
memcpy(new_offs, home_offset, sizeof(new_offs));
for (int8_t i = X_AXIS; i <= Z_AXIS; i++) {
if (axis_homed[i]) {
float base = (new_pos[i] > (min_pos[i] + max_pos[i]) / 2) ? base_home_pos(i) : 0,
diff = new_pos[i] - base;
float base = (current_position[i] > (min_pos[i] + max_pos[i]) / 2) ? base_home_pos(i) : 0,
diff = current_position[i] - base;
if (diff > -20 && diff < 20) {
new_offs[i] -= diff;
new_pos[i] = base;
set_home_offset((AxisEnum)i, home_offset[i] - diff);
}
else {
SERIAL_ERROR_START;
@ -5710,8 +5722,6 @@ inline void gcode_M428() {
}
if (!err) {
memcpy(current_position, new_pos, sizeof(new_pos));
memcpy(home_offset, new_offs, sizeof(new_offs));
sync_plan_position();
LCD_ALERTMESSAGEPGM(MSG_HOME_OFFSETS_APPLIED);
#if HAS_BUZZER
@ -6980,10 +6990,10 @@ void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_
set_current_to_destination();
return;
}
int pix = mbl.select_x_index(current_position[X_AXIS]);
int piy = mbl.select_y_index(current_position[Y_AXIS]);
int ix = mbl.select_x_index(x);
int iy = mbl.select_y_index(y);
int pix = mbl.select_x_index(current_position[X_AXIS] - home_offset[X_AXIS]);
int piy = mbl.select_y_index(current_position[Y_AXIS] - home_offset[Y_AXIS]);
int ix = mbl.select_x_index(x - home_offset[X_AXIS]);
int iy = mbl.select_y_index(y - home_offset[Y_AXIS]);
pix = min(pix, MESH_NUM_X_POINTS - 2);
piy = min(piy, MESH_NUM_Y_POINTS - 2);
ix = min(ix, MESH_NUM_X_POINTS - 2);
@ -6996,7 +7006,7 @@ void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_
}
float nx, ny, nz, ne, normalized_dist;
if (ix > pix && TEST(x_splits, ix)) {
nx = mbl.get_x(ix);
nx = mbl.get_x(ix) + home_offset[X_AXIS];
normalized_dist = (nx - current_position[X_AXIS]) / (x - current_position[X_AXIS]);
ny = current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * normalized_dist;
nz = current_position[Z_AXIS] + (z - current_position[Z_AXIS]) * normalized_dist;
@ -7004,7 +7014,7 @@ void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_
CBI(x_splits, ix);
}
else if (ix < pix && TEST(x_splits, pix)) {
nx = mbl.get_x(pix);
nx = mbl.get_x(pix) + home_offset[X_AXIS];
normalized_dist = (nx - current_position[X_AXIS]) / (x - current_position[X_AXIS]);
ny = current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * normalized_dist;
nz = current_position[Z_AXIS] + (z - current_position[Z_AXIS]) * normalized_dist;
@ -7012,7 +7022,7 @@ void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_
CBI(x_splits, pix);
}
else if (iy > piy && TEST(y_splits, iy)) {
ny = mbl.get_y(iy);
ny = mbl.get_y(iy) + home_offset[Y_AXIS];
normalized_dist = (ny - current_position[Y_AXIS]) / (y - current_position[Y_AXIS]);
nx = current_position[X_AXIS] + (x - current_position[X_AXIS]) * normalized_dist;
nz = current_position[Z_AXIS] + (z - current_position[Z_AXIS]) * normalized_dist;
@ -7020,7 +7030,7 @@ void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_
CBI(y_splits, iy);
}
else if (iy < piy && TEST(y_splits, piy)) {
ny = mbl.get_y(piy);
ny = mbl.get_y(piy) + home_offset[Y_AXIS];
normalized_dist = (ny - current_position[Y_AXIS]) / (y - current_position[Y_AXIS]);
nx = current_position[X_AXIS] + (x - current_position[X_AXIS]) * normalized_dist;
nz = current_position[Z_AXIS] + (z - current_position[Z_AXIS]) * normalized_dist;

View file

@ -568,7 +568,7 @@ float junction_deviation = 0.1;
while (block_buffer_tail == next_buffer_head) idle();
#if ENABLED(MESH_BED_LEVELING)
if (mbl.active) z += mbl.get_z(x, y);
if (mbl.active) z += mbl.get_z(x - home_offset[X_AXIS], y - home_offset[Y_AXIS]);
#elif ENABLED(AUTO_BED_LEVELING_FEATURE)
apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
#endif
@ -1123,7 +1123,7 @@ float junction_deviation = 0.1;
#endif // AUTO_BED_LEVELING_FEATURE || MESH_BED_LEVELING
{
#if ENABLED(MESH_BED_LEVELING)
if (mbl.active) z += mbl.get_z(x, y);
if (mbl.active) z += mbl.get_z(x - home_offset[X_AXIS], y - home_offset[Y_AXIS]);
#elif ENABLED(AUTO_BED_LEVELING_FEATURE)
apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
#endif