3DPrinting/Prusa-Firmware
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Skew calibration: Yet another improvement of the search algorithm

Browse Source 
of the front row induction sensor targets.
Fixes of some movements behind the end stops.
This commit is contained in:
bubnikv 2016-07-06 09:34:42 +02:00
parent 58b2aa9fb8
commit 1394e7efc6
8 changed files with 290 additions and 168 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
Firmware/Marlin_main.cpp
View File

@ -2371,6 +2371,7 @@ void process_commands()
current_position[X_AXIS] = pgm_read_float(bed_ref_points+2*mesh_point);
current_position[Y_AXIS] = pgm_read_float(bed_ref_points+2*mesh_point+1);
world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
// mbl.get_meas_xy(ix, iy, current_position[X_AXIS], current_position[Y_AXIS], false);
enable_endstops(false);
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], XY_AXIS_FEEDRATE, active_extruder);
@ -2802,7 +2803,7 @@ void process_commands()
current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS],current_position[Z_AXIS] , current_position[E_AXIS], homing_feedrate[Z_AXIS]/40, active_extruder);
st_synchronize();
if (result != BED_SKEW_OFFSET_DETECTION_FAILED) {
if (result >= 0) {
// Second half: The fine adjustment.
// Let the planner use the uncorrected coordinates.
mbl.reset();
@ -2817,7 +2818,12 @@ void process_commands()
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS],current_position[Z_AXIS] , current_position[E_AXIS], homing_feedrate[Z_AXIS]/40, active_extruder);
st_synchronize();
}
lcd_bed_calibration_show_result(result);
if (result >= BED_SKEW_OFFSET_DETECTION_FAILED) {
lcd_bed_calibration_show_result(result);
} else {
lcd_bed_calibration_show_result(BED_SKEW_OFFSET_DETECTION_FAILED);
lcd_bed_calibration_show_result(BedSkewOffsetDetectionResultType(- int8_t(result)));
}
/*
if (result != BED_SKEW_OFFSET_DETECTION_FAILED) {
// Mesh bed leveling.
@ -4716,53 +4722,7 @@ void get_arc_coordinates()
void clamp_to_software_endstops(float target[3])
{
if (world2machine_correction_mode == WORLD2MACHINE_CORRECTION_NONE || world2machine_correction_mode == WORLD2MACHINE_CORRECTION_SHIFT) {
// No correction or only a shift correction.
// Save computational cycles by not performing the skew correction.
if (world2machine_correction_mode == WORLD2MACHINE_CORRECTION_SHIFT) {
target[0] += world2machine_shift[0];
target[1] += world2machine_shift[1];
}
if (min_software_endstops) {
if (target[X_AXIS] < min_pos[X_AXIS]) target[X_AXIS] = min_pos[X_AXIS];
if (target[Y_AXIS] < min_pos[Y_AXIS]) target[Y_AXIS] = min_pos[Y_AXIS];
}
if (max_software_endstops) {
if (target[X_AXIS] > max_pos[X_AXIS]) target[X_AXIS] = max_pos[X_AXIS];
if (target[Y_AXIS] > max_pos[Y_AXIS]) target[Y_AXIS] = max_pos[Y_AXIS];
}
if (world2machine_correction_mode == WORLD2MACHINE_CORRECTION_SHIFT) {
target[0] -= world2machine_shift[0];
target[1] -= world2machine_shift[1];
}
} else {
// Skew correction is in action.
float x, y;
world2machine(target[0], target[1], x, y);
bool clamped = false;
if (min_software_endstops) {
if (x < min_pos[X_AXIS]) {
x = min_pos[X_AXIS];
clamped = true;
}
if (y < min_pos[Y_AXIS]) {
y = min_pos[Y_AXIS];
clamped = true;
}
}
if (max_software_endstops) {
if (x > max_pos[X_AXIS]) {
x = max_pos[X_AXIS];
clamped = true;
}
if (y > max_pos[Y_AXIS]) {
y = max_pos[Y_AXIS];
clamped = true;
}
}
if (clamped)
machine2world(x, y, target[X_AXIS], target[Y_AXIS]);
}
world2machine_clamp(target[0], target[1]);
// Clamp the Z coordinate.
if (min_software_endstops) {

29
Firmware/language_all.cpp
View File

@ -291,11 +291,24 @@ const char * const MSG_BED_SKEW_OFFSET_DETECTION_FAILED_LANG_TABLE[LANG_NUM] PRO
MSG_BED_SKEW_OFFSET_DETECTION_FAILED_PL
};
const char MSG_BED_SKEW_OFFSET_DETECTION_FRONT_LEFT_FAR_EN[] PROGMEM = "X/Y calibration bad. Left front corner not reachable. Fix the printer.";
const char MSG_BED_SKEW_OFFSET_DETECTION_FRONT_BOTH_FAR_EN[] PROGMEM = "X/Y calibration bad. Front calibration points not reachable. Fix the printer.";
const char MSG_BED_SKEW_OFFSET_DETECTION_FRONT_BOTH_FAR_CZ[] PROGMEM = "Kalibrace selhala. Predni kalibracni body moc vpredu. Srovnejte tiskarnu.";
const char MSG_BED_SKEW_OFFSET_DETECTION_FRONT_BOTH_FAR_IT[] PROGMEM = "X/Y calibration bad. Front calibration points not reachable. Fix the printer.";
const char MSG_BED_SKEW_OFFSET_DETECTION_FRONT_BOTH_FAR_ES[] PROGMEM = "X/Y calibration bad. Front calibration points not reachable. Fix the printer.";
const char MSG_BED_SKEW_OFFSET_DETECTION_FRONT_BOTH_FAR_PL[] PROGMEM = "X/Y calibration bad. Front calibration points not reachable. Fix the printer.";
const char * const MSG_BED_SKEW_OFFSET_DETECTION_FRONT_BOTH_FAR_LANG_TABLE[LANG_NUM] PROGMEM = {
MSG_BED_SKEW_OFFSET_DETECTION_FRONT_BOTH_FAR_EN,
MSG_BED_SKEW_OFFSET_DETECTION_FRONT_BOTH_FAR_CZ,
MSG_BED_SKEW_OFFSET_DETECTION_FRONT_BOTH_FAR_IT,
MSG_BED_SKEW_OFFSET_DETECTION_FRONT_BOTH_FAR_ES,
MSG_BED_SKEW_OFFSET_DETECTION_FRONT_BOTH_FAR_PL
};
const char MSG_BED_SKEW_OFFSET_DETECTION_FRONT_LEFT_FAR_EN[] PROGMEM = "X/Y calibration bad. Left front calibration point not reachable. Fix the printer.";
const char MSG_BED_SKEW_OFFSET_DETECTION_FRONT_LEFT_FAR_CZ[] PROGMEM = "Kalibrace selhala. Levy predni bod moc vpredu. Srovnejte tiskarnu.";
const char MSG_BED_SKEW_OFFSET_DETECTION_FRONT_LEFT_FAR_IT[] PROGMEM = "X/Y calibration bad. Left front corner not reachable. Fix the printer.";
const char MSG_BED_SKEW_OFFSET_DETECTION_FRONT_LEFT_FAR_ES[] PROGMEM = "X/Y calibration bad. Left front corner not reachable. Fix the printer.";
const char MSG_BED_SKEW_OFFSET_DETECTION_FRONT_LEFT_FAR_PL[] PROGMEM = "X/Y calibration bad. Left front corner not reachable. Fix the printer.";
const char MSG_BED_SKEW_OFFSET_DETECTION_FRONT_LEFT_FAR_IT[] PROGMEM = "X/Y calibration bad. Left front calibration point not reachable. Fix the printer.";
const char MSG_BED_SKEW_OFFSET_DETECTION_FRONT_LEFT_FAR_ES[] PROGMEM = "X/Y calibration bad. Left front calibration point not reachable. Fix the printer.";
const char MSG_BED_SKEW_OFFSET_DETECTION_FRONT_LEFT_FAR_PL[] PROGMEM = "X/Y calibration bad. Left front calibration point not reachable. Fix the printer.";
const char * const MSG_BED_SKEW_OFFSET_DETECTION_FRONT_LEFT_FAR_LANG_TABLE[LANG_NUM] PROGMEM = {
MSG_BED_SKEW_OFFSET_DETECTION_FRONT_LEFT_FAR_EN,
MSG_BED_SKEW_OFFSET_DETECTION_FRONT_LEFT_FAR_CZ,
@ -304,11 +317,11 @@ const char * const MSG_BED_SKEW_OFFSET_DETECTION_FRONT_LEFT_FAR_LANG_TABLE[LANG_
MSG_BED_SKEW_OFFSET_DETECTION_FRONT_LEFT_FAR_PL
};
const char MSG_BED_SKEW_OFFSET_DETECTION_FRONT_RIGHT_FAR_EN[] PROGMEM = "X/Y calibration bad. Right front corner not reachable. Fix the printer.";
const char MSG_BED_SKEW_OFFSET_DETECTION_FRONT_RIGHT_FAR_EN[] PROGMEM = "X/Y calibration bad. Right front calibration point not reachable. Fix the printer.";
const char MSG_BED_SKEW_OFFSET_DETECTION_FRONT_RIGHT_FAR_CZ[] PROGMEM = "Kalibrace selhala. Pravy predni bod moc vpredu. Srovnejte tiskarnu.";
const char MSG_BED_SKEW_OFFSET_DETECTION_FRONT_RIGHT_FAR_IT[] PROGMEM = "X/Y calibration bad. Right front corner not reachable. Fix the printer.";
const char MSG_BED_SKEW_OFFSET_DETECTION_FRONT_RIGHT_FAR_ES[] PROGMEM = "X/Y calibration bad. Right front corner not reachable. Fix the printer.";
const char MSG_BED_SKEW_OFFSET_DETECTION_FRONT_RIGHT_FAR_PL[] PROGMEM = "X/Y calibration bad. Right front corner not reachable. Fix the printer.";
const char MSG_BED_SKEW_OFFSET_DETECTION_FRONT_RIGHT_FAR_IT[] PROGMEM = "X/Y calibration bad. Right front calibration point not reachable. Fix the printer.";
const char MSG_BED_SKEW_OFFSET_DETECTION_FRONT_RIGHT_FAR_ES[] PROGMEM = "X/Y calibration bad. Right front calibration point not reachable. Fix the printer.";
const char MSG_BED_SKEW_OFFSET_DETECTION_FRONT_RIGHT_FAR_PL[] PROGMEM = "X/Y calibration bad. Right front calibration point not reachable. Fix the printer.";
const char * const MSG_BED_SKEW_OFFSET_DETECTION_FRONT_RIGHT_FAR_LANG_TABLE[LANG_NUM] PROGMEM = {
MSG_BED_SKEW_OFFSET_DETECTION_FRONT_RIGHT_FAR_EN,
MSG_BED_SKEW_OFFSET_DETECTION_FRONT_RIGHT_FAR_CZ,

2
Firmware/language_all.h
View File

@ -52,6 +52,8 @@ extern const char* const MSG_BED_LEVELING_FAILED_POINT_LOW_LANG_TABLE[LANG_NUM];
#define MSG_BED_LEVELING_FAILED_POINT_LOW LANG_TABLE_SELECT(MSG_BED_LEVELING_FAILED_POINT_LOW_LANG_TABLE)
extern const char* const MSG_BED_SKEW_OFFSET_DETECTION_FAILED_LANG_TABLE[LANG_NUM];
#define MSG_BED_SKEW_OFFSET_DETECTION_FAILED LANG_TABLE_SELECT(MSG_BED_SKEW_OFFSET_DETECTION_FAILED_LANG_TABLE)
extern const char* const MSG_BED_SKEW_OFFSET_DETECTION_FRONT_BOTH_FAR_LANG_TABLE[LANG_NUM];
#define MSG_BED_SKEW_OFFSET_DETECTION_FRONT_BOTH_FAR LANG_TABLE_SELECT(MSG_BED_SKEW_OFFSET_DETECTION_FRONT_BOTH_FAR_LANG_TABLE)
extern const char* const MSG_BED_SKEW_OFFSET_DETECTION_FRONT_LEFT_FAR_LANG_TABLE[LANG_NUM];
#define MSG_BED_SKEW_OFFSET_DETECTION_FRONT_LEFT_FAR LANG_TABLE_SELECT(MSG_BED_SKEW_OFFSET_DETECTION_FRONT_LEFT_FAR_LANG_TABLE)
extern const char* const MSG_BED_SKEW_OFFSET_DETECTION_FRONT_RIGHT_FAR_LANG_TABLE[LANG_NUM];

1
Firmware/language_cz.h
View File

@ -303,6 +303,7 @@
#define MSG_BED_SKEW_OFFSET_DETECTION_SKEW_EXTREME "X/Y osy jsou silne zkosene. Zkoseni bude automaticky vyrovnano pri tisku."
#define MSG_BED_SKEW_OFFSET_DETECTION_FRONT_LEFT_FAR "Kalibrace selhala. Levy predni bod moc vpredu. Srovnejte tiskarnu."
#define MSG_BED_SKEW_OFFSET_DETECTION_FRONT_RIGHT_FAR "Kalibrace selhala. Pravy predni bod moc vpredu. Srovnejte tiskarnu."
#define MSG_BED_SKEW_OFFSET_DETECTION_FRONT_BOTH_FAR "Kalibrace selhala. Predni kalibracni body moc vpredu. Srovnejte tiskarnu."
#define MSG_BED_LEVELING_FAILED_POINT_LOW "Kalibrace Z selhala. Sensor nesepnul. Znecistena tryska? Cekam na reset."
#define MSG_BED_LEVELING_FAILED_POINT_HIGH "Kalibrace Z selhala. Sensor sepnul prilis vysoko. Cekam na reset."

5
Firmware/language_en.h
View File

@ -294,8 +294,9 @@
#define MSG_BED_SKEW_OFFSET_DETECTION_PERFECT "X/Y calibration ok. X/Y axes are perpendicular."
#define MSG_BED_SKEW_OFFSET_DETECTION_SKEW_MILD "X/Y calibration all right. X/Y axes are slightly skewed."
#define MSG_BED_SKEW_OFFSET_DETECTION_SKEW_EXTREME "X/Y skewed severly. Skew will be corrected automatically."
#define MSG_BED_SKEW_OFFSET_DETECTION_FRONT_LEFT_FAR "X/Y calibration bad. Left front corner not reachable. Fix the printer."
#define MSG_BED_SKEW_OFFSET_DETECTION_FRONT_RIGHT_FAR "X/Y calibration bad. Right front corner not reachable. Fix the printer."
#define MSG_BED_SKEW_OFFSET_DETECTION_FRONT_LEFT_FAR "X/Y calibration bad. Left front calibration point not reachable. Fix the printer."
#define MSG_BED_SKEW_OFFSET_DETECTION_FRONT_RIGHT_FAR "X/Y calibration bad. Right front calibration point not reachable. Fix the printer."
#define MSG_BED_SKEW_OFFSET_DETECTION_FRONT_BOTH_FAR "X/Y calibration bad. Front calibration points not reachable. Fix the printer."
#define MSG_BED_LEVELING_FAILED_POINT_LOW "Bed leveling failed. Sensor didnt trigger. Debris on nozzle? Waiting for reset."
#define MSG_BED_LEVELING_FAILED_POINT_HIGH "Bed leveling failed. Sensor triggered too high. Waiting for reset."

321
Firmware/mesh_bed_calibration.cpp
View File

@ -7,8 +7,6 @@
#include "ultralcd.h"
// #include "qr_solve.h"
extern float home_retract_mm_ext(int axis);
uint8_t world2machine_correction_mode;
float world2machine_rotation_and_skew[2][2];
float world2machine_rotation_and_skew_inv[2][2];
@ -23,16 +21,22 @@ float world2machine_shift[2];
// Scaling of the real machine axes against the programmed dimensions in the firmware.
// The correction is tiny, here around 0.5mm on 250mm length.
#define MACHINE_AXIS_SCALE_X ((250.f + 0.5f) / 250.f)
#define MACHINE_AXIS_SCALE_Y ((250.f + 0.5f) / 250.f)
//#define MACHINE_AXIS_SCALE_X ((250.f - 0.5f) / 250.f)
//#define MACHINE_AXIS_SCALE_Y ((250.f - 0.5f) / 250.f)
#define MACHINE_AXIS_SCALE_X 1.f
#define MACHINE_AXIS_SCALE_Y 1.f
// 0.12 degrees equals to an offset of 0.5mm on 250mm length.
#define BED_SKEW_ANGLE_MILD (0.12f * M_PI / 180.f)
// 0.25 degrees equals to an offset of 1.1mm on 250mm length.
#define BED_SKEW_ANGLE_EXTREME (0.25f * M_PI / 180.f)
#define BED_CALIBRATION_POINT_OFFSET_MAX_EUCLIDIAN (0.8f)
#define BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_X (0.8f)
#define BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_Y (1.5f)
#define MIN_BED_SENSOR_POINT_RESPONSE_DMR (2.0f)
// Positions of the bed reference points in the machine coordinates, referenced to the P.I.N.D.A sensor.
// The points are ordered in a zig-zag fashion to speed up the calibration.
const float bed_ref_points[] PROGMEM = {
@ -357,6 +361,19 @@ bool calculate_machine_skew_and_offset_LS(
#else
static inline float point_weight_y(uint8_t i, BedSkewOffsetDetectionResultType measured_points_status)
{
float w = 1.f;
if (i < 3) {
w = WEIGHT_FIRST_ROW;
if ((i == 0 && (measured_points_status & BED_SKEW_OFFSET_DETECTION_FRONT_LEFT_FAR)) ||
(i == 1 && (measured_points_status & BED_SKEW_OFFSET_DETECTION_FRONT_BOTH_FAR)) ||
(i == 2 && (measured_points_status & BED_SKEW_OFFSET_DETECTION_FRONT_RIGHT_FAR)))
w = 0.f;
}
return w;
}
// Non-Linear Least Squares fitting of the bed to the measured induction points
// using the Gauss-Newton method.
// This method will maintain a unity length of the machine axes,
@ -365,6 +382,7 @@ BedSkewOffsetDetectionResultType calculate_machine_skew_and_offset_LS(
// Matrix of maximum 9 2D points (18 floats)
const float *measured_pts,
uint8_t npts,
BedSkewOffsetDetectionResultType measured_points_status,
const float *true_pts,
// Resulting correction matrix.
float *vec_x,
@ -462,7 +480,7 @@ BedSkewOffsetDetectionResultType calculate_machine_skew_and_offset_LS(
(c == 1) ? 1.f :
((c == 2) ? ( c1 * measured_pts[2 * i]) :
(-s2 * measured_pts[2 * i + 1]));
float w = (i < 3) ? WEIGHT_FIRST_ROW : 1.f;
float w = point_weight_y(i, measured_points_status);
acc += a * b * w;
}
}
@ -487,7 +505,7 @@ BedSkewOffsetDetectionResultType calculate_machine_skew_and_offset_LS(
((r == 2) ? ( c1 * measured_pts[2 * i]) :
(-s2 * measured_pts[2 * i + 1])));
float fy = s1 * measured_pts[2 * i] + c2 * measured_pts[2 * i + 1] + cntr[1] - pgm_read_float(true_pts + i * 2 + 1);
float w = (i < 3) ? WEIGHT_FIRST_ROW : 1.f;
float w = point_weight_y(i, measured_points_status);
acc += j * fy * w;
}
}
@ -594,12 +612,13 @@ BedSkewOffsetDetectionResultType calculate_machine_skew_and_offset_LS(
float errY = sqr(pgm_read_float(true_pts + i * 2 + 1) - y);
float err = sqrt(errX + errY);
if (i < 3) {
float w = point_weight_y(i, measured_points_status);
if (sqrt(errX) > BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_X ||
sqrt(errY) > BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_Y)
result = BED_SKEW_OFFSET_DETECTION_FAILED;
(w != 0.f && sqrt(errY) > BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_Y))
result = (measured_points_status == BED_SKEW_OFFSET_DETECTION_PERFECT) ? BED_SKEW_OFFSET_DETECTION_FAILED : BedSkewOffsetDetectionResultType(- int8_t(measured_points_status));
} else {
if (err > BED_CALIBRATION_POINT_OFFSET_MAX_EUCLIDIAN)
result = BED_SKEW_OFFSET_DETECTION_FAILED;
result = (measured_points_status == BED_SKEW_OFFSET_DETECTION_PERFECT) ? BED_SKEW_OFFSET_DETECTION_FAILED : BedSkewOffsetDetectionResultType(- int8_t(measured_points_status));
}
if (verbosity_level >= 10) {
SERIAL_ECHOPGM("point #");
@ -622,14 +641,42 @@ BedSkewOffsetDetectionResultType calculate_machine_skew_and_offset_LS(
}
}
#if 0
if (result == BED_SKEW_OFFSET_DETECTION_PERFECT && fabs(a1) < BED_SKEW_ANGLE_MILD && fabs(a2) < BED_SKEW_ANGLE_MILD) {
if (verbosity_level > 0)
SERIAL_ECHOLNPGM("Very little skew detected. Disabling skew correction.");
// Just disable the skew correction.
vec_x[0] = MACHINE_AXIS_SCALE_X;
vec_x[1] = 0.f;
vec_y[0] = 0.f;
vec_y[1] = MACHINE_AXIS_SCALE_Y;
}
#else
if (result == BED_SKEW_OFFSET_DETECTION_PERFECT) {
if (verbosity_level > 0)
SERIAL_ECHOLNPGM("Very little skew detected. Orthogonalizing the axes.");
// Orthogonalize the axes.
a1 = 0.5f * (a1 + a2);
vec_x[0] = cos(a1) * MACHINE_AXIS_SCALE_X;
vec_x[1] = sin(a1) * MACHINE_AXIS_SCALE_X;
vec_y[0] = -sin(a1) * MACHINE_AXIS_SCALE_Y;
vec_y[1] = cos(a1) * MACHINE_AXIS_SCALE_Y;
// Refresh the offset.
cntr[0] = 0.f;
cntr[1] = 0.f;
float wy = 0.f;
for (int8_t i = 0; i < 9; ++ i) {
float x = vec_x[0] * measured_pts[i * 2] + vec_y[0] * measured_pts[i * 2 + 1];
float y = vec_x[1] * measured_pts[i * 2] + vec_y[1] * measured_pts[i * 2 + 1];
float w = (i < 3) ? WEIGHT_FIRST_ROW : 1.f;
cntr[0] += pgm_read_float(true_pts + i * 2) - x;
cntr[1] += w * (pgm_read_float(true_pts + i * 2 + 1) - y);
wy += w;
}
cntr[0] /= 9.f;
cntr[1] /= wy;
}
#endif
// Invert the transformation matrix made of vec_x, vec_y and cntr.
{
@ -698,6 +745,9 @@ BedSkewOffsetDetectionResultType calculate_machine_skew_and_offset_LS(
delay_keep_alive(100);
}
if (result >= 0 && measured_points_status > 0)
// Maintain the "left / right / both points out of reach" status.
result = measured_points_status;
return result;
}
@ -885,10 +935,12 @@ static inline void update_current_position_z()
}
// At the current position, find the Z stop.
inline bool find_bed_induction_sensor_point_z(float minimum_z)
inline bool find_bed_induction_sensor_point_z(float minimum_z, uint8_t n_iter)
{
SERIAL_ECHOLNPGM("find_bed_induction_sensor_point_z 1");
bool endstops_enabled = enable_endstops(true);
bool endstop_z_enabled = enable_z_endstop(false);
float z = 0.f;
endstop_z_hit_on_purpose();
// move down until you find the bed
@ -899,24 +951,33 @@ inline bool find_bed_induction_sensor_point_z(float minimum_z)
if (! endstop_z_hit_on_purpose())
goto error;
// move up the retract distance
current_position[Z_AXIS] += home_retract_mm_ext(Z_AXIS);
go_to_current(homing_feedrate[Z_AXIS]/60);
// move back down slowly to find bed
current_position[Z_AXIS] -= home_retract_mm_ext(Z_AXIS) * 2;
current_position[Z_AXIS] = min(current_position[Z_AXIS], minimum_z);
go_to_current(homing_feedrate[Z_AXIS]/(4*60));
// we have to let the planner know where we are right now as it is not where we said to go.
update_current_position_z();
if (! endstop_z_hit_on_purpose())
goto error;
for (uint8_t i = 0; i < n_iter; ++ i) {
// Move up the retract distance.
current_position[Z_AXIS] += .5f;
go_to_current(homing_feedrate[Z_AXIS]/60);
// Move back down slowly to find bed.
current_position[Z_AXIS] = minimum_z;
go_to_current(homing_feedrate[Z_AXIS]/(4*60));
// we have to let the planner know where we are right now as it is not where we said to go.
update_current_position_z();
if (! endstop_z_hit_on_purpose())
goto error;
SERIAL_ECHOPGM("Bed find_bed_induction_sensor_point_z low, height: ");
MYSERIAL.print(current_position[Z_AXIS], 5);
SERIAL_ECHOLNPGM("");
z += current_position[Z_AXIS];
}
current_position[Z_AXIS] = z;
if (n_iter > 1)
current_position[Z_AXIS] /= float(n_iter);
enable_endstops(endstops_enabled);
enable_z_endstop(endstop_z_enabled);
SERIAL_ECHOLNPGM("find_bed_induction_sensor_point_z 3");
return true;
error:
SERIAL_ECHOLNPGM("find_bed_induction_sensor_point_z 4");
enable_endstops(endstops_enabled);
enable_z_endstop(endstop_z_enabled);
return false;
@ -1250,6 +1311,11 @@ inline bool improve_bed_induction_sensor_point2(bool lift_z_on_min_y, int8_t ver
goto canceled;
}
b = current_position[X_AXIS];
if (b - a < MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
// We force the calibration routine to move the Z axis slightly down to make the response more pronounced.
current_position[X_AXIS] = center_old_x;
goto canceled;
}
if (verbosity_level >= 5) {
debug_output_point(PSTR("left" ), a, current_position[Y_AXIS], current_position[Z_AXIS]);
debug_output_point(PSTR("right"), b, current_position[Y_AXIS], current_position[Z_AXIS]);
@ -1304,6 +1370,11 @@ inline bool improve_bed_induction_sensor_point2(bool lift_z_on_min_y, int8_t ver
goto canceled;
}
b = current_position[Y_AXIS];
if (b - a < MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
// We force the calibration routine to move the Z axis slightly down to make the response more pronounced.
current_position[Y_AXIS] = center_old_y;
goto canceled;
}
if (verbosity_level >= 5) {
debug_output_point(PSTR("top" ), current_position[X_AXIS], a, current_position[Z_AXIS]);
debug_output_point(PSTR("bottom"), current_position[X_AXIS], b, current_position[Z_AXIS]);
@ -1332,7 +1403,7 @@ enum InductionSensorPointStatusType
{
INDUCTION_SENSOR_POINT_FAILED = -1,
INDUCTION_SENSOR_POINT_OK = 0,
INDUCTION_SENSOR_POINT_FAR,
INDUCTION_SENSOR_POINT_FAR = 1,
};
inline InductionSensorPointStatusType improve_bed_induction_sensor_point3(int verbosity_level)
{
@ -1370,7 +1441,7 @@ inline InductionSensorPointStatusType improve_bed_induction_sensor_point3(int ve
float dmax = 0.f;
float xmax1 = 0.f;
float xmax2 = 0.f;
for (float y = y0; y < y1; y += IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
for (y = y0; y < y1; y += IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
enable_z_endstop(false);
go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
enable_z_endstop(true);
@ -1409,16 +1480,17 @@ inline InductionSensorPointStatusType improve_bed_induction_sensor_point3(int ve
}
}
if (dmax == 0.) {
SERIAL_PROTOCOLPGM("failed - not found\n");
if (verbosity_level > 0)
SERIAL_PROTOCOLPGM("failed - not found\n");
goto canceled;
}
// SERIAL_PROTOCOLPGM("ok 1\n");
// Search in the negative Y direction, until a maximum diameter is found.
dmax = 0.;
dmax = 0.f;
if (y0 + 1.f < y1)
y1 = y0 + 1.f;
for (float y = y1; y >= y0; y -= IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
for (y = y1; y >= y0; y -= IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
enable_z_endstop(false);
go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
enable_z_endstop(true);
@ -1460,24 +1532,92 @@ inline InductionSensorPointStatusType improve_bed_induction_sensor_point3(int ve
break;
}
}
// SERIAL_PROTOCOLPGM("ok 2\n");
float xmax, ymax;
if (dmax == 0.f) {
// Only the hit in the positive direction found.
xmax = xmax1;
ymax = y0;
} else {
// Both positive and negative directions found.
xmax = xmax2;
ymax = 0.5f * (y0 + y1);
for (; y >= y0; y -= IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
enable_z_endstop(false);
go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
enable_z_endstop(true);
go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
update_current_position_xyz();
if (! endstop_z_hit_on_purpose()) {
continue;
/*
current_position[X_AXIS] = center_old_x;
SERIAL_PROTOCOLPGM("Failed 3\n");
goto canceled;
*/
}
a = current_position[X_AXIS];
enable_z_endstop(false);
go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
enable_z_endstop(true);
go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
update_current_position_xyz();
if (! endstop_z_hit_on_purpose()) {
continue;
/*
current_position[X_AXIS] = center_old_x;
SERIAL_PROTOCOLPGM("Failed 4\n");
goto canceled;
*/
}
b = current_position[X_AXIS];
if (verbosity_level >= 5) {
debug_output_point(PSTR("left" ), a, current_position[Y_AXIS], current_position[Z_AXIS]);
debug_output_point(PSTR("right"), b, current_position[Y_AXIS], current_position[Z_AXIS]);
}
float d = b - a;
if (d > dmax) {
xmax = 0.5f * (a + b);
ymax = y;
dmax = d;
}
}
}
{
// Compare the distance in the Y+ direction with the diameter in the X direction.
// Find the positive Y hit.
enable_z_endstop(false);
go_xy(xmax, ymax + IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS, homing_feedrate[X_AXIS] / 60.f);
enable_z_endstop(true);
go_xy(xmax, max(ymax - IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS, Y_MIN_POS_FOR_BED_CALIBRATION), homing_feedrate[X_AXIS] / 60.f);
update_current_position_xyz();
if (! endstop_z_hit_on_purpose()) {
current_position[Y_AXIS] = center_old_y;
goto canceled;
}
if (verbosity_level >= 5)
debug_output_point(PSTR("top" ), current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
if (current_position[Y_AXIS] - y0 < 0.5f * dmax) {
// Probably not even a half circle was detected. The induction point is too far in the minus Y direction.
if (current_position[Y_AXIS] - 0.5f * dmax < Y_MIN_POS_FOR_BED_CALIBRATION - 0.6f) {
ymax = current_position[Y_AXIS] - 0.5f * dmax;
// ymax = Y_MIN_POS_FOR_BED_CALIBRATION;
y_too_far = true;
} else {
ymax = current_position[Y_AXIS] - 0.5f * dmax;
}
//FIXME
if (ymax < Y_MIN_POS_FOR_BED_CALIBRATION)
ymax = Y_MIN_POS_FOR_BED_CALIBRATION;
} else {
ymax = 0.5f * (y0 + y1);
}
}
// Go to the center.
enable_z_endstop(false);
if (dmax == 0.f) {
// Found only the point going from ymin to ymax.
current_position[X_AXIS] = xmax1;
current_position[Y_AXIS] = y0;
y_too_far = true;
} else {
// Both points found (from ymin to ymax and from ymax to ymin).
float p = 0.5f;
// If the first hit was on the machine boundary,
// give it a higher weight.
if (y0 == Y_MIN_POS_FOR_BED_CALIBRATION)
p = 0.75f;
current_position[X_AXIS] = p * xmax1 + (1.f - p) * xmax2;
current_position[Y_AXIS] = p * y0 + (1.f - p) * y1;
}
current_position[X_AXIS] = xmax;
current_position[Y_AXIS] = ymax;
if (verbosity_level >= 20) {
SERIAL_ECHOPGM("Adjusted position: ");
SERIAL_ECHO(current_position[X_AXIS]);
@ -1489,47 +1629,6 @@ inline InductionSensorPointStatusType improve_bed_induction_sensor_point3(int ve
// delay_keep_alive(3000);
}
// Try yet to improve the X position.
{
float x0 = current_position[X_AXIS] - IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS;
float x1 = current_position[X_AXIS] + IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS;
if (x0 < X_MIN_POS)
x0 = X_MIN_POS;
if (x1 > X_MAX_POS)
x1 = X_MAX_POS;
// Search in the X direction along a cross.
enable_z_endstop(false);
go_xy(x0, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
enable_z_endstop(true);
go_xy(x1, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
update_current_position_xyz();
if (! endstop_z_hit_on_purpose()) {
current_position[X_AXIS] = center_old_x;
goto canceled;
}
a = current_position[X_AXIS];
enable_z_endstop(false);
go_xy(x1, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
enable_z_endstop(true);
go_xy(x0, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
update_current_position_xyz();
if (! endstop_z_hit_on_purpose()) {
current_position[X_AXIS] = center_old_x;
goto canceled;
}
b = current_position[X_AXIS];
if (verbosity_level >= 5) {
debug_output_point(PSTR("left" ), a, current_position[Y_AXIS], current_position[Z_AXIS]);
debug_output_point(PSTR("right"), b, current_position[Y_AXIS], current_position[Z_AXIS]);
}
// Go to the center.
enable_z_endstop(false);
current_position[X_AXIS] = 0.5f * (a + b);
go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
}
return y_too_far ? INDUCTION_SENSOR_POINT_FAR : INDUCTION_SENSOR_POINT_OK;
canceled:
@ -1692,7 +1791,7 @@ BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level
}
}
calculate_machine_skew_and_offset_LS(pts, 4, bed_ref_points_4, vec_x, vec_y, cntr, verbosity_level);
calculate_machine_skew_and_offset_LS(pts, 4, BED_SKEW_OFFSET_DETECTION_PERFECT, bed_ref_points_4, vec_x, vec_y, cntr, verbosity_level);
world2machine_update(vec_x, vec_y, cntr);
#if 1
// Fearlessly store the calibration values into the eeprom.
@ -1760,7 +1859,7 @@ BedSkewOffsetDetectionResultType improve_bed_offset_and_skew(int8_t method, int8
// Collect a matrix of 9x9 points.
bool leftFrontTooFar = false;
bool rightFrontTooFar = false;
BedSkewOffsetDetectionResultType result = BED_SKEW_OFFSET_DETECTION_PERFECT;
int8_t result = BED_SKEW_OFFSET_DETECTION_PERFECT;
for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
// Don't let the manage_inactivity() function remove power from the motors.
refresh_cmd_timeout();
@ -1824,9 +1923,12 @@ BedSkewOffsetDetectionResultType improve_bed_offset_and_skew(int8_t method, int8
found = false;
} else {
found = true;
if (iter == 7 && INDUCTION_SENSOR_POINT_FAR && mesh_point != 1)
if (iter == 7 && INDUCTION_SENSOR_POINT_FAR)
// Remember, which side of the bed is shifted too far in the minus y direction.
result = (mesh_point == 0) ? BED_SKEW_OFFSET_DETECTION_FRONT_LEFT_FAR : BED_SKEW_OFFSET_DETECTION_FRONT_RIGHT_FAR;
result |=
(mesh_point == 1) ? BED_SKEW_OFFSET_DETECTION_FRONT_BOTH_FAR :
((mesh_point == 0) ? BED_SKEW_OFFSET_DETECTION_FRONT_LEFT_FAR :
BED_SKEW_OFFSET_DETECTION_FRONT_RIGHT_FAR);
}
} else {
switch (method) {
@ -1847,11 +1949,11 @@ BedSkewOffsetDetectionResultType improve_bed_offset_and_skew(int8_t method, int8
goto canceled;
} else {
// Try to move the Z axis down a bit to increase a chance of the sensor to trigger.
current_position[Z_AXIS] -= 0.025f;
current_position[Z_AXIS] -= 0.05f;
enable_endstops(false);
enable_z_endstop(false);
go_to_current(homing_feedrate[Z_AXIS]);
if (verbosity_level > 20) {
if (verbosity_level >= 5) {
SERIAL_ECHOPGM("Improving bed point ");
SERIAL_ECHO(mesh_point);
SERIAL_ECHOPGM(", iteration ");
@ -1898,9 +2000,13 @@ BedSkewOffsetDetectionResultType improve_bed_offset_and_skew(int8_t method, int8
}
}
result = calculate_machine_skew_and_offset_LS(pts, 9, bed_ref_points, vec_x, vec_y, cntr, verbosity_level);
if (result == BED_SKEW_OFFSET_DETECTION_FAILED)
goto canceled;
{
result = calculate_machine_skew_and_offset_LS(pts, 9, BedSkewOffsetDetectionResultType(result), bed_ref_points, vec_x, vec_y, cntr, verbosity_level);
if (result < 0) {
SERIAL_ECHOLNPGM("Calculation of the machine skew and offset failed.");
goto canceled;
}
}
world2machine_update(vec_x, vec_y, cntr);
#if 1
@ -1933,13 +2039,17 @@ BedSkewOffsetDetectionResultType improve_bed_offset_and_skew(int8_t method, int8
go_to_current(homing_feedrate[X_AXIS]/60);
delay_keep_alive(3000);
}
SERIAL_ECHOPGM("Final calculated bed point ");
SERIAL_ECHO(mesh_point);
SERIAL_ECHOPGM(": ");
MYSERIAL.print(st_get_position_mm(X_AXIS), 5);
SERIAL_ECHOPGM(", ");
MYSERIAL.print(st_get_position_mm(Y_AXIS), 5);
SERIAL_ECHOLNPGM("");
{
float x, y;
world2machine(current_position[X_AXIS], current_position[Y_AXIS], x, y);
SERIAL_ECHOPGM("Final calculated bed point ");
SERIAL_ECHO(mesh_point);
SERIAL_ECHOPGM(": ");
MYSERIAL.print(x, 5);
SERIAL_ECHOPGM(", ");
MYSERIAL.print(y, 5);
SERIAL_ECHOLNPGM("");
}
}
}
@ -1951,18 +2061,21 @@ BedSkewOffsetDetectionResultType improve_bed_offset_and_skew(int8_t method, int8
go_to_current(homing_feedrate[Z_AXIS]/60);
current_position[X_AXIS] = pgm_read_float(bed_ref_points);
current_position[Y_AXIS] = pgm_read_float(bed_ref_points+1);
world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
go_to_current(homing_feedrate[X_AXIS]/60);
memcpy(destination, current_position, sizeof(destination));
enable_endstops(true);
homeaxis(Z_AXIS);
mbl.set_z(0, 0, current_position[Z_AXIS]);
enable_endstops(false);
find_bed_induction_sensor_point_z();
mbl.set_z(0, 0, current_position[Z_AXIS]);
}
for (int8_t mesh_point = 1; mesh_point != MESH_MEAS_NUM_X_POINTS * MESH_MEAS_NUM_Y_POINTS; ++ mesh_point) {
current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
go_to_current(homing_feedrate[Z_AXIS]/60);
current_position[X_AXIS] = pgm_read_float(bed_ref_points+2*mesh_point);
current_position[Y_AXIS] = pgm_read_float(bed_ref_points+2*mesh_point+1);
world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
go_to_current(homing_feedrate[X_AXIS]/60);
find_bed_induction_sensor_point_z();
// Get cords of measuring point
@ -2029,13 +2142,15 @@ BedSkewOffsetDetectionResultType improve_bed_offset_and_skew(int8_t method, int8
go_to_current(homing_feedrate[Z_AXIS]/60);
current_position[X_AXIS] = X_MIN_POS+0.2;
current_position[Y_AXIS] = Y_MIN_POS+0.2;
// Clamp to the physical coordinates.
world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
go_to_current(homing_feedrate[X_AXIS]/60);
enable_endstops(endstops_enabled);
enable_z_endstop(endstop_z_enabled);
// Don't let the manage_inactivity() function remove power from the motors.
refresh_cmd_timeout();
return result;
return BedSkewOffsetDetectionResultType(result);
canceled:
// Don't let the manage_inactivity() function remove power from the motors.
@ -2047,7 +2162,7 @@ canceled:
reset_bed_offset_and_skew();
enable_endstops(endstops_enabled);
enable_z_endstop(endstop_z_enabled);
return BED_SKEW_OFFSET_DETECTION_FAILED;
return (result == BED_SKEW_OFFSET_DETECTION_PERFECT) ? BED_SKEW_OFFSET_DETECTION_FAILED : BedSkewOffsetDetectionResultType(- int8_t(result));
}
bool scan_bed_induction_points(int8_t verbosity_level)

39
Firmware/mesh_bed_calibration.h
View File

@ -112,7 +112,33 @@ inline void machine2world(float &x, float &y)
}
}
extern bool find_bed_induction_sensor_point_z(float minimum_z = -10.f);
inline bool world2machine_clamp(float &x, float &y)
{
bool clamped = false;
float tmpx, tmpy;
world2machine(x, y, tmpx, tmpy);
if (tmpx < X_MIN_POS) {
tmpx = X_MIN_POS;
clamped = true;
}
if (tmpy < Y_MIN_POS) {
tmpy = Y_MIN_POS;
clamped = true;
}
if (tmpx > X_MAX_POS) {
tmpx = X_MAX_POS;
clamped = true;
}
if (tmpy > Y_MAX_POS) {
tmpy = Y_MAX_POS;
clamped = true;
}
if (clamped)
machine2world(tmpx, tmpy, x, y);
return clamped;
}
extern bool find_bed_induction_sensor_point_z(float minimum_z = -10.f, uint8_t n_iter = 3);
extern bool find_bed_induction_sensor_point_xy();
// Positive or zero: ok
@ -122,12 +148,13 @@ enum BedSkewOffsetDetectionResultType {
BED_SKEW_OFFSET_DETECTION_FAILED = -1,
// Detection finished with success.
BED_SKEW_OFFSET_DETECTION_PERFECT = 0,
BED_SKEW_OFFSET_DETECTION_SKEW_MILD,
BED_SKEW_OFFSET_DETECTION_SKEW_EXTREME,
BED_SKEW_OFFSET_DETECTION_PERFECT = 0,
BED_SKEW_OFFSET_DETECTION_SKEW_MILD = 1,
BED_SKEW_OFFSET_DETECTION_SKEW_EXTREME = 2,
// Detection finished with success, but it is recommended to fix the printer mechanically.
BED_SKEW_OFFSET_DETECTION_FRONT_LEFT_FAR,
BED_SKEW_OFFSET_DETECTION_FRONT_RIGHT_FAR
BED_SKEW_OFFSET_DETECTION_FRONT_LEFT_FAR = 4,
BED_SKEW_OFFSET_DETECTION_FRONT_RIGHT_FAR = 8,
BED_SKEW_OFFSET_DETECTION_FRONT_BOTH_FAR = BED_SKEW_OFFSET_DETECTION_FRONT_LEFT_FAR | BED_SKEW_OFFSET_DETECTION_FRONT_RIGHT_FAR,
};
extern BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level);

3
Firmware/ultralcd.cpp
View File

@ -1376,6 +1376,9 @@ void lcd_bed_calibration_show_result(BedSkewOffsetDetectionResultType result)
case BED_SKEW_OFFSET_DETECTION_FRONT_RIGHT_FAR:
msg = MSG_BED_SKEW_OFFSET_DETECTION_FRONT_RIGHT_FAR;
break;
case BED_SKEW_OFFSET_DETECTION_FRONT_BOTH_FAR:
msg = MSG_BED_SKEW_OFFSET_DETECTION_FRONT_BOTH_FAR;
break;
}
lcd_display_message_fullscreen_P(msg);