Removed support for DELTA, SCARA and BARICUDA.
Implemented bed skew calibration by matching a precise physical model to the measured data using the least squares method. Rewrote handling of the command buffer to preserve memory and allow pushing the commands to the front of the queue.
This commit is contained in:
bubnikv
parent
08bf6acf1b
commit
78ebd522b6
@ -31,20 +31,6 @@
|
||||
// Advanced settings can be found in Configuration_adv.h
|
||||
// BASIC SETTINGS: select your board type, temperature sensor type, axis scaling, and endstop configuration
|
||||
|
||||
//===========================================================================
|
||||
//============================= DELTA Printer ===============================
|
||||
//===========================================================================
|
||||
// For a Delta printer replace the configuration files with the files in the
|
||||
// example_configurations/delta directory.
|
||||
//
|
||||
|
||||
//===========================================================================
|
||||
//============================= SCARA Printer ===============================
|
||||
//===========================================================================
|
||||
// For a Delta printer replace the configuration files with the files in the
|
||||
// example_configurations/SCARA directory.
|
||||
//
|
||||
|
||||
// User-specified version info of this build to display in [Pronterface, etc] terminal window during
|
||||
// startup. Implementation of an idea by Prof Braino to inform user that any changes made to this
|
||||
// build by the user have been successfully uploaded into firmware.
|
||||
@ -396,9 +382,6 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
|
||||
//Manual homing switch locations:
|
||||
// For deltabots this means top and center of the Cartesian print volume.
|
||||
|
||||
//#define MANUAL_Z_HOME_POS 402 // For delta: Distance between nozzle and print surface after homing.
|
||||
|
||||
|
||||
|
||||
// Offset of the extruders (uncomment if using more than one and relying on firmware to position when changing).
|
||||
// The offset has to be X=0, Y=0 for the extruder 0 hotend (default extruder).
|
||||
@ -634,9 +617,6 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
|
||||
// SF send wrong arc g-codes when using Arc Point as fillet procedure
|
||||
//#define SF_ARC_FIX
|
||||
|
||||
// Support for the BariCUDA Paste Extruder.
|
||||
//#define BARICUDA
|
||||
|
||||
//define BlinkM/CyzRgb Support
|
||||
//#define BLINKM
|
||||
|
||||
@ -654,14 +634,6 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
|
||||
//
|
||||
//#define NUM_SERVOS 3 // Servo index starts with 0 for M280 command
|
||||
|
||||
// Servo Endstops
|
||||
//
|
||||
// This allows for servo actuated endstops, primary usage is for the Z Axis to eliminate calibration or bed height changes.
|
||||
// Use M206 command to correct for switch height offset to actual nozzle height. Store that setting with M500.
|
||||
//
|
||||
//#define SERVO_ENDSTOPS {-1, -1, 0} // Servo index for X, Y, Z. Disable with -1
|
||||
//#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
|
||||
|
||||
/**********************************************************************\
|
||||
* Support for a filament diameter sensor
|
||||
* Also allows adjustment of diameter at print time (vs at slicing)
|
||||
|
||||
@ -63,12 +63,6 @@ void Config_StoreSettings()
|
||||
EEPROM_WRITE_VAR(i,max_z_jerk);
|
||||
EEPROM_WRITE_VAR(i,max_e_jerk);
|
||||
EEPROM_WRITE_VAR(i,add_homing);
|
||||
#ifdef DELTA
|
||||
EEPROM_WRITE_VAR(i,endstop_adj);
|
||||
EEPROM_WRITE_VAR(i,delta_radius);
|
||||
EEPROM_WRITE_VAR(i,delta_diagonal_rod);
|
||||
EEPROM_WRITE_VAR(i,delta_segments_per_second);
|
||||
#endif
|
||||
#ifndef ULTIPANEL
|
||||
int plaPreheatHotendTemp = PLA_PREHEAT_HOTEND_TEMP, plaPreheatHPBTemp = PLA_PREHEAT_HPB_TEMP, plaPreheatFanSpeed = PLA_PREHEAT_FAN_SPEED;
|
||||
int absPreheatHotendTemp = ABS_PREHEAT_HOTEND_TEMP, absPreheatHPBTemp = ABS_PREHEAT_HPB_TEMP, absPreheatFanSpeed = ABS_PREHEAT_FAN_SPEED;
|
||||
@ -99,9 +93,6 @@ void Config_StoreSettings()
|
||||
int lcd_contrast = 32;
|
||||
#endif
|
||||
EEPROM_WRITE_VAR(i,lcd_contrast);
|
||||
#ifdef SCARA
|
||||
EEPROM_WRITE_VAR(i,axis_scaling); // Add scaling for SCARA
|
||||
#endif
|
||||
#ifdef FWRETRACT
|
||||
EEPROM_WRITE_VAR(i,autoretract_enabled);
|
||||
EEPROM_WRITE_VAR(i,retract_length);
|
||||
@ -149,16 +140,6 @@ void Config_PrintSettings()
|
||||
SERIAL_ECHOLN("");
|
||||
|
||||
SERIAL_ECHO_START;
|
||||
#ifdef SCARA
|
||||
SERIAL_ECHOLNPGM("Scaling factors:");
|
||||
SERIAL_ECHO_START;
|
||||
SERIAL_ECHOPAIR(" M365 X",axis_scaling[X_AXIS]);
|
||||
SERIAL_ECHOPAIR(" Y",axis_scaling[Y_AXIS]);
|
||||
SERIAL_ECHOPAIR(" Z",axis_scaling[Z_AXIS]);
|
||||
SERIAL_ECHOLN("");
|
||||
|
||||
SERIAL_ECHO_START;
|
||||
#endif
|
||||
SERIAL_ECHOLNPGM("Maximum feedrates (mm/s):");
|
||||
SERIAL_ECHO_START;
|
||||
SERIAL_ECHOPAIR(" M203 X", max_feedrate[X_AXIS]);
|
||||
@ -200,22 +181,6 @@ SERIAL_ECHOLNPGM("Scaling factors:");
|
||||
SERIAL_ECHOPAIR(" Y" ,add_homing[Y_AXIS] );
|
||||
SERIAL_ECHOPAIR(" Z" ,add_homing[Z_AXIS] );
|
||||
SERIAL_ECHOLN("");
|
||||
#ifdef DELTA
|
||||
SERIAL_ECHO_START;
|
||||
SERIAL_ECHOLNPGM("Endstop adjustement (mm):");
|
||||
SERIAL_ECHO_START;
|
||||
SERIAL_ECHOPAIR(" M666 X",endstop_adj[X_AXIS] );
|
||||
SERIAL_ECHOPAIR(" Y" ,endstop_adj[Y_AXIS] );
|
||||
SERIAL_ECHOPAIR(" Z" ,endstop_adj[Z_AXIS] );
|
||||
SERIAL_ECHOLN("");
|
||||
SERIAL_ECHO_START;
|
||||
SERIAL_ECHOLNPGM("Delta settings: L=delta_diagonal_rod, R=delta_radius, S=delta_segments_per_second");
|
||||
SERIAL_ECHO_START;
|
||||
SERIAL_ECHOPAIR(" M665 L",delta_diagonal_rod );
|
||||
SERIAL_ECHOPAIR(" R" ,delta_radius );
|
||||
SERIAL_ECHOPAIR(" S" ,delta_segments_per_second );
|
||||
SERIAL_ECHOLN("");
|
||||
#endif
|
||||
#ifdef PIDTEMP
|
||||
SERIAL_ECHO_START;
|
||||
SERIAL_ECHOLNPGM("PID settings:");
|
||||
@ -305,12 +270,6 @@ void Config_RetrieveSettings()
|
||||
EEPROM_READ_VAR(i,max_z_jerk);
|
||||
EEPROM_READ_VAR(i,max_e_jerk);
|
||||
EEPROM_READ_VAR(i,add_homing);
|
||||
#ifdef DELTA
|
||||
EEPROM_READ_VAR(i,endstop_adj);
|
||||
EEPROM_READ_VAR(i,delta_radius);
|
||||
EEPROM_READ_VAR(i,delta_diagonal_rod);
|
||||
EEPROM_READ_VAR(i,delta_segments_per_second);
|
||||
#endif
|
||||
#ifndef ULTIPANEL
|
||||
int plaPreheatHotendTemp, plaPreheatHPBTemp, plaPreheatFanSpeed;
|
||||
int absPreheatHotendTemp, absPreheatHPBTemp, absPreheatFanSpeed;
|
||||
@ -337,9 +296,6 @@ void Config_RetrieveSettings()
|
||||
int lcd_contrast;
|
||||
#endif
|
||||
EEPROM_READ_VAR(i,lcd_contrast);
|
||||
#ifdef SCARA
|
||||
EEPROM_READ_VAR(i,axis_scaling);
|
||||
#endif
|
||||
|
||||
#ifdef FWRETRACT
|
||||
EEPROM_READ_VAR(i,autoretract_enabled);
|
||||
@ -390,9 +346,6 @@ void Config_ResetDefault()
|
||||
axis_steps_per_unit[i]=tmp1[i];
|
||||
max_feedrate[i]=tmp2[i];
|
||||
max_acceleration_units_per_sq_second[i]=tmp3[i];
|
||||
#ifdef SCARA
|
||||
axis_scaling[i]=1;
|
||||
#endif
|
||||
}
|
||||
|
||||
// steps per sq second need to be updated to agree with the units per sq second
|
||||
@ -407,13 +360,6 @@ void Config_ResetDefault()
|
||||
max_z_jerk=DEFAULT_ZJERK;
|
||||
max_e_jerk=DEFAULT_EJERK;
|
||||
add_homing[X_AXIS] = add_homing[Y_AXIS] = add_homing[Z_AXIS] = 0;
|
||||
#ifdef DELTA
|
||||
endstop_adj[X_AXIS] = endstop_adj[Y_AXIS] = endstop_adj[Z_AXIS] = 0;
|
||||
delta_radius= DELTA_RADIUS;
|
||||
delta_diagonal_rod= DELTA_DIAGONAL_ROD;
|
||||
delta_segments_per_second= DELTA_SEGMENTS_PER_SECOND;
|
||||
recalc_delta_settings(delta_radius, delta_diagonal_rod);
|
||||
#endif
|
||||
#ifdef ULTIPANEL
|
||||
plaPreheatHotendTemp = PLA_PREHEAT_HOTEND_TEMP;
|
||||
plaPreheatHPBTemp = PLA_PREHEAT_HPB_TEMP;
|
||||
|
||||
@ -324,12 +324,6 @@
|
||||
#ifdef COREXY
|
||||
#error BABYSTEPPING not implemented for COREXY yet.
|
||||
#endif
|
||||
|
||||
#ifdef DELTA
|
||||
#ifdef BABYSTEP_XY
|
||||
#error BABYSTEPPING only implemented for Z axis on deltabots.
|
||||
#endif
|
||||
#endif
|
||||
#endif
|
||||
|
||||
// extruder advance constant (s2/mm3)
|
||||
@ -435,10 +429,6 @@ const unsigned int dropsegments=5; //everything with less than this number of st
|
||||
//============================= Define Defines ============================
|
||||
//===========================================================================
|
||||
|
||||
#if defined (ENABLE_AUTO_BED_LEVELING) && defined (DELTA)
|
||||
#error "Bed Auto Leveling is still not compatible with Delta Kinematics."
|
||||
#endif
|
||||
|
||||
#if EXTRUDERS > 1 && defined TEMP_SENSOR_1_AS_REDUNDANT
|
||||
#error "You cannot use TEMP_SENSOR_1_AS_REDUNDANT if EXTRUDERS > 1"
|
||||
#endif
|
||||
|
||||
@ -211,22 +211,21 @@ void FlushSerialRequestResend();
|
||||
void ClearToSend();
|
||||
|
||||
void get_coordinates();
|
||||
#ifdef DELTA
|
||||
void calculate_delta(float cartesian[3]);
|
||||
extern float delta[3];
|
||||
#endif
|
||||
#ifdef SCARA
|
||||
void calculate_delta(float cartesian[3]);
|
||||
void calculate_SCARA_forward_Transform(float f_scara[3]);
|
||||
#endif
|
||||
void prepare_move();
|
||||
void kill();
|
||||
void Stop();
|
||||
|
||||
bool IsStopped();
|
||||
|
||||
void enquecommand(const char *cmd); //put an ASCII command at the end of the current buffer.
|
||||
void enquecommand_P(const char *cmd); //put an ASCII command at the end of the current buffer, read from flash
|
||||
//put an ASCII command at the end of the current buffer.
|
||||
void enquecommand(const char *cmd, bool from_progmem = false);
|
||||
//put an ASCII command at the end of the current buffer, read from flash
|
||||
#define enquecommand_P(cmd) enquecommand(cmd, true)
|
||||
void enquecommand_front(const char *cmd, bool from_progmem = false);
|
||||
//put an ASCII command at the end of the current buffer, read from flash
|
||||
#define enquecommand_P(cmd) enquecommand(cmd, true)
|
||||
#define enquecommand_front_P(cmd) enquecommand_front(cmd, true)
|
||||
|
||||
void prepare_arc_move(char isclockwise);
|
||||
void clamp_to_software_endstops(float target[3]);
|
||||
|
||||
@ -252,25 +251,11 @@ extern float volumetric_multiplier[EXTRUDERS]; // reciprocal of cross-sectional
|
||||
extern float current_position[NUM_AXIS] ;
|
||||
extern float destination[NUM_AXIS] ;
|
||||
extern float add_homing[3];
|
||||
#ifdef DELTA
|
||||
extern float endstop_adj[3];
|
||||
extern float delta_radius;
|
||||
extern float delta_diagonal_rod;
|
||||
extern float delta_segments_per_second;
|
||||
void recalc_delta_settings(float radius, float diagonal_rod);
|
||||
#endif
|
||||
#ifdef SCARA
|
||||
extern float axis_scaling[3]; // Build size scaling
|
||||
#endif
|
||||
extern float min_pos[3];
|
||||
extern float max_pos[3];
|
||||
extern bool axis_known_position[3];
|
||||
extern float zprobe_zoffset;
|
||||
extern int fanSpeed;
|
||||
#ifdef BARICUDA
|
||||
extern int ValvePressure;
|
||||
extern int EtoPPressure;
|
||||
#endif
|
||||
|
||||
#ifdef FAN_SOFT_PWM
|
||||
extern unsigned char fanSpeedSoftPwm;
|
||||
|
||||
@ -53,8 +53,16 @@ FORCE_INLINE void store_char(unsigned char c)
|
||||
//SIGNAL(SIG_USART_RECV)
|
||||
SIGNAL(M_USARTx_RX_vect)
|
||||
{
|
||||
unsigned char c = M_UDRx;
|
||||
store_char(c);
|
||||
// Test for a framing error.
|
||||
if (M_UCSRxA & (1<<M_FEx)) {
|
||||
// Characters received with the framing errors will be ignored.
|
||||
// The temporary variable "c" was made volatile, so the compiler does not optimize this out.
|
||||
volatile unsigned char c = M_UDRx;
|
||||
} else {
|
||||
// Read the input register.
|
||||
unsigned char c = M_UDRx;
|
||||
store_char(c);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
@ -53,6 +53,7 @@
|
||||
#define M_UBRRxH SERIAL_REGNAME(UBRR,SERIAL_PORT,H)
|
||||
#define M_UBRRxL SERIAL_REGNAME(UBRR,SERIAL_PORT,L)
|
||||
#define M_RXCx SERIAL_REGNAME(RXC,SERIAL_PORT,)
|
||||
#define M_FEx SERIAL_REGNAME(FE,SERIAL_PORT,)
|
||||
#define M_USARTx_RX_vect SERIAL_REGNAME(USART,SERIAL_PORT,_RX_vect)
|
||||
#define M_U2Xx SERIAL_REGNAME(U2X,SERIAL_PORT,)
|
||||
|
||||
@ -111,19 +112,25 @@ class MarlinSerial //: public Stream
|
||||
|
||||
FORCE_INLINE void checkRx(void)
|
||||
{
|
||||
if((M_UCSRxA & (1<<M_RXCx)) != 0) {
|
||||
unsigned char c = M_UDRx;
|
||||
int i = (unsigned int)(rx_buffer.head + 1) % RX_BUFFER_SIZE;
|
||||
|
||||
// if we should be storing the received character into the location
|
||||
// just before the tail (meaning that the head would advance to the
|
||||
// current location of the tail), we're about to overflow the buffer
|
||||
// and so we don't write the character or advance the head.
|
||||
if (i != rx_buffer.tail) {
|
||||
rx_buffer.buffer[rx_buffer.head] = c;
|
||||
rx_buffer.head = i;
|
||||
if((M_UCSRxA & (1<<M_RXCx)) != 0) {
|
||||
// Test for a framing error.
|
||||
if (M_UCSRxA & (1<<M_FEx)) {
|
||||
// Characters received with the framing errors will be ignored.
|
||||
// The temporary variable "c" was made volatile, so the compiler does not optimize this out.
|
||||
volatile unsigned char c = M_UDRx;
|
||||
} else {
|
||||
unsigned char c = M_UDRx;
|
||||
int i = (unsigned int)(rx_buffer.head + 1) % RX_BUFFER_SIZE;
|
||||
// if we should be storing the received character into the location
|
||||
// just before the tail (meaning that the head would advance to the
|
||||
// current location of the tail), we're about to overflow the buffer
|
||||
// and so we don't write the character or advance the head.
|
||||
if (i != rx_buffer.tail) {
|
||||
rx_buffer.buffer[rx_buffer.head] = c;
|
||||
rx_buffer.head = i;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -539,9 +539,10 @@ void CardReader::checkautostart(bool force)
|
||||
if(strncmp((char*)p.name,autoname,5)==0)
|
||||
{
|
||||
char cmd[30];
|
||||
|
||||
// M23: Select SD file
|
||||
sprintf_P(cmd, PSTR("M23 %s"), autoname);
|
||||
enquecommand(cmd);
|
||||
// M24: Start/resume SD print
|
||||
enquecommand_P(PSTR("M24"));
|
||||
found=true;
|
||||
}
|
||||
|
||||
@ -9,6 +9,420 @@
|
||||
|
||||
extern float home_retract_mm_ext(int axis);
|
||||
|
||||
float world2machine_rotation_and_skew[2][2];
|
||||
float world2machine_shift[2];
|
||||
|
||||
#define BED_ZERO_REF_X (- 22.f + X_PROBE_OFFSET_FROM_EXTRUDER)
|
||||
#define BED_ZERO_REF_Y (- 0.6f + Y_PROBE_OFFSET_FROM_EXTRUDER)
|
||||
|
||||
// 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 = {
|
||||
13.f - BED_ZERO_REF_X, 6.4f - BED_ZERO_REF_Y,
|
||||
115.f - BED_ZERO_REF_X, 6.4f - BED_ZERO_REF_Y,
|
||||
216.f - BED_ZERO_REF_X, 6.4f - BED_ZERO_REF_Y,
|
||||
|
||||
216.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y,
|
||||
115.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y,
|
||||
13.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y,
|
||||
|
||||
13.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y,
|
||||
115.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y,
|
||||
216.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y
|
||||
};
|
||||
|
||||
// Positions of the bed reference points in the machine coordinates, referenced to the P.I.N.D.A sensor.
|
||||
// The points are the following: center front, center right, center rear, center left.
|
||||
const float bed_ref_points_4[] PROGMEM = {
|
||||
115.f - BED_ZERO_REF_X, 6.4f - BED_ZERO_REF_Y,
|
||||
216.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y,
|
||||
115.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y,
|
||||
13.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y
|
||||
};
|
||||
|
||||
static inline float sqr(float x) { return x * x; }
|
||||
|
||||
bool calculate_machine_skew_and_offset_LS(
|
||||
// Matrix of maximum 9 2D points (18 floats)
|
||||
const float *measured_pts,
|
||||
uint8_t npts,
|
||||
const float *true_pts,
|
||||
// Resulting correction matrix.
|
||||
float *vec_x,
|
||||
float *vec_y,
|
||||
float *cntr
|
||||
// Temporary values, 49-18-(2*3)=25 floats
|
||||
// , float *temp
|
||||
)
|
||||
{
|
||||
SERIAL_ECHOPGM("X vector, initial: ");
|
||||
MYSERIAL.print(vec_x[0], 5);
|
||||
SERIAL_ECHOPGM(", ");
|
||||
MYSERIAL.print(vec_x[1], 5);
|
||||
SERIAL_ECHOLNPGM("");
|
||||
|
||||
SERIAL_ECHOPGM("Y vector, initial: ");
|
||||
MYSERIAL.print(vec_y[0], 5);
|
||||
SERIAL_ECHOPGM(", ");
|
||||
MYSERIAL.print(vec_y[1], 5);
|
||||
SERIAL_ECHOLNPGM("");
|
||||
|
||||
SERIAL_ECHOPGM("center, initial: ");
|
||||
MYSERIAL.print(cntr[0], 5);
|
||||
SERIAL_ECHOPGM(", ");
|
||||
MYSERIAL.print(cntr[1], 5);
|
||||
SERIAL_ECHOLNPGM("");
|
||||
|
||||
for (uint8_t i = 0; i < npts; ++ i) {
|
||||
SERIAL_ECHOPGM("point #");
|
||||
MYSERIAL.print(int(i));
|
||||
SERIAL_ECHOPGM(" measured: (");
|
||||
MYSERIAL.print(measured_pts[i*2], 5);
|
||||
SERIAL_ECHOPGM(", ");
|
||||
MYSERIAL.print(measured_pts[i*2+1], 5);
|
||||
SERIAL_ECHOPGM("); target: (");
|
||||
MYSERIAL.print(pgm_read_float(true_pts+i*2 ), 5);
|
||||
SERIAL_ECHOPGM(", ");
|
||||
MYSERIAL.print(pgm_read_float(true_pts+i*2+1), 5);
|
||||
SERIAL_ECHOPGM("), error: ");
|
||||
MYSERIAL.print(sqrt(
|
||||
sqr(pgm_read_float(true_pts+i*2 ) - measured_pts[i*2 ]) +
|
||||
sqr(pgm_read_float(true_pts+i*2+1) - measured_pts[i*2+1])), 5);
|
||||
SERIAL_ECHOLNPGM("");
|
||||
}
|
||||
delay_keep_alive(100);
|
||||
|
||||
|
||||
{
|
||||
// Create covariance matrix for A, collect the right hand side b.
|
||||
float A[3][3] = { 0.f };
|
||||
float b[3] = { 0.f };
|
||||
float acc;
|
||||
for (uint8_t r = 0; r < 3; ++ r) {
|
||||
for (uint8_t c = 0; c < 3; ++ c) {
|
||||
acc = 0;
|
||||
for (uint8_t i = 0; i < npts; ++ i) {
|
||||
float a = (r == 2) ? 1.f : measured_pts[2 * i + r];
|
||||
float b = (c == 2) ? 1.f : measured_pts[2 * i + c];
|
||||
acc += a * b;
|
||||
}
|
||||
A[r][c] = acc;
|
||||
}
|
||||
acc = 0.f;
|
||||
for (uint8_t i = 0; i < npts; ++ i) {
|
||||
float a = (r == 2) ? 1.f : measured_pts[2 * i + r];
|
||||
float b = pgm_read_float(true_pts+i*2);
|
||||
acc += a * b;
|
||||
}
|
||||
b[r] = acc;
|
||||
}
|
||||
// Solve the linear equation for ax, bx, cx.
|
||||
float x[3] = { 0.f };
|
||||
for (uint8_t iter = 0; iter < 100; ++ iter) {
|
||||
x[0] = (b[0] - A[0][1] * x[1] - A[0][2] * x[2]) / A[0][0];
|
||||
x[1] = (b[1] - A[1][0] * x[0] - A[1][2] * x[2]) / A[1][1];
|
||||
x[2] = (b[2] - A[2][0] * x[0] - A[2][1] * x[1]) / A[2][2];
|
||||
}
|
||||
// Store the result to the output variables.
|
||||
vec_x[0] = x[0];
|
||||
vec_y[0] = x[1];
|
||||
cntr[0] = x[2];
|
||||
|
||||
// Recalculate A and b for the y values.
|
||||
// Note the weighting of the first row of values.
|
||||
// const float weight_1st_row = 0.5f;
|
||||
const float weight_1st_row = 0.2f;
|
||||
for (uint8_t r = 0; r < 3; ++ r) {
|
||||
for (uint8_t c = 0; c < 3; ++ c) {
|
||||
acc = 0;
|
||||
for (uint8_t i = 0; i < npts; ++ i) {
|
||||
float w = (i < 3) ? weight_1st_row : 1.f;
|
||||
float a = (r == 2) ? 1.f : measured_pts[2 * i + r];
|
||||
float b = (c == 2) ? 1.f : measured_pts[2 * i + c];
|
||||
acc += a * b * w;
|
||||
}
|
||||
A[r][c] = acc;
|
||||
}
|
||||
acc = 0.f;
|
||||
for (uint8_t i = 0; i < npts; ++ i) {
|
||||
float w = (i < 3) ? weight_1st_row : 1.f;
|
||||
float a = (r == 2) ? 1.f : measured_pts[2 * i + r];
|
||||
float b = pgm_read_float(true_pts+i*2+1);
|
||||
acc += w * a * b;
|
||||
}
|
||||
b[r] = acc;
|
||||
}
|
||||
// Solve the linear equation for ay, by, cy.
|
||||
x[0] = 0.f, x[1] = 0.f; x[2] = 0.f;
|
||||
for (uint8_t iter = 0; iter < 100; ++ iter) {
|
||||
x[0] = (b[0] - A[0][1] * x[1] - A[0][2] * x[2]) / A[0][0];
|
||||
x[1] = (b[1] - A[1][0] * x[0] - A[1][2] * x[2]) / A[1][1];
|
||||
x[2] = (b[2] - A[2][0] * x[0] - A[2][1] * x[1]) / A[2][2];
|
||||
}
|
||||
// Store the result to the output variables.
|
||||
vec_x[1] = x[0];
|
||||
vec_y[1] = x[1];
|
||||
cntr[1] = x[2];
|
||||
}
|
||||
|
||||
SERIAL_ECHOLNPGM("Error after correction: ");
|
||||
for (uint8_t i = 0; i < npts; ++ i) {
|
||||
float x = vec_x[0] * measured_pts[i*2] + vec_y[0] * measured_pts[i*2+1] + cntr[0];
|
||||
float y = vec_x[1] * measured_pts[i*2] + vec_y[1] * measured_pts[i*2+1] + cntr[1];
|
||||
SERIAL_ECHOPGM("point #");
|
||||
MYSERIAL.print(int(i));
|
||||
SERIAL_ECHOPGM(" measured: (");
|
||||
MYSERIAL.print(measured_pts[i*2], 5);
|
||||
SERIAL_ECHOPGM(", ");
|
||||
MYSERIAL.print(measured_pts[i*2+1], 5);
|
||||
SERIAL_ECHOPGM("); corrected: (");
|
||||
MYSERIAL.print(x, 5);
|
||||
SERIAL_ECHOPGM(", ");
|
||||
MYSERIAL.print(y, 5);
|
||||
SERIAL_ECHOPGM("); target: (");
|
||||
MYSERIAL.print(pgm_read_float(true_pts+i*2 ), 5);
|
||||
SERIAL_ECHOPGM(", ");
|
||||
MYSERIAL.print(pgm_read_float(true_pts+i*2+1), 5);
|
||||
SERIAL_ECHOPGM("), error: ");
|
||||
MYSERIAL.print(sqrt(sqr(pgm_read_float(true_pts+i*2)-x)+sqr(pgm_read_float(true_pts+i*2+1)-y)));
|
||||
SERIAL_ECHOLNPGM("");
|
||||
}
|
||||
|
||||
SERIAL_ECHOPGM("X vector new, inverted: ");
|
||||
MYSERIAL.print(vec_x[0], 5);
|
||||
SERIAL_ECHOPGM(", ");
|
||||
MYSERIAL.print(vec_x[1], 5);
|
||||
SERIAL_ECHOLNPGM("");
|
||||
|
||||
SERIAL_ECHOPGM("Y vector new, inverted: ");
|
||||
MYSERIAL.print(vec_y[0], 5);
|
||||
SERIAL_ECHOPGM(", ");
|
||||
MYSERIAL.print(vec_y[1], 5);
|
||||
SERIAL_ECHOLNPGM("");
|
||||
|
||||
SERIAL_ECHOPGM("center new, inverted: ");
|
||||
MYSERIAL.print(cntr[0], 5);
|
||||
SERIAL_ECHOPGM(", ");
|
||||
MYSERIAL.print(cntr[1], 5);
|
||||
SERIAL_ECHOLNPGM("");
|
||||
delay_keep_alive(100);
|
||||
|
||||
#if 0
|
||||
// Normalize the vectors. We expect, that the machine axes may be skewed a bit, but the distances are correct.
|
||||
// l shall be very close to 1 already.
|
||||
float l = sqrt(vec_x[0]*vec_x[0] + vec_x[1] * vec_x[1]);
|
||||
vec_x[0] /= l;
|
||||
vec_x[1] /= l;
|
||||
SERIAL_ECHOPGM("Length of the X vector: ");
|
||||
MYSERIAL.print(l, 5);
|
||||
SERIAL_ECHOLNPGM("");
|
||||
l = sqrt(vec_y[0]*vec_y[0] + vec_y[1] * vec_y[1]);
|
||||
vec_y[0] /= l;
|
||||
vec_y[1] /= l;
|
||||
SERIAL_ECHOPGM("Length of the Y vector: ");
|
||||
MYSERIAL.print(l, 5);
|
||||
SERIAL_ECHOLNPGM("");
|
||||
|
||||
// Recalculate the center using the adjusted vec_x/vec_y
|
||||
{
|
||||
cntr[0] = 0.f;
|
||||
cntr[1] = 0.f;
|
||||
for (uint8_t i = 0; i < npts; ++ i) {
|
||||
cntr[0] += measured_pts[2 * i ] - pgm_read_float(true_pts+i*2) * vec_x[0] - pgm_read_float(true_pts+i*2+1) * vec_y[0];
|
||||
cntr[1] += measured_pts[2 * i + 1] - pgm_read_float(true_pts+i*2) * vec_x[1] - pgm_read_float(true_pts+i*2+1) * vec_y[1];
|
||||
}
|
||||
cntr[0] /= float(npts);
|
||||
cntr[1] /= float(npts);
|
||||
}
|
||||
|
||||
SERIAL_ECHOPGM("X vector new, inverted, normalized: ");
|
||||
MYSERIAL.print(vec_x[0], 5);
|
||||
SERIAL_ECHOPGM(", ");
|
||||
MYSERIAL.print(vec_x[1], 5);
|
||||
SERIAL_ECHOLNPGM("");
|
||||
|
||||
SERIAL_ECHOPGM("Y vector new, inverted, normalized: ");
|
||||
MYSERIAL.print(vec_y[0], 5);
|
||||
SERIAL_ECHOPGM(", ");
|
||||
MYSERIAL.print(vec_y[1], 5);
|
||||
SERIAL_ECHOLNPGM("");
|
||||
|
||||
SERIAL_ECHOPGM("center new, inverted, normalized: ");
|
||||
MYSERIAL.print(cntr[0], 5);
|
||||
SERIAL_ECHOPGM(", ");
|
||||
MYSERIAL.print(cntr[1], 5);
|
||||
SERIAL_ECHOLNPGM("");
|
||||
#endif
|
||||
|
||||
// Invert the transformation matrix made of vec_x, vec_y and cntr.
|
||||
{
|
||||
float d = vec_x[0] * vec_y[1] - vec_x[1] * vec_y[0];
|
||||
float Ainv[2][2] = {
|
||||
{ vec_y[1] / d, - vec_y[0] / d },
|
||||
{ - vec_x[1] / d, vec_x[0] / d }
|
||||
};
|
||||
float cntrInv[2] = {
|
||||
- Ainv[0][0] * cntr[0] - Ainv[0][1] * cntr[1],
|
||||
- Ainv[1][0] * cntr[0] - Ainv[1][1] * cntr[1]
|
||||
};
|
||||
vec_x[0] = Ainv[0][0];
|
||||
vec_x[1] = Ainv[1][0];
|
||||
vec_y[0] = Ainv[0][1];
|
||||
vec_y[1] = Ainv[1][1];
|
||||
cntr[0] = cntrInv[0];
|
||||
cntr[1] = cntrInv[1];
|
||||
}
|
||||
|
||||
SERIAL_ECHOPGM("X vector, adjusted: ");
|
||||
MYSERIAL.print(vec_x[0], 5);
|
||||
SERIAL_ECHOPGM(", ");
|
||||
MYSERIAL.print(vec_x[1], 5);
|
||||
SERIAL_ECHOLNPGM("");
|
||||
|
||||
SERIAL_ECHOPGM("Y vector, adjusted: ");
|
||||
MYSERIAL.print(vec_y[0], 5);
|
||||
SERIAL_ECHOPGM(", ");
|
||||
MYSERIAL.print(vec_y[1], 5);
|
||||
SERIAL_ECHOLNPGM("");
|
||||
|
||||
SERIAL_ECHOPGM("center, adjusted: ");
|
||||
MYSERIAL.print(cntr[0], 5);
|
||||
SERIAL_ECHOPGM(", ");
|
||||
MYSERIAL.print(cntr[1], 5);
|
||||
SERIAL_ECHOLNPGM("");
|
||||
|
||||
SERIAL_ECHOLNPGM("Difference after correction: ");
|
||||
for (uint8_t i = 0; i < npts; ++ i) {
|
||||
float x = vec_x[0] * pgm_read_float(true_pts+i*2) + vec_y[0] * pgm_read_float(true_pts+i*2+1) + cntr[0];
|
||||
float y = vec_x[1] * pgm_read_float(true_pts+i*2) + vec_y[1] * pgm_read_float(true_pts+i*2+1) + cntr[1];
|
||||
SERIAL_ECHOPGM("point #");
|
||||
MYSERIAL.print(int(i));
|
||||
SERIAL_ECHOPGM("measured: (");
|
||||
MYSERIAL.print(measured_pts[i*2], 5);
|
||||
SERIAL_ECHOPGM(", ");
|
||||
MYSERIAL.print(measured_pts[i*2+1], 5);
|
||||
SERIAL_ECHOPGM("); measured-corrected: (");
|
||||
MYSERIAL.print(x, 5);
|
||||
SERIAL_ECHOPGM(", ");
|
||||
MYSERIAL.print(y, 5);
|
||||
SERIAL_ECHOPGM("); target: (");
|
||||
MYSERIAL.print(pgm_read_float(true_pts+i*2 ), 5);
|
||||
SERIAL_ECHOPGM(", ");
|
||||
MYSERIAL.print(pgm_read_float(true_pts+i*2+1), 5);
|
||||
SERIAL_ECHOPGM("), error: ");
|
||||
MYSERIAL.print(sqrt(sqr(measured_pts[i*2]-x)+sqr(measured_pts[i*2+1]-y)));
|
||||
SERIAL_ECHOLNPGM("");
|
||||
}
|
||||
delay_keep_alive(100);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void reset_bed_offset_and_skew()
|
||||
{
|
||||
eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_CENTER+0), 0x0FFFFFFFF);
|
||||
eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_CENTER+4), 0x0FFFFFFFF);
|
||||
eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_X +0), 0x0FFFFFFFF);
|
||||
eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_X +4), 0x0FFFFFFFF);
|
||||
eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_Y +0), 0x0FFFFFFFF);
|
||||
eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_Y +4), 0x0FFFFFFFF);
|
||||
}
|
||||
|
||||
void world2machine_reset()
|
||||
{
|
||||
// Identity transformation.
|
||||
world2machine_rotation_and_skew[0][0] = 1.f;
|
||||
world2machine_rotation_and_skew[0][1] = 0.f;
|
||||
world2machine_rotation_and_skew[1][0] = 0.f;
|
||||
world2machine_rotation_and_skew[1][1] = 1.f;
|
||||
// Zero shift.
|
||||
world2machine_shift[0] = 0.f;
|
||||
world2machine_shift[1] = 0.f;
|
||||
}
|
||||
|
||||
static inline bool vec_undef(const float v[2])
|
||||
{
|
||||
const uint32_t *vx = (const uint32_t*)v;
|
||||
return vx[0] == 0x0FFFFFFFF || vx[1] == 0x0FFFFFFFF;
|
||||
}
|
||||
|
||||
void world2machine_initialize()
|
||||
{
|
||||
float cntr[2] = {
|
||||
eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_CENTER+0)),
|
||||
eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_CENTER+4))
|
||||
};
|
||||
float vec_x[2] = {
|
||||
eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +0)),
|
||||
eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +4))
|
||||
};
|
||||
float vec_y[2] = {
|
||||
eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +0)),
|
||||
eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +4))
|
||||
};
|
||||
|
||||
bool reset = false;
|
||||
if (vec_undef(cntr) || vec_undef(vec_x) || vec_undef(vec_y))
|
||||
reset = true;
|
||||
else {
|
||||
// Length of the vec_x shall be close to unity.
|
||||
float l = sqrt(vec_x[0] * vec_x[0] + vec_x[1] * vec_x[1]);
|
||||
if (l < 0.9 || l > 1.1) {
|
||||
SERIAL_ECHOLNPGM("Invalid bed correction matrix. Length of the X vector out of range.");
|
||||
reset = true;
|
||||
}
|
||||
// Length of the vec_y shall be close to unity.
|
||||
l = sqrt(vec_y[0] * vec_y[0] + vec_y[1] * vec_y[1]);
|
||||
if (l < 0.9 || l > 1.1) {
|
||||
SERIAL_ECHOLNPGM("Invalid bed correction matrix. Length of the X vector out of range.");
|
||||
reset = true;
|
||||
}
|
||||
// Correction of the zero point shall be reasonably small.
|
||||
l = sqrt(cntr[0] * cntr[0] + cntr[1] * cntr[1]);
|
||||
if (l > 15.f) {
|
||||
SERIAL_ECHOLNPGM("Invalid bed correction matrix. Shift out of range.");
|
||||
reset = true;
|
||||
}
|
||||
// vec_x and vec_y shall be nearly perpendicular.
|
||||
l = vec_x[0] * vec_y[0] + vec_x[1] * vec_y[1];
|
||||
if (fabs(l) > 0.1f) {
|
||||
SERIAL_ECHOLNPGM("Invalid bed correction matrix. X/Y axes are far from being perpendicular.");
|
||||
reset = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (reset) {
|
||||
SERIAL_ECHOLNPGM("Invalid bed correction matrix. Resetting to identity.");
|
||||
reset_bed_offset_and_skew();
|
||||
world2machine_reset();
|
||||
} else {
|
||||
world2machine_rotation_and_skew[0][0] = vec_x[0];
|
||||
world2machine_rotation_and_skew[1][0] = vec_x[1];
|
||||
world2machine_rotation_and_skew[0][1] = vec_y[0];
|
||||
world2machine_rotation_and_skew[1][1] = vec_y[1];
|
||||
world2machine_shift[0] = cntr[0];
|
||||
world2machine_shift[1] = cntr[1];
|
||||
}
|
||||
}
|
||||
|
||||
// When switching from absolute to corrected coordinates,
|
||||
// this will get the absolute coordinates from the servos,
|
||||
// applies the inverse world2machine transformation
|
||||
// and stores the result into current_position[x,y].
|
||||
void world2machine_update_current()
|
||||
{
|
||||
// Invert the transformation matrix made of vec_x, vec_y and cntr.
|
||||
float d = world2machine_rotation_and_skew[0][0] * world2machine_rotation_and_skew[1][1] - world2machine_rotation_and_skew[1][0] * world2machine_rotation_and_skew[0][1];
|
||||
float Ainv[2][2] = {
|
||||
{ world2machine_rotation_and_skew[1][1] / d, - world2machine_rotation_and_skew[0][1] / d },
|
||||
{ - world2machine_rotation_and_skew[1][0] / d, world2machine_rotation_and_skew[0][0] / d }
|
||||
};
|
||||
float x = current_position[X_AXIS] - world2machine_shift[0];
|
||||
float y = current_position[Y_AXIS] - world2machine_shift[1];
|
||||
current_position[X_AXIS] = Ainv[0][0] * x + Ainv[0][1] * y;
|
||||
current_position[Y_AXIS] = Ainv[1][0] * x + Ainv[1][1] * y;
|
||||
}
|
||||
|
||||
static inline void go_xyz(float x, float y, float z, float fr)
|
||||
{
|
||||
plan_buffer_line(x, y, z, current_position[E_AXIS], fr, active_extruder);
|
||||
@ -35,6 +449,12 @@ static inline void update_current_position_xyz()
|
||||
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
|
||||
}
|
||||
|
||||
static inline void update_current_position_z()
|
||||
{
|
||||
current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
|
||||
plan_set_z_position(current_position[Z_AXIS]);
|
||||
}
|
||||
|
||||
// At the current position, find the Z stop.
|
||||
inline void find_bed_induction_sensor_point_z()
|
||||
{
|
||||
@ -45,7 +465,7 @@ inline void find_bed_induction_sensor_point_z()
|
||||
current_position[Z_AXIS] = -10;
|
||||
go_to_current(homing_feedrate[Z_AXIS]/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_xyz();
|
||||
update_current_position_z();
|
||||
|
||||
// move up the retract distance
|
||||
current_position[Z_AXIS] += home_retract_mm_ext(Z_AXIS);
|
||||
@ -55,7 +475,7 @@ inline void find_bed_induction_sensor_point_z()
|
||||
current_position[Z_AXIS] -= home_retract_mm_ext(Z_AXIS) * 2;
|
||||
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_xyz();
|
||||
update_current_position_z();
|
||||
|
||||
enable_endstops(endstops_enabled);
|
||||
enable_z_endstop(endstop_z_enabled);
|
||||
@ -317,6 +737,7 @@ inline bool improve_bed_induction_sensor_point()
|
||||
enable_endstops(true);
|
||||
go_xy(center_old_x, center_old_y, feedrate);
|
||||
update_current_position_xyz();
|
||||
// if (! endstop_z_hit_on_purpose()) return false;
|
||||
center_x += current_position[X_AXIS];
|
||||
center_y += current_position[Y_AXIS];
|
||||
}
|
||||
@ -360,16 +781,20 @@ inline bool improve_bed_induction_sensor_point2(bool lift_z_on_min_y)
|
||||
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())
|
||||
return false;
|
||||
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())
|
||||
return false;
|
||||
if (! endstop_z_hit_on_purpose()) {
|
||||
current_position[X_AXIS] = center_old_x;
|
||||
goto canceled;
|
||||
}
|
||||
b = current_position[X_AXIS];
|
||||
|
||||
// Go to the center.
|
||||
@ -392,20 +817,23 @@ inline bool improve_bed_induction_sensor_point2(bool lift_z_on_min_y)
|
||||
if (lift_z_on_min_y) {
|
||||
// The first row of points are very close to the end stop.
|
||||
// Lift the sensor to disengage the trigger. This is necessary because of the sensor hysteresis.
|
||||
go_xyz(current_position[X_AXIS], y0, current_position[Z_AXIS]+5.f, homing_feedrate[Z_AXIS] / 60.f);
|
||||
go_xyz(current_position[X_AXIS], y0, current_position[Z_AXIS]+1.5f, homing_feedrate[Z_AXIS] / 60.f);
|
||||
// and go back.
|
||||
go_xyz(current_position[X_AXIS], y0, current_position[Z_AXIS], homing_feedrate[Z_AXIS] / 60.f);
|
||||
}
|
||||
if (lift_z_on_min_y && (READ(Z_MIN_PIN) ^ Z_MIN_ENDSTOP_INVERTING) == 1) {
|
||||
// Already triggering before we started the move.
|
||||
// Shift the trigger point slightly outwards.
|
||||
a = current_position[Y_AXIS] - 1.5f;
|
||||
// a = current_position[Y_AXIS] - 1.5f;
|
||||
a = current_position[Y_AXIS];
|
||||
} else {
|
||||
enable_z_endstop(true);
|
||||
go_xy(current_position[X_AXIS], y1, homing_feedrate[X_AXIS] / 60.f);
|
||||
update_current_position_xyz();
|
||||
if (! endstop_z_hit_on_purpose())
|
||||
return false;
|
||||
if (! endstop_z_hit_on_purpose()) {
|
||||
current_position[Y_AXIS] = center_old_y;
|
||||
goto canceled;
|
||||
}
|
||||
a = current_position[Y_AXIS];
|
||||
}
|
||||
enable_z_endstop(false);
|
||||
@ -413,8 +841,10 @@ inline bool improve_bed_induction_sensor_point2(bool lift_z_on_min_y)
|
||||
enable_z_endstop(true);
|
||||
go_xy(current_position[X_AXIS], y0, homing_feedrate[X_AXIS] / 60.f);
|
||||
update_current_position_xyz();
|
||||
if (! endstop_z_hit_on_purpose())
|
||||
return false;
|
||||
if (! endstop_z_hit_on_purpose()) {
|
||||
current_position[Y_AXIS] = center_old_y;
|
||||
goto canceled;
|
||||
}
|
||||
b = current_position[Y_AXIS];
|
||||
|
||||
// Go to the center.
|
||||
@ -424,6 +854,12 @@ inline bool improve_bed_induction_sensor_point2(bool lift_z_on_min_y)
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
canceled:
|
||||
// Go back to the center.
|
||||
enable_z_endstop(false);
|
||||
go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
|
||||
return false;
|
||||
}
|
||||
|
||||
#define MESH_BED_CALIBRATION_SHOW_LCD
|
||||
@ -433,11 +869,14 @@ bool find_bed_offset_and_skew()
|
||||
// Reusing the z_values memory for the measurement cache.
|
||||
// 7x7=49 floats, good for 16 (x,y,z) vectors.
|
||||
float *pts = &mbl.z_values[0][0];
|
||||
float *vec_x = pts + 3 * 4;
|
||||
float *vec_y = vec_x + 3;
|
||||
float *cntr = vec_y + 3;
|
||||
float *vec_x = pts + 2 * 4;
|
||||
float *vec_y = vec_x + 2;
|
||||
float *cntr = vec_y + 2;
|
||||
memset(pts, 0, sizeof(float) * 7 * 7);
|
||||
|
||||
// Let the planner use the uncorrected coordinates.
|
||||
world2machine_reset();
|
||||
|
||||
#ifdef MESH_BED_CALIBRATION_SHOW_LCD
|
||||
lcd_implementation_clear();
|
||||
lcd_print_at_PGM(0, 0, MSG_FIND_BED_OFFSET_AND_SKEW_LINE1);
|
||||
@ -451,33 +890,25 @@ bool find_bed_offset_and_skew()
|
||||
lcd_implementation_print_at(0, 2, k+1);
|
||||
lcd_printPGM(MSG_FIND_BED_OFFSET_AND_SKEW_LINE3);
|
||||
#endif /* MESH_BED_CALIBRATION_SHOW_LCD */
|
||||
|
||||
int i, j;
|
||||
switch (k) {
|
||||
case 0: i = 1; j = 0; break;
|
||||
case 1: i = 2; j = 1; break;
|
||||
case 2: i = 1; j = 2; break;
|
||||
case 3: i = 0; j = 1; break;
|
||||
}
|
||||
float *pt = pts + k * 3;
|
||||
float *pt = pts + k * 2;
|
||||
// Go up to z_initial.
|
||||
go_to_current(homing_feedrate[Z_AXIS] / 60.f);
|
||||
// Go to the measurement point position.
|
||||
mbl.get_meas_xy(i, j, current_position[X_AXIS], current_position[Y_AXIS], true); // use default, uncorrected coordinates
|
||||
current_position[X_AXIS] = pgm_read_float(bed_ref_points_4+k*2);
|
||||
current_position[Y_AXIS] = pgm_read_float(bed_ref_points_4+k*2+1);
|
||||
go_to_current(homing_feedrate[X_AXIS] / 60.f);
|
||||
if (! find_bed_induction_sensor_point_xy())
|
||||
return false;
|
||||
find_bed_induction_sensor_point_z();
|
||||
pt[0] = current_position[X_AXIS];
|
||||
pt[1] = current_position[Y_AXIS];
|
||||
pt[2] = current_position[Z_AXIS];
|
||||
// Start searching for the other points at 3mm above the last point.
|
||||
current_position[Z_AXIS] += 3.f;
|
||||
cntr[0] += pt[0];
|
||||
cntr[1] += pt[1];
|
||||
cntr[2] += pt[2];
|
||||
}
|
||||
|
||||
#if 0
|
||||
// Average the X and Y vectors. They may not be perpendicular, if the printer is built incorrectly.
|
||||
{
|
||||
float len;
|
||||
@ -486,8 +917,8 @@ bool find_bed_offset_and_skew()
|
||||
cntr[1] *= 1.f/4.f;
|
||||
cntr[2] *= 1.f/4.f;
|
||||
// Average the X vector.
|
||||
vec_x[0] = (pts[3 * 1 + 0] - pts[3 * 3 + 0]) / 2.f;
|
||||
vec_x[1] = (pts[3 * 1 + 1] - pts[3 * 3 + 1]) / 2.f;
|
||||
vec_x[0] = (pts[2 * 1 + 0] - pts[2 * 3 + 0]) / 2.f;
|
||||
vec_x[1] = (pts[2 * 1 + 1] - pts[2 * 3 + 1]) / 2.f;
|
||||
len = sqrt(vec_x[0]*vec_x[0] + vec_x[1]*vec_x[1]);
|
||||
if (0) {
|
||||
// if (len < MEAS_NUM_X_DIST) {
|
||||
@ -500,8 +931,8 @@ bool find_bed_offset_and_skew()
|
||||
// Verify the maximum skew?
|
||||
}
|
||||
// Average the Y vector.
|
||||
vec_y[0] = (pts[3 * 2 + 0] - pts[3 * 0 + 0]) / 2.f;
|
||||
vec_y[1] = (pts[3 * 2 + 1] - pts[3 * 0 + 1]) / 2.f;
|
||||
vec_y[0] = (pts[2 * 2 + 0] - pts[2 * 0 + 0]) / 2.f;
|
||||
vec_y[1] = (pts[2 * 2 + 1] - pts[2 * 0 + 1]) / 2.f;
|
||||
len = sqrt(vec_y[0]*vec_y[0] + vec_y[1]*vec_y[1]);
|
||||
if (0) {
|
||||
// if (len < MEAS_NUM_Y_DIST) {
|
||||
@ -539,6 +970,25 @@ bool find_bed_offset_and_skew()
|
||||
SERIAL_ECHOLN("");
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
calculate_machine_skew_and_offset_LS(pts, 4, bed_ref_points_4, vec_x, vec_y, cntr);
|
||||
world2machine_rotation_and_skew[0][0] = vec_x[0];
|
||||
world2machine_rotation_and_skew[1][0] = vec_x[1];
|
||||
world2machine_rotation_and_skew[0][1] = vec_y[0];
|
||||
world2machine_rotation_and_skew[1][1] = vec_y[1];
|
||||
world2machine_shift[0] = cntr[0];
|
||||
world2machine_shift[1] = cntr[1];
|
||||
#if 1
|
||||
// Fearlessly store the calibration values into the eeprom.
|
||||
eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER+0), cntr [0]);
|
||||
eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER+4), cntr [1]);
|
||||
eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +0), vec_x[0]);
|
||||
eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +4), vec_x[1]);
|
||||
eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +0), vec_y[0]);
|
||||
eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +4), vec_y[1]);
|
||||
#endif
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -552,6 +1002,16 @@ bool improve_bed_offset_and_skew(int8_t method)
|
||||
float *cntr = vec_y + 2;
|
||||
memset(pts, 0, sizeof(float) * 7 * 7);
|
||||
|
||||
// Cache the current correction matrix.
|
||||
vec_x[0] = world2machine_rotation_and_skew[0][0];
|
||||
vec_x[1] = world2machine_rotation_and_skew[1][0];
|
||||
vec_y[0] = world2machine_rotation_and_skew[0][1];
|
||||
vec_y[1] = world2machine_rotation_and_skew[1][1];
|
||||
cntr[0] = world2machine_shift[0];
|
||||
cntr[1] = world2machine_shift[1];
|
||||
// and reset the correction matrix, so the planner will not do anything.
|
||||
world2machine_reset();
|
||||
|
||||
bool endstops_enabled = enable_endstops(false);
|
||||
bool endstop_z_enabled = enable_z_endstop(false);
|
||||
|
||||
@ -562,9 +1022,6 @@ bool improve_bed_offset_and_skew(int8_t method)
|
||||
|
||||
// Collect a matrix of 9x9 points.
|
||||
for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
|
||||
int ix = mesh_point % MESH_MEAS_NUM_X_POINTS;
|
||||
int iy = mesh_point / MESH_MEAS_NUM_X_POINTS;
|
||||
if (iy & 1) ix = (MESH_MEAS_NUM_X_POINTS - 1) - ix; // Zig zag
|
||||
// Print the decrasing ID of the measurement point.
|
||||
#ifdef MESH_BED_CALIBRATION_SHOW_LCD
|
||||
lcd_print_at_PGM(0, 1, MSG_IMPROVE_BED_OFFSET_AND_SKEW_LINE2);
|
||||
@ -579,7 +1036,11 @@ bool improve_bed_offset_and_skew(int8_t method)
|
||||
go_to_current(homing_feedrate[Z_AXIS]/60);
|
||||
// Go to the measurement point.
|
||||
// Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
|
||||
mbl.get_meas_xy(ix, iy, current_position[X_AXIS], current_position[Y_AXIS], false);
|
||||
current_position[X_AXIS] = vec_x[0] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[0] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[0];
|
||||
current_position[Y_AXIS] = vec_x[1] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[1] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[1];
|
||||
// The calibration points are very close to the min Y.
|
||||
if (current_position[Y_AXIS] < Y_MIN_POS)
|
||||
current_position[Y_AXIS] = Y_MIN_POS;
|
||||
go_to_current(homing_feedrate[X_AXIS]/60);
|
||||
// Find its Z position by running the normal vertical search.
|
||||
// delay_keep_alive(3000);
|
||||
@ -587,34 +1048,53 @@ bool improve_bed_offset_and_skew(int8_t method)
|
||||
// delay_keep_alive(3000);
|
||||
// Improve the point position by searching its center in a current plane.
|
||||
int8_t n_errors = 3;
|
||||
for (int8_t iter = 0; iter < 4; ++ iter) {
|
||||
for (int8_t iter = 0; iter < 8; ) {
|
||||
bool found = false;
|
||||
switch (method) {
|
||||
case 0: found = improve_bed_induction_sensor_point(); break;
|
||||
case 1: found = improve_bed_induction_sensor_point2(iy == 0); break;
|
||||
case 1: found = improve_bed_induction_sensor_point2(mesh_point < 3); break;
|
||||
default: break;
|
||||
}
|
||||
if (! found) {
|
||||
if (n_errors -- == 0) {
|
||||
// Give up.
|
||||
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;
|
||||
enable_endstops(false);
|
||||
enable_z_endstop(false);
|
||||
go_to_current(homing_feedrate[Z_AXIS]);
|
||||
if (found) {
|
||||
if (iter > 3) {
|
||||
// Average the last 4 measurements.
|
||||
pts[mesh_point*2 ] += current_position[X_AXIS];
|
||||
pts[mesh_point*2+1] += current_position[Y_AXIS];
|
||||
}
|
||||
++ iter;
|
||||
} else if (n_errors -- == 0) {
|
||||
// Give up.
|
||||
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;
|
||||
enable_endstops(false);
|
||||
enable_z_endstop(false);
|
||||
go_to_current(homing_feedrate[Z_AXIS]);
|
||||
}
|
||||
}
|
||||
// delay_keep_alive(3000);
|
||||
float *pt = pts + 2 * (ix + iy * 3);
|
||||
pt[0] = current_position[X_AXIS];
|
||||
pt[1] = current_position[Y_AXIS];
|
||||
cntr[0] += pt[0];
|
||||
cntr[1] += pt[1];
|
||||
delay_keep_alive(3000);
|
||||
}
|
||||
|
||||
// Average the last 4 measurements.
|
||||
for (int8_t i = 0; i < 18; ++ i)
|
||||
pts[i] *= (1.f/4.f);
|
||||
|
||||
// Test the positions. Are the positions reproducible?
|
||||
#if 1
|
||||
enable_endstops(false);
|
||||
enable_z_endstop(false);
|
||||
for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
|
||||
// Go to the measurement point.
|
||||
// Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
|
||||
current_position[X_AXIS] = pts[mesh_point*2];
|
||||
current_position[Y_AXIS] = pts[mesh_point*2+1];
|
||||
go_to_current(homing_feedrate[X_AXIS]/60);
|
||||
delay_keep_alive(3000);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if 0
|
||||
// Average the X and Y vectors. They may not be perpendicular, if the printer is built incorrectly.
|
||||
// Average the center point.
|
||||
cntr[0] *= 1.f/9.f;
|
||||
@ -625,7 +1105,6 @@ bool improve_bed_offset_and_skew(int8_t method)
|
||||
// Average the Y vector.
|
||||
vec_y[0] = (pts[2 * 6 + 0] - pts[2 * 0 + 0] + pts[2 * 7 + 0] - pts[2 * 1 + 0] + pts[2 * 8 + 0] - pts[2 * 2 + 0]) / 6.f;
|
||||
vec_y[1] = (pts[2 * 6 + 1] - pts[2 * 0 + 1] + pts[2 * 7 + 1] - pts[2 * 1 + 1] + pts[2 * 8 + 1] - pts[2 * 2 + 1]) / 6.f;
|
||||
|
||||
#if 1
|
||||
// Fearlessly store the calibration values into the eeprom.
|
||||
eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER+0), cntr [0]);
|
||||
@ -635,6 +1114,39 @@ bool improve_bed_offset_and_skew(int8_t method)
|
||||
eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +0), vec_y[0]);
|
||||
eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +4), vec_y[1]);
|
||||
#endif
|
||||
#else
|
||||
calculate_machine_skew_and_offset_LS(pts, 9, bed_ref_points, vec_x, vec_y, cntr);
|
||||
world2machine_rotation_and_skew[0][0] = vec_x[0];
|
||||
world2machine_rotation_and_skew[1][0] = vec_x[1];
|
||||
world2machine_rotation_and_skew[0][1] = vec_y[0];
|
||||
world2machine_rotation_and_skew[1][1] = vec_y[1];
|
||||
world2machine_shift[0] = cntr[0];
|
||||
world2machine_shift[1] = cntr[1];
|
||||
#if 1
|
||||
// Fearlessly store the calibration values into the eeprom.
|
||||
eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER+0), cntr [0]);
|
||||
eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER+4), cntr [1]);
|
||||
eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +0), vec_x[0]);
|
||||
eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +4), vec_x[1]);
|
||||
eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +0), vec_y[0]);
|
||||
eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +4), vec_y[1]);
|
||||
#endif
|
||||
#endif
|
||||
|
||||
// Test the positions. Are the positions reproducible? Now the calibration is active in the planner.
|
||||
#if 1
|
||||
enable_endstops(false);
|
||||
enable_z_endstop(false);
|
||||
delay_keep_alive(3000);
|
||||
for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
|
||||
// Go to the measurement point.
|
||||
// Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
|
||||
current_position[X_AXIS] = pgm_read_float(bed_ref_points+mesh_point*2);
|
||||
current_position[Y_AXIS] = pgm_read_float(bed_ref_points+mesh_point*2+1);
|
||||
go_to_current(homing_feedrate[X_AXIS]/60);
|
||||
delay_keep_alive(3000);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if 0
|
||||
// and let us know the result.
|
||||
@ -659,105 +1171,9 @@ bool improve_bed_offset_and_skew(int8_t method)
|
||||
return true;
|
||||
|
||||
canceled:
|
||||
// Store the identity matrix to EEPROM.
|
||||
reset_bed_offset_and_skew();
|
||||
enable_endstops(endstops_enabled);
|
||||
enable_z_endstop(endstop_z_enabled);
|
||||
return false;
|
||||
}
|
||||
|
||||
void reset_bed_offset_and_skew()
|
||||
{
|
||||
eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_CENTER+0), 0x0FFFFFFFF);
|
||||
eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_CENTER+4), 0x0FFFFFFFF);
|
||||
eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_X +0), 0x0FFFFFFFF);
|
||||
eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_X +4), 0x0FFFFFFFF);
|
||||
eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_Y +0), 0x0FFFFFFFF);
|
||||
eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_Y +4), 0x0FFFFFFFF);
|
||||
}
|
||||
|
||||
#if 0
|
||||
static const float[9][2] PROGMEM bed_points = {
|
||||
};
|
||||
|
||||
bool calculate_machine_skew_and_offset_LS(
|
||||
// Matrix of 9 2D points (18 floats)
|
||||
float *pts,
|
||||
// Resulting correction matrix.
|
||||
float *vec_x,
|
||||
float *vec_y,
|
||||
float *cntr,
|
||||
// Temporary values, 49-18-(2*3)=25 floats
|
||||
float *temp
|
||||
{
|
||||
{
|
||||
// Create covariance matrix for A, collect the right hand side b.
|
||||
float A[3][3] = { 0.f };
|
||||
float b[3] = { 0.f };
|
||||
float acc;
|
||||
for (uint8_t r = 0; r < 3; ++ r) {
|
||||
for (uint8_t c = 0; c < 3; ++ c) {
|
||||
acc = 0;
|
||||
for (uint8_t i = 0; i < 9; ++ i) {
|
||||
float a = (r == 2) ? 1.f : pts[2 * i + r];
|
||||
float b = (c == 2) ? 1.f : pts[2 * i + c];
|
||||
acc += a * b;
|
||||
}
|
||||
A[r][c] = acc;
|
||||
}
|
||||
acc = 0.f;
|
||||
for (uint8_t i = 0; i < 9; ++ i) {
|
||||
float a = (r == 2) ? 1.f : pts[2 * i + r];
|
||||
float b = pgm_read_float(&coeff2[i][0]);
|
||||
acc += a * b;
|
||||
}
|
||||
b[r] = acc;
|
||||
}
|
||||
// Solve the linear equation for ax, bx, cx.
|
||||
float x[3] = { 0.f };
|
||||
for (uint8_t iter = 0; iter < 100; ++ iter) {
|
||||
x[0] = (b[0] - A[1] * x[1] - A[2] * x[2]) / A[0];
|
||||
x[1] = (b[1] - A[0] * x[0] - A[2] * x[2]) / A[1];
|
||||
x[2] = (b[2] - A[0] * x[0] - A[1] * x[1]) / A[2];
|
||||
}
|
||||
// Store the result to the output variables.
|
||||
vec_x[0] = x[0];
|
||||
vec_y[0] = x[1];
|
||||
cntr[0] = x[2];
|
||||
|
||||
// Recalculate b for the y values.
|
||||
for (uint8_t r = 0; r < 3; ++ r) {
|
||||
acc = 0.f;
|
||||
for (uint8_t i = 0; i < 9; ++ i) {
|
||||
float a = (r == 2) ? 1.f : pts[2 * i + r];
|
||||
float b = pgm_read_float(&coeff2[i][1]);
|
||||
acc += a * b;
|
||||
}
|
||||
b[r] = acc;
|
||||
}
|
||||
// Solve the linear equation for ay, by, cy.
|
||||
x[0] = 0.f, x[1] = 0.f; x[2] = 0.f;
|
||||
for (uint8_t iter = 0; iter < 100; ++ iter) {
|
||||
x[0] = (b[0] - A[1] * x[1] - A[2] * x[2]) / A[0];
|
||||
x[1] = (b[1] - A[0] * x[0] - A[2] * x[2]) / A[1];
|
||||
x[2] = (b[2] - A[0] * x[0] - A[1] * x[1]) / A[2];
|
||||
}
|
||||
// Store the result to the output variables.
|
||||
vec_x[1] = x[0];
|
||||
vec_y[1] = x[1];
|
||||
cntr[1] = x[2];
|
||||
}
|
||||
|
||||
// Normalize the vectors. We expect, that the machine axes may be skewed a bit, but the distances are correct.
|
||||
// l shall be very close to 1 already.
|
||||
float l = sqrt(vec_x[0]*vec_x[0] + vec_x[1] * vec_x[1]);
|
||||
vec_x[0] /= l;
|
||||
vec_x[1] /= l;
|
||||
l = sqrt(vec_y[0]*vec_y[0] + vec_y[1] * vec_y[1]);
|
||||
vec_y[0] /= l;
|
||||
vec_y[1] /= l;
|
||||
|
||||
|
||||
|
||||
// Invert the transformation matrix made of vec_x, vec_y and cntr.
|
||||
|
||||
}
|
||||
#endif
|
||||
@ -1,6 +1,29 @@
|
||||
#ifndef MESH_BED_CALIBRATION_H
|
||||
#define MESH_BED_CALIBRATION_H
|
||||
|
||||
// Exact positions of the print head above the bed reference points, in the world coordinates.
|
||||
// The world coordinates match the machine coordinates only in case, when the machine
|
||||
// is built properly, the end stops are at the correct positions and the axes are perpendicular.
|
||||
extern const float bed_ref_points[] PROGMEM;
|
||||
|
||||
// 2x2 transformation matrix from the world coordinates to the machine coordinates.
|
||||
// Corrects for the rotation and skew of the machine axes.
|
||||
// Used by the planner's plan_buffer_line() and plan_set_position().
|
||||
extern float world2machine_rotation_and_skew[2][2];
|
||||
// Shift of the machine zero point, in the machine coordinates.
|
||||
extern float world2machine_shift[2];
|
||||
|
||||
// Resets the transformation to identity.
|
||||
extern void world2machine_reset();
|
||||
// Loads the transformation from the EEPROM, if available.
|
||||
extern void world2machine_initialize();
|
||||
|
||||
// When switching from absolute to corrected coordinates,
|
||||
// this will apply an inverse world2machine transformation
|
||||
// to current_position[x,y].
|
||||
extern void world2machine_update_current();
|
||||
|
||||
|
||||
extern void find_bed_induction_sensor_point_z();
|
||||
extern bool find_bed_induction_sensor_point_xy();
|
||||
|
||||
|
||||
@ -1,4 +1,5 @@
|
||||
#include "mesh_bed_leveling.h"
|
||||
#include "mesh_bed_calibration.h"
|
||||
#include "Configuration.h"
|
||||
|
||||
#ifdef MESH_BED_LEVELING
|
||||
@ -22,6 +23,7 @@ static inline bool vec_undef(const float v[2])
|
||||
|
||||
void mesh_bed_leveling::get_meas_xy(int ix, int iy, float &x, float &y, bool use_default)
|
||||
{
|
||||
#if 0
|
||||
float cntr[2] = {
|
||||
eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_CENTER+0)),
|
||||
eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_CENTER+4))
|
||||
@ -83,10 +85,18 @@ void mesh_bed_leveling::get_meas_xy(int ix, int iy, float &x, float &y, bool use
|
||||
SERIAL_ECHOLN("");
|
||||
#endif
|
||||
}
|
||||
#else
|
||||
// Default, uncorrected positions of the calibration points.
|
||||
// This coordinate will be corrected by the planner.
|
||||
x = pgm_read_float(bed_ref_points + 2 * (iy * 3 + ix));
|
||||
y = pgm_read_float(bed_ref_points + 2 * (iy * 3 + ix) + 1);
|
||||
#endif
|
||||
}
|
||||
|
||||
#if MESH_NUM_X_POINTS>=5 && MESH_NUM_Y_POINTS>=5 && (MESH_NUM_X_POINTS&1)==1 && (MESH_NUM_Y_POINTS&1)==1
|
||||
// Works for an odd number of MESH_NUM_X_POINTS and MESH_NUM_Y_POINTS
|
||||
|
||||
// #define MBL_BILINEAR
|
||||
void mesh_bed_leveling::upsample_3x3()
|
||||
{
|
||||
int idx0 = 0;
|
||||
@ -106,9 +116,16 @@ void mesh_bed_leveling::upsample_3x3()
|
||||
if (i == idx1)
|
||||
continue;
|
||||
float x = get_x(i);
|
||||
z_values[j][i] = z_values[j][idx0] * (x - x1) * (x - x2) / ((x0 - x1) * (x0 - x2)) +
|
||||
z_values[j][idx1] * (x - x0) * (x - x2) / ((x1 - x0) * (x1 - x2)) +
|
||||
z_values[j][idx2] * (x - x0) * (x - x1) / ((x2 - x0) * (x2 - x1));
|
||||
#ifdef MBL_BILINEAR
|
||||
z_values[j][i] = (x < x1) ?
|
||||
((z_values[j][idx0] * (x - x0) + z_values[j][idx1] * (x1 - x)) / (x1 - x0)) :
|
||||
((z_values[j][idx1] * (x - x1) + z_values[j][idx2] * (x2 - x)) / (x2 - x1));
|
||||
#else
|
||||
z_values[j][i] =
|
||||
z_values[j][idx0] * (x - x1) * (x - x2) / ((x0 - x1) * (x0 - x2)) +
|
||||
z_values[j][idx1] * (x - x0) * (x - x2) / ((x1 - x0) * (x1 - x2)) +
|
||||
z_values[j][idx2] * (x - x0) * (x - x1) / ((x2 - x0) * (x2 - x1));
|
||||
#endif
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -126,12 +143,36 @@ void mesh_bed_leveling::upsample_3x3()
|
||||
if (j == idx1)
|
||||
continue;
|
||||
float y = get_y(j);
|
||||
z_values[j][i] = z_values[idx0][i] * (y - y1) * (y - y2) / ((y0 - y1) * (y0 - y2)) +
|
||||
z_values[idx1][i] * (y - y0) * (y - y2) / ((y1 - y0) * (y1 - y2)) +
|
||||
z_values[idx2][i] * (y - y0) * (y - y1) / ((y2 - y0) * (y2 - y1));
|
||||
#ifdef MBL_BILINEAR
|
||||
z_values[j][i] = (y < y1) ?
|
||||
((z_values[idx0][i] * (y - y0) + z_values[idx1][i] * (y1 - y)) / (y1 - y0)) :
|
||||
((z_values[idx1][i] * (y - y1) + z_values[idx2][i] * (y2 - y)) / (y2 - y1));
|
||||
#else
|
||||
z_values[j][i] =
|
||||
z_values[idx0][i] * (y - y1) * (y - y2) / ((y0 - y1) * (y0 - y2)) +
|
||||
z_values[idx1][i] * (y - y0) * (y - y2) / ((y1 - y0) * (y1 - y2)) +
|
||||
z_values[idx2][i] * (y - y0) * (y - y1) / ((y2 - y0) * (y2 - y1));
|
||||
#endif
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
// Relax the non-measured points.
|
||||
const float weight = 0.2f;
|
||||
for (uint8_t iter = 0; iter < 20; ++ iter) {
|
||||
for (int8_t j = 1; j < 6; ++ j) {
|
||||
for (int8_t i = 1; i < 6; ++ i) {
|
||||
if (i == 3 || j == 3)
|
||||
continue;
|
||||
if ((i % 3) == 0 && (j % 3) == 0)
|
||||
continue;
|
||||
float avg = 0.25f * (z_values[j][i-1]+z_values[j][i+1]+z_values[j-1][i]+z_values[j+1][i]);
|
||||
z_values[j][i] = (1.f-weight)*z_values[j][i] + weight*avg;
|
||||
}
|
||||
}
|
||||
}
|
||||
*/
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
@ -104,11 +104,7 @@
|
||||
#define KILL_PIN -1 //80 with Smart Controller LCD
|
||||
#define SUICIDE_PIN -1 //PIN that has to be turned on right after start, to keep power flowing.
|
||||
#define SDPOWER -1
|
||||
#ifdef BARICUDA
|
||||
#define HEATER_2_PIN 6
|
||||
#else
|
||||
#define HEATER_2_PIN -1
|
||||
#endif
|
||||
#define HEATER_2_PIN -1
|
||||
#ifdef MINI_RAMBO
|
||||
|
||||
#define ELECTRONICS "RAMBo13a"
|
||||
@ -282,11 +278,7 @@
|
||||
#define KILL_PIN -1 //80 with Smart Controller LCD
|
||||
#define SUICIDE_PIN -1 //PIN that has to be turned on right after start, to keep power flowing.
|
||||
#define SDPOWER -1
|
||||
#ifdef BARICUDA
|
||||
#define HEATER_2_PIN 6
|
||||
#else
|
||||
#define HEATER_2_PIN -1
|
||||
#endif
|
||||
#define HEATER_2_PIN -1
|
||||
|
||||
#define HEATER_0_PIN 3
|
||||
#define HEATER_BED_PIN 4
|
||||
|
||||
@ -60,6 +60,7 @@
|
||||
|
||||
#ifdef MESH_BED_LEVELING
|
||||
#include "mesh_bed_leveling.h"
|
||||
#include "mesh_bed_calibration.h"
|
||||
#endif
|
||||
|
||||
//===========================================================================
|
||||
@ -458,20 +459,12 @@ void check_axes_activity()
|
||||
unsigned char z_active = 0;
|
||||
unsigned char e_active = 0;
|
||||
unsigned char tail_fan_speed = fanSpeed;
|
||||
#ifdef BARICUDA
|
||||
unsigned char tail_valve_pressure = ValvePressure;
|
||||
unsigned char tail_e_to_p_pressure = EtoPPressure;
|
||||
#endif
|
||||
block_t *block;
|
||||
|
||||
if(block_buffer_tail != block_buffer_head)
|
||||
{
|
||||
uint8_t block_index = block_buffer_tail;
|
||||
tail_fan_speed = block_buffer[block_index].fan_speed;
|
||||
#ifdef BARICUDA
|
||||
tail_valve_pressure = block_buffer[block_index].valve_pressure;
|
||||
tail_e_to_p_pressure = block_buffer[block_index].e_to_p_pressure;
|
||||
#endif
|
||||
while(block_index != block_buffer_head)
|
||||
{
|
||||
block = &block_buffer[block_index];
|
||||
@ -515,16 +508,6 @@ void check_axes_activity()
|
||||
#ifdef AUTOTEMP
|
||||
getHighESpeed();
|
||||
#endif
|
||||
|
||||
#ifdef BARICUDA
|
||||
#if defined(HEATER_1_PIN) && HEATER_1_PIN > -1
|
||||
analogWrite(HEATER_1_PIN,tail_valve_pressure);
|
||||
#endif
|
||||
|
||||
#if defined(HEATER_2_PIN) && HEATER_2_PIN > -1
|
||||
analogWrite(HEATER_2_PIN,tail_e_to_p_pressure);
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
@ -532,11 +515,7 @@ float junction_deviation = 0.1;
|
||||
// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in
|
||||
// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
|
||||
// calculation the caller must also provide the physical length of the line in millimeters.
|
||||
#ifdef ENABLE_AUTO_BED_LEVELING
|
||||
void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder)
|
||||
#else
|
||||
void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder)
|
||||
#endif //ENABLE_AUTO_BED_LEVELING
|
||||
{
|
||||
// Calculate the buffer head after we push this byte
|
||||
int next_buffer_head = next_block_index(block_buffer_head);
|
||||
@ -554,6 +533,53 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
|
||||
apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
|
||||
#endif // ENABLE_AUTO_BED_LEVELING
|
||||
|
||||
// Apply the machine correction matrix.
|
||||
{
|
||||
#if 0
|
||||
SERIAL_ECHOPGM("Planner, current position - servos: ");
|
||||
MYSERIAL.print(st_get_position_mm(X_AXIS), 5);
|
||||
SERIAL_ECHOPGM(", ");
|
||||
MYSERIAL.print(st_get_position_mm(Y_AXIS), 5);
|
||||
SERIAL_ECHOPGM(", ");
|
||||
MYSERIAL.print(st_get_position_mm(Z_AXIS), 5);
|
||||
SERIAL_ECHOLNPGM("");
|
||||
|
||||
SERIAL_ECHOPGM("Planner, target position, initial: ");
|
||||
MYSERIAL.print(x, 5);
|
||||
SERIAL_ECHOPGM(", ");
|
||||
MYSERIAL.print(y, 5);
|
||||
SERIAL_ECHOLNPGM("");
|
||||
|
||||
SERIAL_ECHOPGM("Planner, world2machine: ");
|
||||
MYSERIAL.print(world2machine_rotation_and_skew[0][0], 5);
|
||||
SERIAL_ECHOPGM(", ");
|
||||
MYSERIAL.print(world2machine_rotation_and_skew[0][1], 5);
|
||||
SERIAL_ECHOPGM(", ");
|
||||
MYSERIAL.print(world2machine_rotation_and_skew[1][0], 5);
|
||||
SERIAL_ECHOPGM(", ");
|
||||
MYSERIAL.print(world2machine_rotation_and_skew[1][1], 5);
|
||||
SERIAL_ECHOLNPGM("");
|
||||
SERIAL_ECHOPGM("Planner, offset: ");
|
||||
MYSERIAL.print(world2machine_shift[0], 5);
|
||||
SERIAL_ECHOPGM(", ");
|
||||
MYSERIAL.print(world2machine_shift[1], 5);
|
||||
SERIAL_ECHOLNPGM("");
|
||||
#endif
|
||||
|
||||
float tmpx = x;
|
||||
float tmpy = y;
|
||||
x = world2machine_rotation_and_skew[0][0] * tmpx + world2machine_rotation_and_skew[0][1] * tmpy + world2machine_shift[0];
|
||||
y = world2machine_rotation_and_skew[1][0] * tmpx + world2machine_rotation_and_skew[1][1] * tmpy + world2machine_shift[1];
|
||||
|
||||
#if 0
|
||||
SERIAL_ECHOPGM("Planner, target position, corrected: ");
|
||||
MYSERIAL.print(x, 5);
|
||||
SERIAL_ECHOPGM(", ");
|
||||
MYSERIAL.print(y, 5);
|
||||
SERIAL_ECHOLNPGM("");
|
||||
#endif
|
||||
}
|
||||
|
||||
// The target position of the tool in absolute steps
|
||||
// Calculate target position in absolute steps
|
||||
//this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
|
||||
@ -622,10 +648,6 @@ block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-positi
|
||||
}
|
||||
|
||||
block->fan_speed = fanSpeed;
|
||||
#ifdef BARICUDA
|
||||
block->valve_pressure = ValvePressure;
|
||||
block->e_to_p_pressure = EtoPPressure;
|
||||
#endif
|
||||
|
||||
// Compute direction bits for this block
|
||||
block->direction_bits = 0;
|
||||
@ -1061,16 +1083,20 @@ vector_3 plan_get_position() {
|
||||
}
|
||||
#endif // ENABLE_AUTO_BED_LEVELING
|
||||
|
||||
#ifdef ENABLE_AUTO_BED_LEVELING
|
||||
void plan_set_position(float x, float y, float z, const float &e)
|
||||
{
|
||||
apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
|
||||
#else
|
||||
void plan_set_position(const float &x, const float &y, const float &z, const float &e)
|
||||
{
|
||||
#ifdef ENABLE_AUTO_BED_LEVELING
|
||||
apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
|
||||
#endif // ENABLE_AUTO_BED_LEVELING
|
||||
|
||||
|
||||
|
||||
// Apply the machine correction matrix.
|
||||
{
|
||||
float tmpx = x;
|
||||
float tmpy = y;
|
||||
x = world2machine_rotation_and_skew[0][0] * tmpx + world2machine_rotation_and_skew[0][1] * tmpy + world2machine_shift[0];
|
||||
y = world2machine_rotation_and_skew[1][0] * tmpx + world2machine_rotation_and_skew[1][1] * tmpy + world2machine_shift[1];
|
||||
}
|
||||
|
||||
position[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
|
||||
position[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
|
||||
#ifdef MESH_BED_LEVELING
|
||||
@ -1091,6 +1117,13 @@ void plan_set_position(const float &x, const float &y, const float &z, const flo
|
||||
previous_speed[3] = 0.0;
|
||||
}
|
||||
|
||||
// Only useful in the bed leveling routine, when the mesh bed leveling is off.
|
||||
void plan_set_z_position(float z)
|
||||
{
|
||||
position[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
|
||||
st_set_position(position[X_AXIS], position[Y_AXIS], position[Z_AXIS], position[E_AXIS]);
|
||||
}
|
||||
|
||||
void plan_set_e_position(const float &e)
|
||||
{
|
||||
position[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);
|
||||
@ -1116,4 +1149,4 @@ void reset_acceleration_rates()
|
||||
{
|
||||
axis_steps_per_sqr_second[i] = max_acceleration_units_per_sq_second[i] * axis_steps_per_unit[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -64,10 +64,6 @@ typedef struct {
|
||||
unsigned long final_rate; // The minimal rate at exit
|
||||
unsigned long acceleration_st; // acceleration steps/sec^2
|
||||
unsigned long fan_speed;
|
||||
#ifdef BARICUDA
|
||||
unsigned long valve_pressure;
|
||||
unsigned long e_to_p_pressure;
|
||||
#endif
|
||||
volatile char busy;
|
||||
} block_t;
|
||||
|
||||
@ -88,16 +84,18 @@ void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate
|
||||
// Get the position applying the bed level matrix if enabled
|
||||
vector_3 plan_get_position();
|
||||
#else
|
||||
void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder);
|
||||
void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder);
|
||||
//void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder);
|
||||
#endif // ENABLE_AUTO_BED_LEVELING
|
||||
|
||||
// Set position. Used for G92 instructions.
|
||||
#ifdef ENABLE_AUTO_BED_LEVELING
|
||||
//#ifdef ENABLE_AUTO_BED_LEVELING
|
||||
void plan_set_position(float x, float y, float z, const float &e);
|
||||
#else
|
||||
void plan_set_position(const float &x, const float &y, const float &z, const float &e);
|
||||
#endif // ENABLE_AUTO_BED_LEVELING
|
||||
//#else
|
||||
//void plan_set_position(const float &x, const float &y, const float &z, const float &e);
|
||||
//#endif // ENABLE_AUTO_BED_LEVELING
|
||||
|
||||
void plan_set_z_position(const float z);
|
||||
void plan_set_e_position(const float &e);
|
||||
|
||||
|
||||
|
||||
@ -1197,7 +1197,6 @@ void babystep(const uint8_t axis,const bool direction)
|
||||
}
|
||||
break;
|
||||
|
||||
#ifndef DELTA
|
||||
case Z_AXIS:
|
||||
{
|
||||
enable_z();
|
||||
@ -1229,41 +1228,6 @@ void babystep(const uint8_t axis,const bool direction)
|
||||
|
||||
}
|
||||
break;
|
||||
#else //DELTA
|
||||
case Z_AXIS:
|
||||
{
|
||||
enable_x();
|
||||
enable_y();
|
||||
enable_z();
|
||||
uint8_t old_x_dir_pin= READ(X_DIR_PIN);
|
||||
uint8_t old_y_dir_pin= READ(Y_DIR_PIN);
|
||||
uint8_t old_z_dir_pin= READ(Z_DIR_PIN);
|
||||
//setup new step
|
||||
WRITE(X_DIR_PIN,(INVERT_X_DIR)^direction^BABYSTEP_INVERT_Z);
|
||||
WRITE(Y_DIR_PIN,(INVERT_Y_DIR)^direction^BABYSTEP_INVERT_Z);
|
||||
WRITE(Z_DIR_PIN,(INVERT_Z_DIR)^direction^BABYSTEP_INVERT_Z);
|
||||
|
||||
//perform step
|
||||
WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN);
|
||||
WRITE(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
|
||||
WRITE(Z_STEP_PIN, !INVERT_Z_STEP_PIN);
|
||||
|
||||
//wait a tiny bit
|
||||
{
|
||||
float x=1./float(axis+1); //absolutely useless
|
||||
}
|
||||
WRITE(X_STEP_PIN, INVERT_X_STEP_PIN);
|
||||
WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN);
|
||||
WRITE(Z_STEP_PIN, INVERT_Z_STEP_PIN);
|
||||
|
||||
//get old pin state back.
|
||||
WRITE(X_DIR_PIN,old_x_dir_pin);
|
||||
WRITE(Y_DIR_PIN,old_y_dir_pin);
|
||||
WRITE(Z_DIR_PIN,old_z_dir_pin);
|
||||
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
|
||||
default: break;
|
||||
}
|
||||
|
||||
@ -125,10 +125,8 @@ static void lcd_quick_feedback();//Cause an LCD refresh, and give the user visua
|
||||
/* Different types of actions that can be used in menu items. */
|
||||
static void menu_action_back(menuFunc_t data);
|
||||
static void menu_action_submenu(menuFunc_t data);
|
||||
static void menu_action_gcode(const char* pgcode);
|
||||
static void menu_action_function(menuFunc_t data);
|
||||
static void menu_action_setlang(unsigned char lang);
|
||||
static void menu_action_sdfile(const char* filename, char* longFilename);
|
||||
static void menu_action_sddirectory(const char* filename, char* longFilename);
|
||||
static void menu_action_setting_edit_bool(const char* pstr, bool* ptr);
|
||||
static void menu_action_setting_edit_int3(const char* pstr, int* ptr, int minValue, int maxValue);
|
||||
@ -1021,12 +1019,7 @@ static void _lcd_move(const char *name, int axis, int min, int max) {
|
||||
if (min_software_endstops && current_position[axis] < min) current_position[axis] = min;
|
||||
if (max_software_endstops && current_position[axis] > max) current_position[axis] = max;
|
||||
encoderPosition = 0;
|
||||
#ifdef DELTA
|
||||
calculate_delta(current_position);
|
||||
plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], manual_feedrate[axis] / 60, active_extruder);
|
||||
#else
|
||||
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[axis] / 60, active_extruder);
|
||||
#endif
|
||||
lcdDrawUpdate = 1;
|
||||
}
|
||||
if (lcdDrawUpdate) lcd_implementation_drawedit(name, ftostr31(current_position[axis]));
|
||||
@ -1040,12 +1033,7 @@ static void lcd_move_e()
|
||||
{
|
||||
current_position[E_AXIS] += float((int)encoderPosition) * move_menu_scale;
|
||||
encoderPosition = 0;
|
||||
#ifdef DELTA
|
||||
calculate_delta(current_position);
|
||||
plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], manual_feedrate[E_AXIS] / 60, active_extruder);
|
||||
#else
|
||||
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[E_AXIS] / 60, active_extruder);
|
||||
#endif
|
||||
lcdDrawUpdate = 1;
|
||||
}
|
||||
if (lcdDrawUpdate)
|
||||
@ -1543,7 +1531,7 @@ void lcd_mesh_calibration()
|
||||
|
||||
void lcd_mesh_calibration_reset()
|
||||
{
|
||||
enquecommand_P(PSTR("M44"));
|
||||
enquecommand_P(PSTR("M45"));
|
||||
lcd_return_to_status();
|
||||
}
|
||||
|
||||
@ -2520,26 +2508,12 @@ static void menu_action_back(menuFunc_t data) {
|
||||
static void menu_action_submenu(menuFunc_t data) {
|
||||
lcd_goto_menu(data);
|
||||
}
|
||||
static void menu_action_gcode(const char* pgcode) {
|
||||
enquecommand_P(pgcode);
|
||||
}
|
||||
static void menu_action_setlang(unsigned char lang) {
|
||||
lcd_set_lang(lang);
|
||||
}
|
||||
static void menu_action_function(menuFunc_t data) {
|
||||
(*data)();
|
||||
}
|
||||
static void menu_action_sdfile(const char* filename, char* longFilename)
|
||||
{
|
||||
char cmd[30];
|
||||
char* c;
|
||||
sprintf_P(cmd, PSTR("M23 %s"), filename);
|
||||
for (c = &cmd[4]; *c; c++)
|
||||
*c = tolower(*c);
|
||||
enquecommand(cmd);
|
||||
enquecommand_P(PSTR("M24"));
|
||||
lcd_return_to_status();
|
||||
}
|
||||
static void menu_action_sddirectory(const char* filename, char* longFilename)
|
||||
{
|
||||
card.chdir(filename);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user