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

Removed support for DELTA, SCARA and BARICUDA.

Browse Source 
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 2016-06-23 08:46:15 +02:00
parent 08bf6acf1b
commit 78ebd522b6
16 changed files with 1204 additions and 1388 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
Firmware/Configuration.h
View File

@ -31,20 +31,6 @@
// Advanced settings can be found in Configuration_adv.h // Advanced settings can be found in Configuration_adv.h
// BASIC SETTINGS: select your board type, temperature sensor type, axis scaling, and endstop configuration // 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 // 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 // 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. // 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: //Manual homing switch locations:
// For deltabots this means top and center of the Cartesian print volume. // 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). // 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). // 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 // SF send wrong arc g-codes when using Arc Point as fillet procedure
//#define SF_ARC_FIX //#define SF_ARC_FIX
// Support for the BariCUDA Paste Extruder.
//#define BARICUDA
//define BlinkM/CyzRgb Support //define BlinkM/CyzRgb Support
//#define BLINKM //#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 //#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 * Support for a filament diameter sensor
* Also allows adjustment of diameter at print time (vs at slicing) * Also allows adjustment of diameter at print time (vs at slicing)

54
Firmware/ConfigurationStore.cpp
View File

@ -63,12 +63,6 @@ void Config_StoreSettings()
EEPROM_WRITE_VAR(i,max_z_jerk); EEPROM_WRITE_VAR(i,max_z_jerk);
EEPROM_WRITE_VAR(i,max_e_jerk); EEPROM_WRITE_VAR(i,max_e_jerk);
EEPROM_WRITE_VAR(i,add_homing); 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 #ifndef ULTIPANEL
int plaPreheatHotendTemp = PLA_PREHEAT_HOTEND_TEMP, plaPreheatHPBTemp = PLA_PREHEAT_HPB_TEMP, plaPreheatFanSpeed = PLA_PREHEAT_FAN_SPEED; 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; 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; int lcd_contrast = 32;
#endif #endif
EEPROM_WRITE_VAR(i,lcd_contrast); EEPROM_WRITE_VAR(i,lcd_contrast);
#ifdef SCARA
EEPROM_WRITE_VAR(i,axis_scaling); // Add scaling for SCARA
#endif
#ifdef FWRETRACT #ifdef FWRETRACT
EEPROM_WRITE_VAR(i,autoretract_enabled); EEPROM_WRITE_VAR(i,autoretract_enabled);
EEPROM_WRITE_VAR(i,retract_length); EEPROM_WRITE_VAR(i,retract_length);
@ -149,16 +140,6 @@ void Config_PrintSettings()
SERIAL_ECHOLN(""); SERIAL_ECHOLN("");
SERIAL_ECHO_START; 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_ECHOLNPGM("Maximum feedrates (mm/s):");
SERIAL_ECHO_START; SERIAL_ECHO_START;
SERIAL_ECHOPAIR(" M203 X", max_feedrate[X_AXIS]); 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(" Y" ,add_homing[Y_AXIS] );
SERIAL_ECHOPAIR(" Z" ,add_homing[Z_AXIS] ); SERIAL_ECHOPAIR(" Z" ,add_homing[Z_AXIS] );
SERIAL_ECHOLN(""); 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 #ifdef PIDTEMP
SERIAL_ECHO_START; SERIAL_ECHO_START;
SERIAL_ECHOLNPGM("PID settings:"); SERIAL_ECHOLNPGM("PID settings:");
@ -305,12 +270,6 @@ void Config_RetrieveSettings()
EEPROM_READ_VAR(i,max_z_jerk); EEPROM_READ_VAR(i,max_z_jerk);
EEPROM_READ_VAR(i,max_e_jerk); EEPROM_READ_VAR(i,max_e_jerk);
EEPROM_READ_VAR(i,add_homing); 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 #ifndef ULTIPANEL
int plaPreheatHotendTemp, plaPreheatHPBTemp, plaPreheatFanSpeed; int plaPreheatHotendTemp, plaPreheatHPBTemp, plaPreheatFanSpeed;
int absPreheatHotendTemp, absPreheatHPBTemp, absPreheatFanSpeed; int absPreheatHotendTemp, absPreheatHPBTemp, absPreheatFanSpeed;
@ -337,9 +296,6 @@ void Config_RetrieveSettings()
int lcd_contrast; int lcd_contrast;
#endif #endif
EEPROM_READ_VAR(i,lcd_contrast); EEPROM_READ_VAR(i,lcd_contrast);
#ifdef SCARA
EEPROM_READ_VAR(i,axis_scaling);
#endif
#ifdef FWRETRACT #ifdef FWRETRACT
EEPROM_READ_VAR(i,autoretract_enabled); EEPROM_READ_VAR(i,autoretract_enabled);
@ -390,9 +346,6 @@ void Config_ResetDefault()
axis_steps_per_unit[i]=tmp1[i]; axis_steps_per_unit[i]=tmp1[i];
max_feedrate[i]=tmp2[i]; max_feedrate[i]=tmp2[i];
max_acceleration_units_per_sq_second[i]=tmp3[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 // 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_z_jerk=DEFAULT_ZJERK;
max_e_jerk=DEFAULT_EJERK; max_e_jerk=DEFAULT_EJERK;
add_homing[X_AXIS] = add_homing[Y_AXIS] = add_homing[Z_AXIS] = 0; 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 #ifdef ULTIPANEL
plaPreheatHotendTemp = PLA_PREHEAT_HOTEND_TEMP; plaPreheatHotendTemp = PLA_PREHEAT_HOTEND_TEMP;
plaPreheatHPBTemp = PLA_PREHEAT_HPB_TEMP; plaPreheatHPBTemp = PLA_PREHEAT_HPB_TEMP;

10
Firmware/Configuration_adv.h
View File

@ -324,12 +324,6 @@
#ifdef COREXY #ifdef COREXY
#error BABYSTEPPING not implemented for COREXY yet. #error BABYSTEPPING not implemented for COREXY yet.
#endif #endif
#ifdef DELTA
#ifdef BABYSTEP_XY
#error BABYSTEPPING only implemented for Z axis on deltabots.
#endif
#endif
#endif #endif
// extruder advance constant (s2/mm3) // extruder advance constant (s2/mm3)
@ -435,10 +429,6 @@ const unsigned int dropsegments=5; //everything with less than this number of st
//============================= Define Defines ============================ //============================= 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 #if EXTRUDERS > 1 && defined TEMP_SENSOR_1_AS_REDUNDANT
#error "You cannot use TEMP_SENSOR_1_AS_REDUNDANT if EXTRUDERS > 1" #error "You cannot use TEMP_SENSOR_1_AS_REDUNDANT if EXTRUDERS > 1"
#endif #endif

33
Firmware/Marlin.h
View File

@ -211,22 +211,21 @@ void FlushSerialRequestResend();
void ClearToSend(); void ClearToSend();
void get_coordinates(); 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 prepare_move();
void kill(); void kill();
void Stop(); void Stop();
bool IsStopped(); bool IsStopped();
void enquecommand(const char *cmd); //put an ASCII command at the end of the current buffer. //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 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 prepare_arc_move(char isclockwise);
void clamp_to_software_endstops(float target[3]); 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 current_position[NUM_AXIS] ;
extern float destination[NUM_AXIS] ; extern float destination[NUM_AXIS] ;
extern float add_homing[3]; 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 min_pos[3];
extern float max_pos[3]; extern float max_pos[3];
extern bool axis_known_position[3]; extern bool axis_known_position[3];
extern float zprobe_zoffset; extern float zprobe_zoffset;
extern int fanSpeed; extern int fanSpeed;
#ifdef BARICUDA
extern int ValvePressure;
extern int EtoPPressure;
#endif
#ifdef FAN_SOFT_PWM #ifdef FAN_SOFT_PWM
extern unsigned char fanSpeedSoftPwm; extern unsigned char fanSpeedSoftPwm;

8
Firmware/MarlinSerial.cpp
View File

@ -53,9 +53,17 @@ FORCE_INLINE void store_char(unsigned char c)
//SIGNAL(SIG_USART_RECV) //SIGNAL(SIG_USART_RECV)
SIGNAL(M_USARTx_RX_vect) SIGNAL(M_USARTx_RX_vect)
{ {
// 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; unsigned char c = M_UDRx;
store_char(c); store_char(c);
} }
}
#endif #endif
// Constructors //////////////////////////////////////////////////////////////// // Constructors ////////////////////////////////////////////////////////////////

9
Firmware/MarlinSerial.h
View File

@ -53,6 +53,7 @@
#define M_UBRRxH SERIAL_REGNAME(UBRR,SERIAL_PORT,H) #define M_UBRRxH SERIAL_REGNAME(UBRR,SERIAL_PORT,H)
#define M_UBRRxL SERIAL_REGNAME(UBRR,SERIAL_PORT,L) #define M_UBRRxL SERIAL_REGNAME(UBRR,SERIAL_PORT,L)
#define M_RXCx SERIAL_REGNAME(RXC,SERIAL_PORT,) #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_USARTx_RX_vect SERIAL_REGNAME(USART,SERIAL_PORT,_RX_vect)
#define M_U2Xx SERIAL_REGNAME(U2X,SERIAL_PORT,) #define M_U2Xx SERIAL_REGNAME(U2X,SERIAL_PORT,)
@ -112,9 +113,14 @@ class MarlinSerial //: public Stream
FORCE_INLINE void checkRx(void) FORCE_INLINE void checkRx(void)
{ {
if((M_UCSRxA & (1<<M_RXCx)) != 0) { 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; unsigned char c = M_UDRx;
int i = (unsigned int)(rx_buffer.head + 1) % RX_BUFFER_SIZE; int i = (unsigned int)(rx_buffer.head + 1) % RX_BUFFER_SIZE;
// if we should be storing the received character into the location // if we should be storing the received character into the location
// just before the tail (meaning that the head would advance to the // just before the tail (meaning that the head would advance to the
// current location of the tail), we're about to overflow the buffer // current location of the tail), we're about to overflow the buffer
@ -125,6 +131,7 @@ class MarlinSerial //: public Stream
} }
} }
} }
}
private: private:

1390
Firmware/Marlin_main.cpp
View File

File diff suppressed because it is too large Load Diff

3
Firmware/cardreader.cpp
View File

@ -539,9 +539,10 @@ void CardReader::checkautostart(bool force)
if(strncmp((char*)p.name,autoname,5)==0) if(strncmp((char*)p.name,autoname,5)==0)
{ {
char cmd[30]; char cmd[30];
// M23: Select SD file
sprintf_P(cmd, PSTR("M23 %s"), autoname); sprintf_P(cmd, PSTR("M23 %s"), autoname);
enquecommand(cmd); enquecommand(cmd);
// M24: Start/resume SD print
enquecommand_P(PSTR("M24")); enquecommand_P(PSTR("M24"));
found=true; found=true;
} }

706
Firmware/mesh_bed_calibration.cpp
View File

@ -9,6 +9,420 @@
extern float home_retract_mm_ext(int axis); 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) 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); 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]); 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. // At the current position, find the Z stop.
inline void find_bed_induction_sensor_point_z() 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; current_position[Z_AXIS] = -10;
go_to_current(homing_feedrate[Z_AXIS]/60); 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. // 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 // move up the retract distance
current_position[Z_AXIS] += home_retract_mm_ext(Z_AXIS); 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; current_position[Z_AXIS] -= home_retract_mm_ext(Z_AXIS) * 2;
go_to_current(homing_feedrate[Z_AXIS]/(4*60)); 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. // 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_endstops(endstops_enabled);
enable_z_endstop(endstop_z_enabled); enable_z_endstop(endstop_z_enabled);
@ -317,6 +737,7 @@ inline bool improve_bed_induction_sensor_point()
enable_endstops(true); enable_endstops(true);
go_xy(center_old_x, center_old_y, feedrate); go_xy(center_old_x, center_old_y, feedrate);
update_current_position_xyz(); update_current_position_xyz();
// if (! endstop_z_hit_on_purpose()) return false;
center_x += current_position[X_AXIS]; center_x += current_position[X_AXIS];
center_y += current_position[Y_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); enable_z_endstop(true);
go_xy(x1, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f); go_xy(x1, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
update_current_position_xyz(); update_current_position_xyz();
if (! endstop_z_hit_on_purpose()) if (! endstop_z_hit_on_purpose()) {
return false; current_position[X_AXIS] = center_old_x;
goto canceled;
}
a = current_position[X_AXIS]; a = current_position[X_AXIS];
enable_z_endstop(false); enable_z_endstop(false);
go_xy(x1, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f); go_xy(x1, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
enable_z_endstop(true); enable_z_endstop(true);
go_xy(x0, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f); go_xy(x0, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
update_current_position_xyz(); update_current_position_xyz();
if (! endstop_z_hit_on_purpose()) if (! endstop_z_hit_on_purpose()) {
return false; current_position[X_AXIS] = center_old_x;
goto canceled;
}
b = current_position[X_AXIS]; b = current_position[X_AXIS];
// Go to the center. // 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) { if (lift_z_on_min_y) {
// The first row of points are very close to the end stop. // 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. // 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. // and go back.
go_xyz(current_position[X_AXIS], y0, current_position[Z_AXIS], homing_feedrate[Z_AXIS] / 60.f); 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) { if (lift_z_on_min_y && (READ(Z_MIN_PIN) ^ Z_MIN_ENDSTOP_INVERTING) == 1) {
// Already triggering before we started the move. // Already triggering before we started the move.
// Shift the trigger point slightly outwards. // 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 { } else {
enable_z_endstop(true); enable_z_endstop(true);
go_xy(current_position[X_AXIS], y1, homing_feedrate[X_AXIS] / 60.f); go_xy(current_position[X_AXIS], y1, homing_feedrate[X_AXIS] / 60.f);
update_current_position_xyz(); update_current_position_xyz();
if (! endstop_z_hit_on_purpose()) if (! endstop_z_hit_on_purpose()) {
return false; current_position[Y_AXIS] = center_old_y;
goto canceled;
}
a = current_position[Y_AXIS]; a = current_position[Y_AXIS];
} }
enable_z_endstop(false); 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); enable_z_endstop(true);
go_xy(current_position[X_AXIS], y0, homing_feedrate[X_AXIS] / 60.f); go_xy(current_position[X_AXIS], y0, homing_feedrate[X_AXIS] / 60.f);
update_current_position_xyz(); update_current_position_xyz();
if (! endstop_z_hit_on_purpose()) if (! endstop_z_hit_on_purpose()) {
return false; current_position[Y_AXIS] = center_old_y;
goto canceled;
}
b = current_position[Y_AXIS]; b = current_position[Y_AXIS];
// Go to the center. // Go to the center.
@ -424,6 +854,12 @@ inline bool improve_bed_induction_sensor_point2(bool lift_z_on_min_y)
} }
return true; 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 #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. // Reusing the z_values memory for the measurement cache.
// 7x7=49 floats, good for 16 (x,y,z) vectors. // 7x7=49 floats, good for 16 (x,y,z) vectors.
float *pts = &mbl.z_values[0][0]; float *pts = &mbl.z_values[0][0];
float *vec_x = pts + 3 * 4; float *vec_x = pts + 2 * 4;
float *vec_y = vec_x + 3; float *vec_y = vec_x + 2;
float *cntr = vec_y + 3; float *cntr = vec_y + 2;
memset(pts, 0, sizeof(float) * 7 * 7); memset(pts, 0, sizeof(float) * 7 * 7);
// Let the planner use the uncorrected coordinates.
world2machine_reset();
#ifdef MESH_BED_CALIBRATION_SHOW_LCD #ifdef MESH_BED_CALIBRATION_SHOW_LCD
lcd_implementation_clear(); lcd_implementation_clear();
lcd_print_at_PGM(0, 0, MSG_FIND_BED_OFFSET_AND_SKEW_LINE1); 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_implementation_print_at(0, 2, k+1);
lcd_printPGM(MSG_FIND_BED_OFFSET_AND_SKEW_LINE3); lcd_printPGM(MSG_FIND_BED_OFFSET_AND_SKEW_LINE3);
#endif /* MESH_BED_CALIBRATION_SHOW_LCD */ #endif /* MESH_BED_CALIBRATION_SHOW_LCD */
float *pt = pts + k * 2;
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;
// Go up to z_initial. // Go up to z_initial.
go_to_current(homing_feedrate[Z_AXIS] / 60.f); go_to_current(homing_feedrate[Z_AXIS] / 60.f);
// Go to the measurement point position. // 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); go_to_current(homing_feedrate[X_AXIS] / 60.f);
if (! find_bed_induction_sensor_point_xy()) if (! find_bed_induction_sensor_point_xy())
return false; return false;
find_bed_induction_sensor_point_z(); find_bed_induction_sensor_point_z();
pt[0] = current_position[X_AXIS]; pt[0] = current_position[X_AXIS];
pt[1] = current_position[Y_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. // Start searching for the other points at 3mm above the last point.
current_position[Z_AXIS] += 3.f; current_position[Z_AXIS] += 3.f;
cntr[0] += pt[0]; cntr[0] += pt[0];
cntr[1] += pt[1]; 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. // Average the X and Y vectors. They may not be perpendicular, if the printer is built incorrectly.
{ {
float len; float len;
@ -486,8 +917,8 @@ bool find_bed_offset_and_skew()
cntr[1] *= 1.f/4.f; cntr[1] *= 1.f/4.f;
cntr[2] *= 1.f/4.f; cntr[2] *= 1.f/4.f;
// Average the X vector. // Average the X vector.
vec_x[0] = (pts[3 * 1 + 0] - pts[3 * 3 + 0]) / 2.f; vec_x[0] = (pts[2 * 1 + 0] - pts[2 * 3 + 0]) / 2.f;
vec_x[1] = (pts[3 * 1 + 1] - pts[3 * 3 + 1]) / 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]); len = sqrt(vec_x[0]*vec_x[0] + vec_x[1]*vec_x[1]);
if (0) { if (0) {
// if (len < MEAS_NUM_X_DIST) { // if (len < MEAS_NUM_X_DIST) {
@ -500,8 +931,8 @@ bool find_bed_offset_and_skew()
// Verify the maximum skew? // Verify the maximum skew?
} }
// Average the Y vector. // Average the Y vector.
vec_y[0] = (pts[3 * 2 + 0] - pts[3 * 0 + 0]) / 2.f; vec_y[0] = (pts[2 * 2 + 0] - pts[2 * 0 + 0]) / 2.f;
vec_y[1] = (pts[3 * 2 + 1] - pts[3 * 0 + 1]) / 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]); len = sqrt(vec_y[0]*vec_y[0] + vec_y[1]*vec_y[1]);
if (0) { if (0) {
// if (len < MEAS_NUM_Y_DIST) { // if (len < MEAS_NUM_Y_DIST) {
@ -539,6 +970,25 @@ bool find_bed_offset_and_skew()
SERIAL_ECHOLN(""); SERIAL_ECHOLN("");
#endif #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; return true;
} }
@ -552,6 +1002,16 @@ bool improve_bed_offset_and_skew(int8_t method)
float *cntr = vec_y + 2; float *cntr = vec_y + 2;
memset(pts, 0, sizeof(float) * 7 * 7); 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 endstops_enabled = enable_endstops(false);
bool endstop_z_enabled = enable_z_endstop(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. // Collect a matrix of 9x9 points.
for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) { 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. // Print the decrasing ID of the measurement point.
#ifdef MESH_BED_CALIBRATION_SHOW_LCD #ifdef MESH_BED_CALIBRATION_SHOW_LCD
lcd_print_at_PGM(0, 1, MSG_IMPROVE_BED_OFFSET_AND_SKEW_LINE2); 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_current(homing_feedrate[Z_AXIS]/60);
// Go to the measurement point. // Go to the measurement point.
// Use the coorrected coordinate, which is a result of find_bed_offset_and_skew(). // 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); go_to_current(homing_feedrate[X_AXIS]/60);
// Find its Z position by running the normal vertical search. // Find its Z position by running the normal vertical search.
// delay_keep_alive(3000); // delay_keep_alive(3000);
@ -587,15 +1048,21 @@ bool improve_bed_offset_and_skew(int8_t method)
// delay_keep_alive(3000); // delay_keep_alive(3000);
// Improve the point position by searching its center in a current plane. // Improve the point position by searching its center in a current plane.
int8_t n_errors = 3; int8_t n_errors = 3;
for (int8_t iter = 0; iter < 4; ++ iter) { for (int8_t iter = 0; iter < 8; ) {
bool found = false; bool found = false;
switch (method) { switch (method) {
case 0: found = improve_bed_induction_sensor_point(); break; 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; default: break;
} }
if (! found) { if (found) {
if (n_errors -- == 0) { 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. // Give up.
goto canceled; goto canceled;
} else { } else {
@ -606,15 +1073,28 @@ bool improve_bed_offset_and_skew(int8_t method)
go_to_current(homing_feedrate[Z_AXIS]); go_to_current(homing_feedrate[Z_AXIS]);
} }
} }
} delay_keep_alive(3000);
// 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];
} }
// 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 X and Y vectors. They may not be perpendicular, if the printer is built incorrectly.
// Average the center point. // Average the center point.
cntr[0] *= 1.f/9.f; cntr[0] *= 1.f/9.f;
@ -625,7 +1105,6 @@ bool improve_bed_offset_and_skew(int8_t method)
// Average the Y vector. // 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[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; 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 #if 1
// Fearlessly store the calibration values into the eeprom. // 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+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 +0), vec_y[0]);
eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +4), vec_y[1]); eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +4), vec_y[1]);
#endif #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 #if 0
// and let us know the result. // and let us know the result.
@ -659,105 +1171,9 @@ bool improve_bed_offset_and_skew(int8_t method)
return true; return true;
canceled: canceled:
// Store the identity matrix to EEPROM.
reset_bed_offset_and_skew();
enable_endstops(endstops_enabled); enable_endstops(endstops_enabled);
enable_z_endstop(endstop_z_enabled); enable_z_endstop(endstop_z_enabled);
return false; 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

23
Firmware/mesh_bed_calibration.h
View File

@ -1,6 +1,29 @@
#ifndef MESH_BED_CALIBRATION_H #ifndef MESH_BED_CALIBRATION_H
#define 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 void find_bed_induction_sensor_point_z();
extern bool find_bed_induction_sensor_point_xy(); extern bool find_bed_induction_sensor_point_xy();

45
Firmware/mesh_bed_leveling.cpp
View File

@ -1,4 +1,5 @@
#include "mesh_bed_leveling.h" #include "mesh_bed_leveling.h"
#include "mesh_bed_calibration.h"
#include "Configuration.h" #include "Configuration.h"
#ifdef MESH_BED_LEVELING #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) void mesh_bed_leveling::get_meas_xy(int ix, int iy, float &x, float &y, bool use_default)
{ {
#if 0
float cntr[2] = { float cntr[2] = {
eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_CENTER+0)), eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_CENTER+0)),
eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_CENTER+4)) 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(""); SERIAL_ECHOLN("");
#endif #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 #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 // Works for an odd number of MESH_NUM_X_POINTS and MESH_NUM_Y_POINTS
// #define MBL_BILINEAR
void mesh_bed_leveling::upsample_3x3() void mesh_bed_leveling::upsample_3x3()
{ {
int idx0 = 0; int idx0 = 0;
@ -106,9 +116,16 @@ void mesh_bed_leveling::upsample_3x3()
if (i == idx1) if (i == idx1)
continue; continue;
float x = get_x(i); float x = get_x(i);
z_values[j][i] = z_values[j][idx0] * (x - x1) * (x - x2) / ((x0 - x1) * (x0 - x2)) + #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][idx1] * (x - x0) * (x - x2) / ((x1 - x0) * (x1 - x2)) +
z_values[j][idx2] * (x - x0) * (x - x1) / ((x2 - x0) * (x2 - x1)); 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) if (j == idx1)
continue; continue;
float y = get_y(j); float y = get_y(j);
z_values[j][i] = z_values[idx0][i] * (y - y1) * (y - y2) / ((y0 - y1) * (y0 - y2)) + #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[idx1][i] * (y - y0) * (y - y2) / ((y1 - y0) * (y1 - y2)) +
z_values[idx2][i] * (y - y0) * (y - y1) / ((y2 - y0) * (y2 - y1)); 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 #endif

8
Firmware/pins.h
View File

@ -104,11 +104,7 @@
#define KILL_PIN -1 //80 with Smart Controller LCD #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 SUICIDE_PIN -1 //PIN that has to be turned on right after start, to keep power flowing.
#define SDPOWER -1 #define SDPOWER -1
#ifdef BARICUDA
#define HEATER_2_PIN 6
#else
#define HEATER_2_PIN -1 #define HEATER_2_PIN -1
#endif
#ifdef MINI_RAMBO #ifdef MINI_RAMBO
#define ELECTRONICS "RAMBo13a" #define ELECTRONICS "RAMBo13a"
@ -282,11 +278,7 @@
#define KILL_PIN -1 //80 with Smart Controller LCD #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 SUICIDE_PIN -1 //PIN that has to be turned on right after start, to keep power flowing.
#define SDPOWER -1 #define SDPOWER -1
#ifdef BARICUDA
#define HEATER_2_PIN 6
#else
#define HEATER_2_PIN -1 #define HEATER_2_PIN -1
#endif
#define HEATER_0_PIN 3 #define HEATER_0_PIN 3
#define HEATER_BED_PIN 4 #define HEATER_BED_PIN 4

93
Firmware/planner.cpp
View File

@ -60,6 +60,7 @@
#ifdef MESH_BED_LEVELING #ifdef MESH_BED_LEVELING
#include "mesh_bed_leveling.h" #include "mesh_bed_leveling.h"
#include "mesh_bed_calibration.h"
#endif #endif
//=========================================================================== //===========================================================================
@ -458,20 +459,12 @@ void check_axes_activity()
unsigned char z_active = 0; unsigned char z_active = 0;
unsigned char e_active = 0; unsigned char e_active = 0;
unsigned char tail_fan_speed = fanSpeed; 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; block_t *block;
if(block_buffer_tail != block_buffer_head) if(block_buffer_tail != block_buffer_head)
{ {
uint8_t block_index = block_buffer_tail; uint8_t block_index = block_buffer_tail;
tail_fan_speed = block_buffer[block_index].fan_speed; 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) while(block_index != block_buffer_head)
{ {
block = &block_buffer[block_index]; block = &block_buffer[block_index];
@ -515,16 +508,6 @@ void check_axes_activity()
#ifdef AUTOTEMP #ifdef AUTOTEMP
getHighESpeed(); getHighESpeed();
#endif #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 // 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 // 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. // 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) 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 // Calculate the buffer head after we push this byte
int next_buffer_head = next_block_index(block_buffer_head); 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); apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
#endif // ENABLE_AUTO_BED_LEVELING #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 // The target position of the tool in absolute steps
// Calculate target position 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 //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; block->fan_speed = fanSpeed;
#ifdef BARICUDA
block->valve_pressure = ValvePressure;
block->e_to_p_pressure = EtoPPressure;
#endif
// Compute direction bits for this block // Compute direction bits for this block
block->direction_bits = 0; block->direction_bits = 0;
@ -1061,15 +1083,19 @@ vector_3 plan_get_position() {
} }
#endif // ENABLE_AUTO_BED_LEVELING #endif // ENABLE_AUTO_BED_LEVELING
#ifdef ENABLE_AUTO_BED_LEVELING
void plan_set_position(float x, float y, float z, const float &e) void plan_set_position(float x, float y, float z, const float &e)
{ {
#ifdef ENABLE_AUTO_BED_LEVELING
apply_rotation_xyz(plan_bed_level_matrix, x, y, z); 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)
{
#endif // ENABLE_AUTO_BED_LEVELING #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[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
position[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]); position[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
@ -1091,6 +1117,13 @@ void plan_set_position(const float &x, const float &y, const float &z, const flo
previous_speed[3] = 0.0; 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) void plan_set_e_position(const float &e)
{ {
position[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]); position[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);

16
Firmware/planner.h
View File

@ -64,10 +64,6 @@ typedef struct {
unsigned long final_rate; // The minimal rate at exit unsigned long final_rate; // The minimal rate at exit
unsigned long acceleration_st; // acceleration steps/sec^2 unsigned long acceleration_st; // acceleration steps/sec^2
unsigned long fan_speed; unsigned long fan_speed;
#ifdef BARICUDA
unsigned long valve_pressure;
unsigned long e_to_p_pressure;
#endif
volatile char busy; volatile char busy;
} block_t; } 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 // Get the position applying the bed level matrix if enabled
vector_3 plan_get_position(); vector_3 plan_get_position();
#else #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 #endif // ENABLE_AUTO_BED_LEVELING
// Set position. Used for G92 instructions. // 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); void plan_set_position(float x, float y, float z, const float &e);
#else //#else
void plan_set_position(const float &x, const float &y, const float &z, const float &e); //void plan_set_position(const float &x, const float &y, const float &z, const float &e);
#endif // ENABLE_AUTO_BED_LEVELING //#endif // ENABLE_AUTO_BED_LEVELING
void plan_set_z_position(const float z);
void plan_set_e_position(const float &e); void plan_set_e_position(const float &e);

36
Firmware/stepper.cpp
View File

@ -1197,7 +1197,6 @@ void babystep(const uint8_t axis,const bool direction)
} }
break; break;
#ifndef DELTA
case Z_AXIS: case Z_AXIS:
{ {
enable_z(); enable_z();
@ -1229,41 +1228,6 @@ void babystep(const uint8_t axis,const bool direction)
} }
break; 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; default: break;
} }

28
Firmware/ultralcd.cpp
View File

@ -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. */ /* Different types of actions that can be used in menu items. */
static void menu_action_back(menuFunc_t data); static void menu_action_back(menuFunc_t data);
static void menu_action_submenu(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_function(menuFunc_t data);
static void menu_action_setlang(unsigned char lang); 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_sddirectory(const char* filename, char* longFilename);
static void menu_action_setting_edit_bool(const char* pstr, bool* ptr); 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); 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 (min_software_endstops && current_position[axis] < min) current_position[axis] = min;
if (max_software_endstops && current_position[axis] > max) current_position[axis] = max; if (max_software_endstops && current_position[axis] > max) current_position[axis] = max;
encoderPosition = 0; 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); 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; lcdDrawUpdate = 1;
} }
if (lcdDrawUpdate) lcd_implementation_drawedit(name, ftostr31(current_position[axis])); 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; current_position[E_AXIS] += float((int)encoderPosition) * move_menu_scale;
encoderPosition = 0; 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); 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; lcdDrawUpdate = 1;
} }
if (lcdDrawUpdate) if (lcdDrawUpdate)
@ -1543,7 +1531,7 @@ void lcd_mesh_calibration()
void lcd_mesh_calibration_reset() void lcd_mesh_calibration_reset()
{ {
enquecommand_P(PSTR("M44")); enquecommand_P(PSTR("M45"));
lcd_return_to_status(); lcd_return_to_status();
} }
@ -2520,26 +2508,12 @@ static void menu_action_back(menuFunc_t data) {
static void menu_action_submenu(menuFunc_t data) { static void menu_action_submenu(menuFunc_t data) {
lcd_goto_menu(data); lcd_goto_menu(data);
} }
static void menu_action_gcode(const char* pgcode) {
enquecommand_P(pgcode);
}
static void menu_action_setlang(unsigned char lang) { static void menu_action_setlang(unsigned char lang) {
lcd_set_lang(lang); lcd_set_lang(lang);
} }
static void menu_action_function(menuFunc_t data) { static void menu_action_function(menuFunc_t data) {
(*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) static void menu_action_sddirectory(const char* filename, char* longFilename)
{ {
card.chdir(filename); card.chdir(filename);