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.

Firmware/Configuration.h

@@ -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)

Firmware/ConfigurationStore.cpp

@@ -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;

Firmware/Configuration_adv.h

@@ -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

Firmware/Marlin.h

@@ -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;

Firmware/MarlinSerial.cpp

@@ -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
 

Firmware/MarlinSerial.h

@@ -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;
+                }
+            }
         }
-      }
     }
     
     

Firmware/Marlin_main.cpp

@@ -204,23 +204,12 @@
 // M509 - force language selection on next restart
 // M540 - Use S[0|1] to enable or disable the stop SD card print on endstop hit (requires ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
 // M600 - Pause for filament change X[pos] Y[pos] Z[relative lift] E[initial retract] L[later retract distance for removal]
-// M665 - set delta configurations
-// M666 - set delta endstop adjustment
 // M605 - Set dual x-carriage movement mode: S<mode> [ X<duplication x-offset> R<duplication temp offset> ]
 // M907 - Set digital trimpot motor current using axis codes.
 // M908 - Control digital trimpot directly.
 // M350 - Set microstepping mode.
 // M351 - Toggle MS1 MS2 pins directly.
 
-// ************ SCARA Specific - This can change to suit future G-code regulations
-// M360 - SCARA calibration: Move to cal-position ThetaA (0 deg calibration)
-// M361 - SCARA calibration: Move to cal-position ThetaB (90 deg calibration - steps per degree)
-// M362 - SCARA calibration: Move to cal-position PsiA (0 deg calibration)
-// M363 - SCARA calibration: Move to cal-position PsiB (90 deg calibration - steps per degree)
-// M364 - SCARA calibration: Move to cal-position PSIC (90 deg to Theta calibration position)
-// M365 - SCARA calibration: Scaling factor, X, Y, Z axis
-//************* SCARA End ***************
-
 // M928 - Start SD logging (M928 filename.g) - ended by M29
 // M999 - Restart after being stopped by error
 
@@ -298,9 +287,6 @@ float volumetric_multiplier[EXTRUDERS] = {1.0
 };
 float current_position[NUM_AXIS] = { 0.0, 0.0, 0.0, 0.0 };
 float add_homing[3]={0,0,0};
-#ifdef DELTA
-float endstop_adj[3]={0,0,0};
-#endif
 
 float min_pos[3] = { X_MIN_POS, Y_MIN_POS, Z_MIN_POS };
 float max_pos[3] = { X_MAX_POS, Y_MAX_POS, Z_MAX_POS };
@@ -322,14 +308,6 @@ float extruder_offset[NUM_EXTRUDER_OFFSETS][EXTRUDERS] = {
 #endif
 uint8_t active_extruder = 0;
 int fanSpeed=0;
-#ifdef SERVO_ENDSTOPS
-  int servo_endstops[] = SERVO_ENDSTOPS;
-  int servo_endstop_angles[] = SERVO_ENDSTOP_ANGLES;
-#endif
-#ifdef BARICUDA
-int ValvePressure=0;
-int EtoPPressure=0;
-#endif
 
 #ifdef FWRETRACT
   bool autoretract_enabled=false;
@@ -367,27 +345,6 @@ int EtoPPressure=0;
   #endif
 #endif
 
-#ifdef DELTA
-  float delta[3] = {0.0, 0.0, 0.0};
-  #define SIN_60 0.8660254037844386
-  #define COS_60 0.5
-  // these are the default values, can be overriden with M665
-  float delta_radius= DELTA_RADIUS;
-  float delta_tower1_x= -SIN_60*delta_radius; // front left tower
-  float delta_tower1_y= -COS_60*delta_radius;	   
-  float delta_tower2_x=  SIN_60*delta_radius; // front right tower
-  float delta_tower2_y= -COS_60*delta_radius;	   
-  float delta_tower3_x= 0.0;                  // back middle tower
-  float delta_tower3_y= delta_radius;
-  float delta_diagonal_rod= DELTA_DIAGONAL_ROD;
-  float delta_diagonal_rod_2= sq(delta_diagonal_rod);
-  float delta_segments_per_second= DELTA_SEGMENTS_PER_SECOND;
-#endif
-
-#ifdef SCARA                              // Build size scaling
-float axis_scaling[3]={1,1,1};  // Build size scaling, default to 1
-#endif				
-
 bool cancel_heatup = false ;
 
 #ifdef FILAMENT_SENSOR
@@ -411,9 +368,7 @@ const char echomagic[] PROGMEM = "echo:";
 const char axis_codes[NUM_AXIS] = {'X', 'Y', 'Z', 'E'};
 float destination[NUM_AXIS] = {  0.0, 0.0, 0.0, 0.0};
 
-#ifndef DELTA
 static float delta[3] = {0.0, 0.0, 0.0};
-#endif
 
 // For tracing an arc
 static float offset[3] = {0.0, 0.0, 0.0};
@@ -425,14 +380,44 @@ static long gcode_N, gcode_LastN, Stopped_gcode_LastN = 0;
 // Also there is bool axis_relative_modes[] per axis flag.
 static bool relative_mode = false;  
 
-static char cmdbuffer[BUFSIZE][MAX_CMD_SIZE];
-// Marking a line in the cmdbuffer. If false, the command is confirmed by sending an "OK" on the serial line.
-static bool fromsd[BUFSIZE];
+// String circular buffer. Commands may be pushed to the buffer from both sides:
+// Chained commands will be pushed to the front, interactive (from LCD menu) 
+// and printing commands (from serial line or from SD card) are pushed to the tail.
+// First character of each entry indicates the type of the entry: 
+#define CMDBUFFER_CURRENT_TYPE_UNKNOWN  0
+// Command in cmdbuffer was sent over USB.
+#define CMDBUFFER_CURRENT_TYPE_USB      1
+// Command in cmdbuffer was read from SDCARD.
+#define CMDBUFFER_CURRENT_TYPE_SDCARD   2
+// Command in cmdbuffer was generated by the UI.
+#define CMDBUFFER_CURRENT_TYPE_UI       3
+// Command in cmdbuffer was generated by another G-code.
+#define CMDBUFFER_CURRENT_TYPE_CHAINED  4
+
+// How much space to reserve for the chained commands
+// of type CMDBUFFER_CURRENT_TYPE_CHAINED,
+// which are pushed to the front of the queue?
+// Maximum 5 commands of max length 20 + null terminator.
+#define CMDBUFFER_RESERVE_FRONT       (5*21)
+// Reserve BUFSIZE lines of length MAX_CMD_SIZE plus CMDBUFFER_RESERVE_FRONT.
+static char cmdbuffer[BUFSIZE * (MAX_CMD_SIZE + 1) + CMDBUFFER_RESERVE_FRONT];
+// Head of the circular buffer, where to read.
 static int bufindr = 0;
+// Tail of the buffer, where to write.
 static int bufindw = 0;
+// Number of lines in cmdbuffer.
 static int buflen = 0;
-//static int i = 0;
-static char serial_char;
+// Flag for processing the current command inside the main Arduino loop().
+// If a new command was pushed to the front of a command buffer while
+// processing another command, this replaces the command on the top.
+// Therefore don't remove the command from the queue in the loop() function.
+static bool cmdbuffer_front_already_processed = false;
+
+// Type of a command, which is to be executed right now.
+#define CMDBUFFER_CURRENT_TYPE   (cmdbuffer[bufindr])
+// String of a command, which is to be executed right now.
+#define CMDBUFFER_CURRENT_STRING (cmdbuffer+bufindr+1)
+
 static int serial_count = 0;
 static boolean comment_mode = false;
 static char *strchr_pointer; // just a pointer to find chars in the command string like X, Y, Z, E, etc
@@ -505,37 +490,236 @@ void serial_echopair_P(const char *s_P, unsigned long v)
   }
 #endif //!SDSUPPORT
 
+// Pop the currently processed command from the queue.
+// It is expected, that there is at least one command in the queue.
+void cmdqueue_pop_front()
+{
+    if (buflen > 0) {
+        SERIAL_ECHOPGM("Dequeing ");
+        SERIAL_ECHO(cmdbuffer+bufindr+1);
+        SERIAL_ECHOLNPGM("");
+        SERIAL_ECHOPGM("Old indices: buflen ");
+        SERIAL_ECHO(buflen);
+        SERIAL_ECHOPGM(", bufindr ");
+        SERIAL_ECHO(bufindr);
+        SERIAL_ECHOPGM(", bufindw ");
+        SERIAL_ECHO(bufindw);
+        SERIAL_ECHOPGM(", serial_count ");
+        SERIAL_ECHO(serial_count);
+        SERIAL_ECHOPGM(", bufsize ");
+        SERIAL_ECHO(sizeof(cmdbuffer));
+        SERIAL_ECHOLNPGM("");
+        if (-- buflen == 0) {
+            // Empty buffer.
+            if (serial_count == 0)
+                // No serial communication is pending. Reset both pointers to zero.
+                bufindw = 0;
+            bufindr = bufindw;
+        } else {
+            // There is at least one ready line in the buffer.
+            // First skip the current command ID and iterate up to the end of the string.
+            for (++ bufindr; cmdbuffer[bufindr] != 0; ++ bufindr) ;
+            // Second, skip the end of string null character and iterate until a nonzero command ID is found.
+            for (++ bufindr; bufindr < sizeof(cmdbuffer) && cmdbuffer[bufindr] == 0; ++ bufindr) ;
+            // If the end of the buffer was empty,
+            if (bufindr == sizeof(cmdbuffer)) {
+                // skip to the start and find the nonzero command.
+                for (bufindr = 0; cmdbuffer[bufindr] == 0; ++ bufindr) ;
+            }
+            SERIAL_ECHOPGM("New indices: buflen ");
+            SERIAL_ECHO(buflen);
+            SERIAL_ECHOPGM(", bufindr ");
+            SERIAL_ECHO(bufindr);
+            SERIAL_ECHOPGM(", bufindw ");
+            SERIAL_ECHO(bufindw);
+            SERIAL_ECHOPGM(", serial_count ");
+            SERIAL_ECHO(serial_count);
+            SERIAL_ECHOPGM(" new command on the top: ");
+            SERIAL_ECHO(cmdbuffer+bufindr+1);
+            SERIAL_ECHOLNPGM("");
+        }
+    }
+}
+
+// How long a string could be pushed to the front of the command queue?
+// If yes, adjust bufindr to the new position, where the new command could be enqued.
+// len_asked does not contain the zero terminator size.
+bool cmdqueue_could_enqueue_front(int len_asked)
+{
+    // MAX_CMD_SIZE has to accommodate the zero terminator.
+    if (len_asked >= MAX_CMD_SIZE)
+        return false;
+    // Remove the currently processed command from the queue.
+    if (! cmdbuffer_front_already_processed) {
+        cmdqueue_pop_front();
+        cmdbuffer_front_already_processed = true;
+    }
+    if (bufindr == bufindw && buflen > 0)
+        // Full buffer.
+        return false;
+    // Adjust the end of the write buffer based on whether a partial line is in the receive buffer.
+    int endw = (serial_count > 0) ? (bufindw + MAX_CMD_SIZE + 1) : bufindw;
+    if (bufindw < bufindr)
+        // Simple case. There is a contiguous space between the write buffer and the read buffer.
+        return endw + len_asked + 2 < bufindr;
+    // Otherwise the free space is split between the start and end.
+    if (len_asked + 2 <= bufindr) {
+        // Could fit at the start.
+        bufindr -= len_asked + 2;
+        return true;
+    }
+    int bufindr_new = sizeof(cmdbuffer) - len_asked - 2;
+    if (endw <= bufindr_new) {
+        memset(cmdbuffer, 0, bufindr);
+        bufindr = bufindr_new;
+        return true;
+    }
+    return false;
+}
+
+// Could one enqueue a command of lenthg len_asked into the buffer,
+// while leaving CMDBUFFER_RESERVE_FRONT at the start?
+// If yes, adjust bufindw to the new position, where the new command could be enqued.
+// len_asked does not contain the zero terminator size.
+bool cmdqueue_could_enqueue_back(int len_asked)
+{
+    // MAX_CMD_SIZE has to accommodate the zero terminator.
+    if (len_asked >= MAX_CMD_SIZE)
+        return false;
+
+    if (bufindr == bufindw && buflen > 0)
+        // Full buffer.
+        return false;
+
+    if (serial_count > 0) {
+        // If there is some data stored starting at bufindw, len_asked is certainly smaller than
+        // the allocated data buffer. Try to reserve a new buffer and to move the already received
+        // serial data.
+        // How much memory to reserve for the commands pushed to the front?
+        // End of the queue, when pushing to the end.
+        int endw = bufindw + len_asked + 2;
+        if (bufindw < bufindr)
+            // Simple case. There is a contiguous space between the write buffer and the read buffer.
+            return endw + CMDBUFFER_RESERVE_FRONT <= bufindr;
+        // Otherwise the free space is split between the start and end.
+        if (// Could one fit to the end, including the reserve?
+            endw + CMDBUFFER_RESERVE_FRONT <= sizeof(cmdbuffer) ||
+            // Could one fit to the end, and the reserve to the start?
+            (endw <= sizeof(cmdbuffer) && CMDBUFFER_RESERVE_FRONT <= bufindr))
+            return true;
+        // Could one fit both to the start?
+        if (len_asked + 2 + CMDBUFFER_RESERVE_FRONT <= bufindr) {
+            // Mark the rest of the buffer as used.
+            memset(cmdbuffer+bufindw, 0, sizeof(cmdbuffer)-bufindw);
+            // and point to the start.
+            bufindw = 0;
+            return true;
+        }
+    } else {
+        // How much memory to reserve for the commands pushed to the front?
+        // End of the queue, when pushing to the end.
+        int endw = bufindw + len_asked + 2;
+        if (bufindw < bufindr)
+            // Simple case. There is a contiguous space between the write buffer and the read buffer.
+            return endw + CMDBUFFER_RESERVE_FRONT <= bufindr;
+        // Otherwise the free space is split between the start and end.
+        if (// Could one fit to the end, including the reserve?
+            endw + CMDBUFFER_RESERVE_FRONT <= sizeof(cmdbuffer) ||
+            // Could one fit to the end, and the reserve to the start?
+            (endw <= sizeof(cmdbuffer) && CMDBUFFER_RESERVE_FRONT <= bufindr))
+            return true;
+        // Could one fit both to the start?
+        if (len_asked + 2 + CMDBUFFER_RESERVE_FRONT <= bufindr) {
+            // Mark the rest of the buffer as used.
+            memset(cmdbuffer+bufindw, 0, sizeof(cmdbuffer)-bufindw);
+            // and point to the start.
+            bufindw = 0;
+            return true;
+        }
+    }
+    return false;
+}
+
+void cmdqueue_dump_to_serial()
+{
+    SERIAL_ECHOLNPGM("Content of the buffer: ");
+    if (buflen == 0) {
+        SERIAL_ECHOLNPGM("The command buffer is empty.");
+    } else {
+        SERIAL_ECHOPGM("Number of entries: ");
+        SERIAL_ECHO(buflen);
+        SERIAL_ECHOLNPGM("");
+    }
+    if (bufindr < bufindw) {
+
+    } else {
+//        for (uint8_t i = 0; i < BUFSIZE; ++ i)
+//            SERIAL_ECHO(cmdbuffer[(i+bufindw)%BUFSIZE]);
+    }
+    SERIAL_ECHOLNPGM("End of the buffer.");
+}
+
 //adds an command to the main command buffer
 //thats really done in a non-safe way.
 //needs overworking someday
-void enquecommand(const char *cmd)
+// Currently the maximum length of a command piped through this function is around 20 characters
+void enquecommand(const char *cmd, bool from_progmem)
 {
-  if(buflen < BUFSIZE)
-  {
-    //this is dangerous if a mixing of serial and this happens
-    strcpy(&(cmdbuffer[bufindw][0]),cmd);
-    SERIAL_ECHO_START;
-    SERIAL_ECHORPGM(MSG_Enqueing);
-    SERIAL_ECHO(cmdbuffer[bufindw]);
-    SERIAL_ECHOLNPGM("\"");
-    bufindw= (bufindw + 1)%BUFSIZE;
-    buflen += 1;
-  }
+    int len = from_progmem ? strlen_P(cmd) : strlen(cmd);
+    // Does cmd fit the queue while leaving sufficient space at the front for the chained commands?
+    // If it fits, it may move bufindw, so it points to a contiguous buffer, which fits cmd.
+    if (cmdqueue_could_enqueue_back(len)) {
+        // This is dangerous if a mixing of serial and this happens
+        // This may easily be tested: If serial_count > 0, we have a problem.
+        cmdbuffer[bufindw] = CMDBUFFER_CURRENT_TYPE_UI;
+        if (from_progmem)
+            strcpy_P(cmdbuffer + bufindw + 1, cmd);
+        else
+            strcpy(cmdbuffer + bufindw + 1, cmd);
+        SERIAL_ECHO_START;
+        SERIAL_ECHORPGM(MSG_Enqueing);
+        SERIAL_ECHO(cmdbuffer + bufindw + 1);
+        SERIAL_ECHOLNPGM("\"");
+        bufindw += len + 2;
+        if (bufindw == sizeof(cmdbuffer))
+            bufindw = 0;
+        ++ buflen;
+    } else {
+        SERIAL_ECHO_START;
+        SERIAL_ECHORPGM(MSG_Enqueing);
+        if (from_progmem)
+            SERIAL_PROTOCOLRPGM(cmd);
+        else
+            SERIAL_ECHO(cmd);
+        SERIAL_ECHOLNPGM("\" failed: Buffer full!");
+        cmdqueue_dump_to_serial();
+    }
 }
 
-void enquecommand_P(const char *cmd)
+void enquecommand_front(const char *cmd, bool from_progmem)
 {
-  if(buflen < BUFSIZE)
-  {
-    //this is dangerous if a mixing of serial and this happens
-    strcpy_P(&(cmdbuffer[bufindw][0]),cmd);
-    SERIAL_ECHO_START;
-    SERIAL_ECHORPGM(MSG_Enqueing);
-    SERIAL_ECHO(cmdbuffer[bufindw]);
-    SERIAL_ECHOLNPGM("\"");
-    bufindw= (bufindw + 1)%BUFSIZE;
-    buflen += 1;
-  }
+    int len = from_progmem ? strlen_P(cmd) : strlen(cmd);
+    // Does cmd fit the queue? This call shall move bufindr, so the command may be copied.
+    if (cmdqueue_could_enqueue_front(len)) {
+        cmdbuffer[bufindr] = CMDBUFFER_CURRENT_TYPE_UI;
+        if (from_progmem)
+            strcpy_P(cmdbuffer + bufindr + 1, cmd);
+        else
+            strcpy(cmdbuffer + bufindr + 1, cmd);
+        SERIAL_ECHO_START;
+        SERIAL_ECHOPGM("Enqueing to the front: \"");
+        SERIAL_ECHO(cmdbuffer + bufindr + 1);
+        SERIAL_ECHOLNPGM("\"");
+    } else {
+        SERIAL_ECHO_START;
+        SERIAL_ECHOPGM("Enqueing to the front: \"");
+        if (from_progmem)
+            SERIAL_PROTOCOLRPGM(cmd);
+        else
+            SERIAL_ECHO(cmd);
+        SERIAL_ECHOLNPGM("\" failed: Buffer full!");
+        cmdqueue_dump_to_serial();
+    }
 }
 
 void setup_killpin()
@@ -546,16 +730,6 @@ void setup_killpin()
   #endif
 }
 
-// Set home pin
-void setup_homepin(void)
-{
-#if defined(HOME_PIN) && HOME_PIN > -1
-   SET_INPUT(HOME_PIN);
-   WRITE(HOME_PIN,HIGH);
-#endif
-}
-
-
 void setup_photpin()
 {
   #if defined(PHOTOGRAPH_PIN) && PHOTOGRAPH_PIN > -1
@@ -605,21 +779,6 @@ void servo_init()
   #if (NUM_SERVOS >= 5)
     #error "TODO: enter initalisation code for more servos"
   #endif
-
-  // Set position of Servo Endstops that are defined
-  #ifdef SERVO_ENDSTOPS
-  for(int8_t i = 0; i < 3; i++)
-  {
-    if(servo_endstops[i] > -1) {
-      servos[servo_endstops[i]].write(servo_endstop_angles[i * 2 + 1]);
-    }
-  }
-  #endif
-
-  #if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
-  delay(PROBE_SERVO_DEACTIVATION_DELAY);
-  servos[servo_endstops[Z_AXIS]].detach();
-  #endif
 }
 
 static void lcd_language_menu();
@@ -629,7 +788,9 @@ static void lcd_language_menu();
    enum MeshLevelingState { MeshReport, MeshStart, MeshNext, MeshSet };
 #endif
 
-
+// "Setup" function is called by the Arduino framework on startup.
+// Before startup, the Timers-functions (PWM)/Analog RW and HardwareSerial provided by the Arduino-code 
+// are initialized by the main() routine provided by the Arduino framework.
 void setup()
 {
   setup_killpin();
@@ -681,10 +842,6 @@ void setup()
   SERIAL_ECHO(freeMemory());
   SERIAL_ECHORPGM(MSG_PLANNER_BUFFER_BYTES);
   SERIAL_ECHOLN((int)sizeof(block_t)*BLOCK_BUFFER_SIZE);
-  for(int8_t i = 0; i < BUFSIZE; i++)
-  {
-    fromsd[i] = false;
-  }
   
   // loads data from EEPROM if available else uses defaults (and resets step acceleration rate)
   Config_RetrieveSettings();
@@ -695,7 +852,9 @@ void setup()
   st_init();    // Initialize stepper, this enables interrupts!
   setup_photpin();
   servo_init();
-  
+  // Reset the machine correction matrix.
+  // It does not make sense to load the correction matrix until the machine is homed.
+  world2machine_reset();
 
   lcd_init();
   if(!READ(BTN_ENC) ){
@@ -723,10 +882,6 @@ void setup()
   #ifdef DIGIPOT_I2C
     digipot_i2c_init();
   #endif
-#ifdef Z_PROBE_SLED
-  pinMode(SERVO0_PIN, OUTPUT);
-  digitalWrite(SERVO0_PIN, LOW); // turn it off
-#endif // Z_PROBE_SLED
   setup_homepin();
 
 #if defined(Z_AXIS_ALWAYS_ON)
@@ -734,8 +889,8 @@ void setup()
 #endif
 }
 
-//unsigned char first_run_ever=1;
-//void first_time_menu();
+// The loop() function is called in an endless loop by the Arduino framework from the default main() routine.
+// Before loop(), the setup() function is called by the main() routine.
 void loop()
 {
 
@@ -750,10 +905,8 @@ void loop()
 		is_usb_printing = false;
 	}
 
+  get_command();
 
-
-  if(buflen < (BUFSIZE-1))
-    get_command();
   #ifdef SDSUPPORT
   card.checkautostart(false);
   #endif
@@ -762,33 +915,27 @@ void loop()
     #ifdef SDSUPPORT
       if(card.saving)
       {
-        if(strstr_P(cmdbuffer[bufindr], PSTR("M29")) == NULL)
-        {
-          card.write_command(cmdbuffer[bufindr]);
+        // Saving a G-code file onto an SD-card is in progress.
+        // Saving starts with M28, saving until M29 is seen.
+        if(strstr_P(CMDBUFFER_CURRENT_STRING, PSTR("M29")) == NULL) {
+          card.write_command(CMDBUFFER_CURRENT_STRING);
           if(card.logging)
-          {
             process_commands();
-          }
           else
-          {
            SERIAL_PROTOCOLLNRPGM(MSG_OK);
-          }
-        }
-        else
-        {
+        } else {
           card.closefile();
           SERIAL_PROTOCOLLNRPGM(MSG_FILE_SAVED);
         }
-      }
-      else
-      {
+      } else {
         process_commands();
       }
     #else
       process_commands();
     #endif //SDSUPPORT
-    buflen = (buflen-1);
-    bufindr = (bufindr + 1)%BUFSIZE;
+      if (! cmdbuffer_front_already_processed)
+          cmdqueue_pop_front();
+      cmdbuffer_front_already_processed = false;
   }
   //check heater every n milliseconds
   manage_heater();
@@ -799,8 +946,12 @@ void loop()
 
 void get_command()
 {
-  while( MYSERIAL.available() > 0  && buflen < BUFSIZE) {
-    serial_char = MYSERIAL.read();
+    // Test and reserve space for the new command string.
+    if (! cmdqueue_could_enqueue_back(MAX_CMD_SIZE-1))
+        return;
+
+  while (MYSERIAL.available() > 0) {
+    char serial_char = MYSERIAL.read();
     if(serial_char == '\n' ||
        serial_char == '\r' ||
        (serial_char == ':' && comment_mode == false) ||
@@ -810,15 +961,17 @@ void get_command()
         comment_mode = false; //for new command
         return;
       }
-      cmdbuffer[bufindw][serial_count] = 0; //terminate string
+      cmdbuffer[bufindw+serial_count+1] = 0; //terminate string
       if(!comment_mode){
         comment_mode = false; //for new command
-        fromsd[bufindw] = false;
-        if(strchr(cmdbuffer[bufindw], 'N') != NULL)
+        if ((strchr_pointer = strchr(cmdbuffer+bufindw+1, 'N')) != NULL)
         {
-          strchr_pointer = strchr(cmdbuffer[bufindw], 'N');
-          gcode_N = (strtol(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL, 10));
-          if(gcode_N != gcode_LastN+1 && (strstr_P(cmdbuffer[bufindw], PSTR("M110")) == NULL) ) {
+          // Line number met. When sending a G-code over a serial line, each line may be stamped with its index,
+          // and Marlin tests, whether the successive lines are stamped with an increasing line number ID.
+          gcode_N = (strtol(strchr_pointer+1, NULL, 10));
+          if(gcode_N != gcode_LastN+1 && (strstr_P(cmdbuffer+bufindw+1, PSTR("M110")) == NULL) ) {
+            // M110 - set current line number.
+            // Line numbers not sent in succession.
             SERIAL_ERROR_START;
             SERIAL_ERRORRPGM(MSG_ERR_LINE_NO);
             SERIAL_ERRORLN(gcode_LastN);
@@ -828,14 +981,13 @@ void get_command()
             return;
           }
 
-          if(strchr(cmdbuffer[bufindw], '*') != NULL)
+          if((strchr_pointer = strchr(cmdbuffer+bufindw+1, '*')) != NULL)
           {
             byte checksum = 0;
-            byte count = 0;
-            while(cmdbuffer[bufindw][count] != '*') checksum = checksum^cmdbuffer[bufindw][count++];
-            strchr_pointer = strchr(cmdbuffer[bufindw], '*');
-
-            if( (int)(strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL)) != checksum) {
+            char *p = cmdbuffer+bufindw+1;
+            while (p != strchr_pointer)
+                checksum = checksum^(*p++);
+            if (int(strtol(strchr_pointer+1, NULL, 10)) != int(checksum)) {
               SERIAL_ERROR_START;
               SERIAL_ERRORRPGM(MSG_ERR_CHECKSUM_MISMATCH);
               SERIAL_ERRORLN(gcode_LastN);
@@ -843,7 +995,8 @@ void get_command()
               serial_count = 0;
               return;
             }
-            //if no errors, continue parsing
+            // If no errors, remove the checksum and continue parsing.
+            *strchr_pointer = 0;
           }
           else
           {
@@ -857,10 +1010,10 @@ void get_command()
 
           gcode_LastN = gcode_N;
           //if no errors, continue parsing
-        }
+        } // end of 'N' command
         else  // if we don't receive 'N' but still see '*'
         {
-          if((strchr(cmdbuffer[bufindw], '*') != NULL))
+          if((strchr(cmdbuffer+bufindw+1, '*') != NULL))
           {
             SERIAL_ERROR_START;
             SERIAL_ERRORRPGM(MSG_ERR_NO_LINENUMBER_WITH_CHECKSUM);
@@ -868,51 +1021,54 @@ void get_command()
             serial_count = 0;
             return;
           }
-        }
-        if((strchr(cmdbuffer[bufindw], 'G') != NULL)){
-          strchr_pointer = strchr(cmdbuffer[bufindw], 'G');
-
-		  if (!IS_SD_PRINTING)
-		  {
-			  usb_printing_counter = 10;
-			  is_usb_printing = true;
-		  }
-
-          switch((int)((strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL))))
-		  {
-			case 0:
-			case 1:
-			case 2:
-			case 3:
-				if (Stopped == true) 
-				{
-				  SERIAL_ERRORLNRPGM(MSG_ERR_STOPPED);
-				  LCD_MESSAGERPGM(MSG_STOPPED);
-				}
-				break;
-          default:
-            break;
-          }
-
-        }
+        } // end of '*' command
+        if ((strchr_pointer = strchr(cmdbuffer+bufindw+1, 'G')) != NULL) {
+      		  if (! IS_SD_PRINTING) {
+        			  usb_printing_counter = 10;
+        			  is_usb_printing = true;
+      		  }
+            if (Stopped == true) {
+                int gcode = strtol(strchr_pointer+1, NULL, 10);
+                if (gcode >= 0 && gcode <= 3) {
+                    SERIAL_ERRORLNRPGM(MSG_ERR_STOPPED);
+                    LCD_MESSAGERPGM(MSG_STOPPED);
+                }
+            }
+        } // end of 'G' command
 
         //If command was e-stop process now
-        if(strcmp(cmdbuffer[bufindw], "M112") == 0)
+        if(strcmp(cmdbuffer+bufindw+1, "M112") == 0)
           kill();
         
-        bufindw = (bufindw + 1)%BUFSIZE;
-        buflen += 1;
-      }
+        // Store the current line into buffer, move to the next line.
+        cmdbuffer[bufindw] = CMDBUFFER_CURRENT_TYPE_USB;
+        SERIAL_ECHO_START;
+        SERIAL_ECHOPGM("Storing a command line to buffer: ");
+        SERIAL_ECHO(cmdbuffer+bufindw+1);
+        SERIAL_ECHOLNPGM("");
+        bufindw += strlen(cmdbuffer+bufindw+1) + 2;
+        if (bufindw == sizeof(cmdbuffer))
+            bufindw = 0;
+        ++ buflen;
+      } // end of 'not comment mode'
       serial_count = 0; //clear buffer
-    }
-    else
-    {
+      // Don't call cmdqueue_could_enqueue_back if there are no characters waiting
+      // in the queue, as this function will reserve the memory.
+      if (MYSERIAL.available() == 0 || ! cmdqueue_could_enqueue_back(MAX_CMD_SIZE-1))
+          return;
+    } // end of "end of line" processing
+    else {
+      // Not an "end of line" symbol. Store the new character into a buffer.
       if(serial_char == ';') comment_mode = true;
-      if(!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char;
+      if(!comment_mode) cmdbuffer[bufindw+1+serial_count++] = serial_char;
     }
-  }
+  } // end of serial line processing loop
+
+
   #ifdef SDSUPPORT
   if(!card.sdprinting || serial_count!=0){
+    // If there is a half filled buffer from serial line, wait until return before
+    // continuing with the serial line.
     return;
   }
 
@@ -923,9 +1079,10 @@ void get_command()
   static bool stop_buffering=false;
   if(buflen==0) stop_buffering=false;
 
-  while( !card.eof()  && buflen < BUFSIZE && !stop_buffering) {
+  // Reads whole lines from the SD card. Never leaves a half-filled line in the cmdbuffer.
+  while( !card.eof() && !stop_buffering) {
     int16_t n=card.get();
-    serial_char = (char)n;
+    char serial_char = (char)n;
     if(serial_char == '\n' ||
        serial_char == '\r' ||
        (serial_char == '#' && comment_mode == false) ||
@@ -957,46 +1114,34 @@ void get_command()
         comment_mode = false; //for new command
         return; //if empty line
       }
-      cmdbuffer[bufindw][serial_count] = 0; //terminate string
-//      if(!comment_mode){
-        fromsd[bufindw] = true;
-        buflen += 1;
-        bufindw = (bufindw + 1)%BUFSIZE;
-//      }
+      cmdbuffer[bufindw+serial_count+1] = 0; //terminate string
+      cmdbuffer[bufindw] = CMDBUFFER_CURRENT_TYPE_SDCARD;
+      ++ buflen;
+      bufindw += strlen(cmdbuffer+bufindw+1) + 2;
+      if (bufindw == sizeof(cmdbuffer))
+          bufindw = 0;
       comment_mode = false; //for new command
       serial_count = 0; //clear buffer
+      // The following line will reserve buffer space if available.
+      if (! cmdqueue_could_enqueue_back(MAX_CMD_SIZE-1))
+          return;
     }
     else
     {
       if(serial_char == ';') comment_mode = true;
-      if(!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char;
+      if(!comment_mode) cmdbuffer[bufindw+1+serial_count++] = serial_char;
     }
   }
 
   #endif //SDSUPPORT
-
-}
-
-
-float code_value()
-{
-  return (strtod(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL));
 }
 
-long code_value_long()
-{
-  return (strtol(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL, 10));
-}
 
-int16_t code_value_short() {
-    return (int16_t)(strtol(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL, 10));
-}
-
-bool code_seen(char code)
-{
-  strchr_pointer = strchr(cmdbuffer[bufindr], code);
-  return (strchr_pointer != NULL);  //Return True if a character was found
-}
+// Return True if a character was found
+static inline bool    code_seen(char code) { return (strchr_pointer = strchr(CMDBUFFER_CURRENT_STRING, code)) != NULL; }
+static inline float   code_value()         { return strtod(strchr_pointer+1, NULL); }
+static inline long    code_value_long()    { return strtol(strchr_pointer+1, NULL, 10); }
+static inline int16_t code_value_short()   { return int16_t(strtol(strchr_pointer+1, NULL, 10)); };
 
 #define DEFINE_PGM_READ_ANY(type, reader)       \
     static inline type pgm_read_any(const type *p)  \
@@ -1078,59 +1223,9 @@ static void axis_is_at_home(int axis) {
     }
   }
 #endif
-#ifdef SCARA
-   float homeposition[3];
-   char i;
-   
-   if (axis < 2)
-   {
-   
-     for (i=0; i<3; i++)
-     {
-        homeposition[i] = base_home_pos(i); 
-     }  
-	// SERIAL_ECHOPGM("homeposition[x]= "); SERIAL_ECHO(homeposition[0]);
-   //  SERIAL_ECHOPGM("homeposition[y]= "); SERIAL_ECHOLN(homeposition[1]);
-   // Works out real Homeposition angles using inverse kinematics, 
-   // and calculates homing offset using forward kinematics
-     calculate_delta(homeposition);
-     
-    // SERIAL_ECHOPGM("base Theta= "); SERIAL_ECHO(delta[X_AXIS]);
-    // SERIAL_ECHOPGM(" base Psi+Theta="); SERIAL_ECHOLN(delta[Y_AXIS]);
-     
-     for (i=0; i<2; i++)
-     {
-        delta[i] -= add_homing[i];
-     } 
-     
-    // SERIAL_ECHOPGM("addhome X="); SERIAL_ECHO(add_homing[X_AXIS]);
-	// SERIAL_ECHOPGM(" addhome Y="); SERIAL_ECHO(add_homing[Y_AXIS]);
-    // SERIAL_ECHOPGM(" addhome Theta="); SERIAL_ECHO(delta[X_AXIS]);
-    // SERIAL_ECHOPGM(" addhome Psi+Theta="); SERIAL_ECHOLN(delta[Y_AXIS]);
-      
-     calculate_SCARA_forward_Transform(delta);
-     
-    // SERIAL_ECHOPGM("Delta X="); SERIAL_ECHO(delta[X_AXIS]);
-    // SERIAL_ECHOPGM(" Delta Y="); SERIAL_ECHOLN(delta[Y_AXIS]);
-     
-    current_position[axis] = delta[axis];
-    
-    // SCARA home positions are based on configuration since the actual limits are determined by the 
-    // inverse kinematic transform.
-    min_pos[axis] =          base_min_pos(axis); // + (delta[axis] - base_home_pos(axis));
-    max_pos[axis] =          base_max_pos(axis); // + (delta[axis] - base_home_pos(axis));
-   } 
-   else
-   {
-      current_position[axis] = base_home_pos(axis) + add_homing[axis];
-      min_pos[axis] =          base_min_pos(axis) + add_homing[axis];
-      max_pos[axis] =          base_max_pos(axis) + add_homing[axis];
-   }
-#else
   current_position[axis] = base_home_pos(axis) + add_homing[axis];
   min_pos[axis] =          base_min_pos(axis) + add_homing[axis];
   max_pos[axis] =          base_max_pos(axis) + add_homing[axis];
-#endif
 }
 
 
@@ -1268,52 +1363,14 @@ static void do_blocking_move_relative(float offset_x, float offset_y, float offs
 }
 
 
-static void engage_z_probe() {
-    // Engage Z Servo endstop if enabled
-    #ifdef SERVO_ENDSTOPS
-    if (servo_endstops[Z_AXIS] > -1) {
-#if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
-        servos[servo_endstops[Z_AXIS]].attach(0);
-#endif
-        servos[servo_endstops[Z_AXIS]].write(servo_endstop_angles[Z_AXIS * 2]);
-#if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
-        delay(PROBE_SERVO_DEACTIVATION_DELAY);
-        servos[servo_endstops[Z_AXIS]].detach();
-#endif
-    }
-    #endif
-}
-
-static void retract_z_probe() {
-    // Retract Z Servo endstop if enabled
-    #ifdef SERVO_ENDSTOPS
-    if (servo_endstops[Z_AXIS] > -1) {
-#if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
-        servos[servo_endstops[Z_AXIS]].attach(0);
-#endif
-        servos[servo_endstops[Z_AXIS]].write(servo_endstop_angles[Z_AXIS * 2 + 1]);
-#if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
-        delay(PROBE_SERVO_DEACTIVATION_DELAY);
-        servos[servo_endstops[Z_AXIS]].detach();
-#endif
-    }
-    #endif
-}
-
 /// Probe bed height at position (x,y), returns the measured z value
 static float probe_pt(float x, float y, float z_before) {
   // move to right place
   do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_before);
   do_blocking_move_to(x - X_PROBE_OFFSET_FROM_EXTRUDER, y - Y_PROBE_OFFSET_FROM_EXTRUDER, current_position[Z_AXIS]);
 
-#ifndef Z_PROBE_SLED
-  engage_z_probe();   // Engage Z Servo endstop if available
-#endif // Z_PROBE_SLED
   run_z_probe();
   float measured_z = current_position[Z_AXIS];
-#ifndef Z_PROBE_SLED
-  retract_z_probe();
-#endif // Z_PROBE_SLED
 
   SERIAL_PROTOCOLRPGM(MSG_BED);
   SERIAL_PROTOCOLPGM(" x: ");
@@ -1341,27 +1398,10 @@ static void homeaxis(int axis) {
     if (axis == X_AXIS)
       axis_home_dir = x_home_dir(active_extruder);
 #endif
-      
-
 
     current_position[axis] = 0;
     plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
 
-
-#ifndef Z_PROBE_SLED
-    // Engage Servo endstop if enabled
-    #ifdef SERVO_ENDSTOPS
-      #if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
-        if (axis==Z_AXIS) {
-          engage_z_probe();
-        }
-	    else
-      #endif
-      if (servo_endstops[axis] > -1) {
-        servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2]);
-      }
-    #endif
-#endif // Z_PROBE_SLED
     destination[axis] = 1.5 * max_length(axis) * axis_home_dir;
     feedrate = homing_feedrate[axis];
     plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
@@ -1374,43 +1414,16 @@ static void homeaxis(int axis) {
     st_synchronize();
 
     destination[axis] = 2*home_retract_mm(axis) * axis_home_dir;
-#ifdef DELTA
-    feedrate = homing_feedrate[axis]/10;
-#else
     feedrate = homing_feedrate[axis]/2 ;
-#endif
     plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
     st_synchronize();
-#ifdef DELTA
-    // retrace by the amount specified in endstop_adj
-    if (endstop_adj[axis] * axis_home_dir < 0) {
-      plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-      destination[axis] = endstop_adj[axis];
-      plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
-      st_synchronize();
-    }
-#endif
     axis_is_at_home(axis);
     destination[axis] = current_position[axis];
     feedrate = 0.0;
     endstops_hit_on_purpose();
     axis_known_position[axis] = true;
-
-    // Retract Servo endstop if enabled
-    #ifdef SERVO_ENDSTOPS
-      if (servo_endstops[axis] > -1) {
-        servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2 + 1]);
-      }
-    #endif
-#if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
-  #ifndef Z_PROBE_SLED
-    if (axis==Z_AXIS) retract_z_probe();
-  #endif
-#endif
-
   }
 }
-#define HOMEAXIS(LETTER) homeaxis(LETTER##_AXIS)
 
 void refresh_cmd_timeout(void)
 {
@@ -1435,12 +1448,7 @@ void refresh_cmd_timeout(void)
       retracted[active_extruder]=true;
       prepare_move();
       current_position[Z_AXIS]-=retract_zlift;
-#ifdef DELTA
-      calculate_delta(current_position); // change cartesian kinematic to  delta kinematic;
-      plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
-#else
       plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-#endif
       prepare_move();
       feedrate = oldFeedrate;
     } else if(!retracting && retracted[active_extruder]) {
@@ -1449,12 +1457,7 @@ void refresh_cmd_timeout(void)
       destination[Z_AXIS]=current_position[Z_AXIS];
       destination[E_AXIS]=current_position[E_AXIS];
       current_position[Z_AXIS]+=retract_zlift;
-#ifdef DELTA
-      calculate_delta(current_position); // change cartesian kinematic  to  delta kinematic;
-      plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
-#else
       plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-#endif
       //prepare_move();
       if (swapretract) {
         current_position[E_AXIS]-=(retract_length_swap+retract_recover_length_swap)/volumetric_multiplier[active_extruder]; 
@@ -1471,42 +1474,6 @@ void refresh_cmd_timeout(void)
   } //retract
 #endif //FWRETRACT
 
-#ifdef Z_PROBE_SLED
-//
-// Method to dock/undock a sled designed by Charles Bell.
-//
-// dock[in]     If true, move to MAX_X and engage the electromagnet
-// offset[in]   The additional distance to move to adjust docking location
-//
-static void dock_sled(bool dock, int offset=0) {
- int z_loc;
- 
- if (!((axis_known_position[X_AXIS]) && (axis_known_position[Y_AXIS]))) {
-   LCD_MESSAGERPGM(MSG_POSITION_UNKNOWN);
-   SERIAL_ECHO_START;
-   SERIAL_ECHOLNRPGM(MSG_POSITION_UNKNOWN);
-   return;
- }
-
- if (dock) {
-   do_blocking_move_to(X_MAX_POS + SLED_DOCKING_OFFSET + offset,
-                       current_position[Y_AXIS],
-                       current_position[Z_AXIS]);
-   // turn off magnet
-   digitalWrite(SERVO0_PIN, LOW);
- } else {
-   if (current_position[Z_AXIS] < (Z_RAISE_BEFORE_PROBING + 5))
-     z_loc = Z_RAISE_BEFORE_PROBING;
-   else
-     z_loc = current_position[Z_AXIS];
-   do_blocking_move_to(X_MAX_POS + SLED_DOCKING_OFFSET + offset,
-                       Y_PROBE_OFFSET_FROM_EXTRUDER, z_loc);
-   // turn on magnet
-   digitalWrite(SERVO0_PIN, HIGH);
- }
-}
-#endif
-
 void process_commands()
 {
   #ifdef FILAMENT_RUNOUT_SUPPORT
@@ -1747,7 +1714,6 @@ void process_commands()
         //ClearToSend();
       }
       break;
-#ifndef SCARA //disable arc support
     case 2: // G2  - CW ARC
       if(Stopped == false) {
         get_arc_coordinates();
@@ -1760,7 +1726,6 @@ void process_commands()
         prepare_arc_move(false);
       }
       break;
-#endif
     case 4: // G4 dwell
       LCD_MESSAGERPGM(MSG_DWELL);
       codenum = 0;
@@ -1805,6 +1770,10 @@ void process_commands()
             mbl.active = 0;
         #endif
 
+      // Reset world2machine_rotation_and_skew and world2machine_shift, therefore
+      // the planner will not perform any adjustments in the XY plane. 
+      world2machine_reset();
+
       saved_feedrate = feedrate;
       saved_feedmultiply = feedmultiply;
       feedmultiply = 100;
@@ -1817,47 +1786,16 @@ void process_commands()
       }
       feedrate = 0.0;
 
-#ifdef DELTA
-          // A delta can only safely home all axis at the same time
-          // all axis have to home at the same time
-
-          // Move all carriages up together until the first endstop is hit.
-          current_position[X_AXIS] = 0;
-          current_position[Y_AXIS] = 0;
-          current_position[Z_AXIS] = 0;
-          plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-
-          destination[X_AXIS] = 3 * Z_MAX_LENGTH;
-          destination[Y_AXIS] = 3 * Z_MAX_LENGTH;
-          destination[Z_AXIS] = 3 * Z_MAX_LENGTH;
-          feedrate = 1.732 * homing_feedrate[X_AXIS];
-          plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
-          st_synchronize();
-          endstops_hit_on_purpose();
-
-          current_position[X_AXIS] = destination[X_AXIS];
-          current_position[Y_AXIS] = destination[Y_AXIS];
-          current_position[Z_AXIS] = destination[Z_AXIS];
-
-          // take care of back off and rehome now we are all at the top
-          HOMEAXIS(X);
-          HOMEAXIS(Y);
-          HOMEAXIS(Z);
-
-          calculate_delta(current_position);
-          plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
-
-#else // NOT DELTA
-
       home_all_axis = !((code_seen(axis_codes[X_AXIS])) || (code_seen(axis_codes[Y_AXIS])) || (code_seen(axis_codes[Z_AXIS])));
 
       #if Z_HOME_DIR > 0                      // If homing away from BED do Z first
       if((home_all_axis) || (code_seen(axis_codes[Z_AXIS]))) {
-        HOMEAXIS(Z);
+        homeaxis(Z_AXIS);
       }
       #endif
 
       #ifdef QUICK_HOME
+      // In the quick mode, if both x and y are to be homed, a diagonal move will be performed initially.
       if((home_all_axis)||( code_seen(axis_codes[X_AXIS]) && code_seen(axis_codes[Y_AXIS])) )  //first diagonal move
       {
         current_position[X_AXIS] = 0;current_position[Y_AXIS] = 0;
@@ -1894,11 +1832,9 @@ void process_commands()
 
         current_position[X_AXIS] = destination[X_AXIS];
         current_position[Y_AXIS] = destination[Y_AXIS];
-		#ifndef SCARA
         current_position[Z_AXIS] = destination[Z_AXIS];
-		#endif
       }
-      #endif
+      #endif /* QUICK_HOME */
 
 	  if (home_all_axis)
 	  {
@@ -1911,41 +1847,33 @@ void process_commands()
         int tmp_extruder = active_extruder;
         extruder_duplication_enabled = false;
         active_extruder = !active_extruder;
-        HOMEAXIS(X);
+        homeaxis(X_AXIS);
         inactive_extruder_x_pos = current_position[X_AXIS];
         active_extruder = tmp_extruder;
-        HOMEAXIS(X);
+        homeaxis(X_AXIS);
         // reset state used by the different modes
         memcpy(raised_parked_position, current_position, sizeof(raised_parked_position));
         delayed_move_time = 0;
         active_extruder_parked = true;
       #else
-        HOMEAXIS(X);
+        homeaxis(X_AXIS);
       #endif
       }
 
       if((home_all_axis) || (code_seen(axis_codes[Y_AXIS]))) {
-        HOMEAXIS(Y);
+        homeaxis(Y_AXIS);
       }
 
       if(code_seen(axis_codes[X_AXIS]))
       {
         if(code_value_long() != 0) {
-		#ifdef SCARA
-		   current_position[X_AXIS]=code_value();
-		#else
 		   current_position[X_AXIS]=code_value()+add_homing[X_AXIS];
-		#endif
         }
       }
 
       if(code_seen(axis_codes[Y_AXIS])) {
         if(code_value_long() != 0) {
-         #ifdef SCARA
-		   current_position[Y_AXIS]=code_value();
-		#else
 		   current_position[Y_AXIS]=code_value()+add_homing[Y_AXIS];
-		#endif
         }
       }
 
@@ -1961,11 +1889,15 @@ void process_commands()
             #ifdef MESH_BED_LEVELING // If Mesh bed leveling, moxve X&Y to safe position for home
       			  if (!(axis_known_position[X_AXIS] && axis_known_position[Y_AXIS] )) 
       			  {
-      				  HOMEAXIS(X);
-      				  HOMEAXIS(Y);
+                homeaxis(X_AXIS);
+                homeaxis(Y_AXIS);
       			  } 
               // 1st mesh bed leveling measurement point, corrected.
-              mbl.get_meas_xy(0, 0, destination[X_AXIS], destination[Y_AXIS], false);
+              world2machine_initialize();
+              current_position[X_AXIS] = world2machine_rotation_and_skew[0][0] * pgm_read_float(bed_ref_points) + world2machine_rotation_and_skew[0][1] * pgm_read_float(bed_ref_points+1) + world2machine_shift[0];
+              current_position[Y_AXIS] = world2machine_rotation_and_skew[1][0] * pgm_read_float(bed_ref_points) + world2machine_rotation_and_skew[1][1] * pgm_read_float(bed_ref_points+1) + world2machine_shift[1];
+              world2machine_reset();
+//              mbl.get_meas_xy(0, 0, destination[X_AXIS], destination[Y_AXIS], false);
               // destination[X_AXIS] = MESH_MIN_X - X_PROBE_OFFSET_FROM_EXTRUDER;
               // destination[Y_AXIS] = MESH_MIN_Y - Y_PROBE_OFFSET_FROM_EXTRUDER;
               destination[Z_AXIS] = MESH_HOME_Z_SEARCH;    // Set destination away from bed
@@ -1977,10 +1909,10 @@ void process_commands()
               st_synchronize();
               current_position[X_AXIS] = destination[X_AXIS];
               current_position[Y_AXIS] = destination[Y_AXIS];
-              HOMEAXIS(Z);
+              homeaxis(Z_AXIS);
   		        _doMeshL = true;
             #else // MESH_BED_LEVELING
-              HOMEAXIS(Z);
+              homeaxis(Z_AXIS);
             #endif // MESH_BED_LEVELING
           }
         #else // defined(Z_SAFE_HOMING): Z Safe mode activated.
@@ -1997,7 +1929,7 @@ void process_commands()
             current_position[X_AXIS] = destination[X_AXIS];
             current_position[Y_AXIS] = destination[Y_AXIS];
 
-            HOMEAXIS(Z);
+            homeaxis(Z_AXIS);
           }
                                                 // Let's see if X and Y are homed and probe is inside bed area.
           if(code_seen(axis_codes[Z_AXIS])) {
@@ -2014,7 +1946,7 @@ void process_commands()
               plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
               st_synchronize();
 
-              HOMEAXIS(Z);
+              homeaxis(Z_AXIS);
             } else if (!((axis_known_position[X_AXIS]) && (axis_known_position[Y_AXIS]))) {
                 LCD_MESSAGERPGM(MSG_POSITION_UNKNOWN);
                 SERIAL_ECHO_START;
@@ -2038,15 +1970,8 @@ void process_commands()
           current_position[Z_AXIS] += zprobe_zoffset;  //Add Z_Probe offset (the distance is negative)
         }
       #endif
-    
   
       plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-#endif // else DELTA
-
-#ifdef SCARA
-	  calculate_delta(current_position);
-      plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
-#endif // SCARA
 
       #ifdef ENDSTOPS_ONLY_FOR_HOMING
         enable_endstops(false);
@@ -2065,6 +1990,11 @@ void process_commands()
       }
 #endif
 
+    // Load the machine correction matrix
+    world2machine_initialize();
+    // and correct the current_position to match the transformed coordinate system.
+    world2machine_update_current();
+
 #ifdef MESH_BED_LEVELING
 	  if (code_seen('W'))
 	  {
@@ -2075,7 +2005,9 @@ void process_commands()
 	  if ( _doMeshL)
 	  {
 		  st_synchronize();
-		  enquecommand_P((PSTR("G80")));
+      // Push the commands to the front of the message queue in the reverse order!
+      // There shall be always enough space reserved for these commands.
+		  enquecommand_front_P((PSTR("G80")));
 	  }
 #endif
 
@@ -2097,9 +2029,6 @@ void process_commands()
                 break; // abort G29, since we don't know where we are
             }
 
-#ifdef Z_PROBE_SLED
-            dock_sled(false);
-#endif // Z_PROBE_SLED
             st_synchronize();
             // make sure the bed_level_rotation_matrix is identity or the planner will get it incorectly
             //vector_3 corrected_position = plan_get_position_mm();
@@ -2225,15 +2154,11 @@ void process_commands()
             apply_rotation_xyz(plan_bed_level_matrix, x_tmp, y_tmp, z_tmp);         //Apply the correction sending the probe offset
             current_position[Z_AXIS] = z_tmp - real_z + current_position[Z_AXIS];   //The difference is added to current position and sent to planner.
             plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-#ifdef Z_PROBE_SLED
-            dock_sled(true, -SLED_DOCKING_OFFSET); // correct for over travel.
-#endif // Z_PROBE_SLED
         }
         break;
 #ifndef Z_PROBE_SLED
     case 30: // G30 Single Z Probe
         {
-            engage_z_probe(); // Engage Z Servo endstop if available
             st_synchronize();
             // TODO: make sure the bed_level_rotation_matrix is identity or the planner will get set incorectly
             setup_for_endstop_move();
@@ -2251,7 +2176,6 @@ void process_commands()
             SERIAL_PROTOCOLPGM("\n");
 
             clean_up_after_endstop_move();
-            retract_z_probe(); // Retract Z Servo endstop if available
         }
         break;
 #else
@@ -2290,8 +2214,10 @@ void process_commands()
             // Firstly check if we know where we are
             if ( !( axis_known_position[X_AXIS] && axis_known_position[Y_AXIS] && axis_known_position[Z_AXIS] ) ){
                 // We don't know where we are! HOME!
-                enquecommand_P((PSTR("G28 W0")));
-                enquecommand_P((PSTR("G80")));
+                // Push the commands to the front of the message queue in the reverse order!
+                // There shall be always enough space reserved for these commands.
+                enquecommand_front_P((PSTR("G80")));
+                enquecommand_front_P((PSTR("G28 W0")));
                 break;
             }
             
@@ -2302,7 +2228,9 @@ void process_commands()
             current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
             plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[Z_AXIS]/60, active_extruder);
             // The move to the first calibration point.
-            mbl.get_meas_xy(0, 0, current_position[X_AXIS], current_position[Y_AXIS], false);            
+            current_position[X_AXIS] = pgm_read_float(bed_ref_points);
+            current_position[Y_AXIS] = pgm_read_float(bed_ref_points+1);
+//            mbl.get_meas_xy(0, 0, current_position[X_AXIS], current_position[Y_AXIS], false);            
             plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[X_AXIS]/30, active_extruder);
             // Wait until the move is finished.
             st_synchronize();
@@ -2316,7 +2244,7 @@ void process_commands()
             int Z_PROBE_FEEDRATE = homing_feedrate[Z_AXIS]/60;
             int Z_LIFT_FEEDRATE = homing_feedrate[Z_AXIS]/40;
             setup_for_endstop_move();
-            while (!(mesh_point == ((MESH_MEAS_NUM_X_POINTS * MESH_MEAS_NUM_Y_POINTS) ))) {
+            while (mesh_point != MESH_MEAS_NUM_X_POINTS * MESH_MEAS_NUM_Y_POINTS) {
             
                 // Move Z to proper distance
                 current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
@@ -2328,7 +2256,9 @@ void process_commands()
                 iy = mesh_point / MESH_MEAS_NUM_X_POINTS;
                 if (iy & 1) ix = (MESH_MEAS_NUM_X_POINTS - 1) - ix; // Zig zag
                 
-                mbl.get_meas_xy(ix, iy, current_position[X_AXIS], current_position[Y_AXIS], false);
+                current_position[X_AXIS] = pgm_read_float(bed_ref_points+2*mesh_point);
+                current_position[Y_AXIS] = pgm_read_float(bed_ref_points+2*mesh_point+1);
+//                mbl.get_meas_xy(ix, iy, current_position[X_AXIS], current_position[Y_AXIS], false);
                 enable_endstops(false);
                 plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], XY_AXIS_FEEDRATE, active_extruder);
                 st_synchronize();
@@ -2479,23 +2409,14 @@ void process_commands()
              plan_set_e_position(current_position[E_AXIS]);
            }
            else {
-#ifdef SCARA
-		if (i == X_AXIS || i == Y_AXIS) {
-                	current_position[i] = code_value();  
-		}
-		else {
-                current_position[i] = code_value()+add_homing[i];  
-            	}  
-#else
 		current_position[i] = code_value()+add_homing[i];
-#endif
             plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
            }
         }
       }
       break;
     }
-  }
+  } // end if(code_seen('G'))
 
   else if(code_seen('M'))
   {
@@ -2603,7 +2524,7 @@ void process_commands()
     case 28: //M28 - Start SD write
       starpos = (strchr(strchr_pointer + 4,'*'));
       if(starpos != NULL){
-        char* npos = strchr(cmdbuffer[bufindr], 'N');
+        char* npos = strchr(CMDBUFFER_CURRENT_STRING, 'N');
         strchr_pointer = strchr(npos,' ') + 1;
         *(starpos) = '\0';
       }
@@ -2618,7 +2539,7 @@ void process_commands()
         card.closefile();
         starpos = (strchr(strchr_pointer + 4,'*'));
         if(starpos != NULL){
-          char* npos = strchr(cmdbuffer[bufindr], 'N');
+          char* npos = strchr(CMDBUFFER_CURRENT_STRING, 'N');
           strchr_pointer = strchr(npos,' ') + 1;
           *(starpos) = '\0';
         }
@@ -2663,7 +2584,7 @@ void process_commands()
     case 928: //M928 - Start SD write
       starpos = (strchr(strchr_pointer + 5,'*'));
       if(starpos != NULL){
-        char* npos = strchr(cmdbuffer[bufindr], 'N');
+        char* npos = strchr(CMDBUFFER_CURRENT_STRING, 'N');
         strchr_pointer = strchr(npos,' ') + 1;
         *(starpos) = '\0';
       }
@@ -2715,69 +2636,44 @@ void process_commands()
       }
      break;
 
-    case 44:
+    case 45:
         reset_bed_offset_and_skew();
+        world2machine_reset();
         break;
 
-    case 45: // M45: mesh_bed_calibration
-        {
-            // Firstly check if we know where we are
-            if ( !( axis_known_position[X_AXIS] && axis_known_position[Y_AXIS]) ){
-                // We don't know where we are! HOME!
-                enquecommand_P((PSTR("G28 X0 Y0")));
-                enquecommand_P((PSTR("M45")));
-                break;
-            }
-            
-            setup_for_endstop_move();
-            find_bed_offset_and_skew();
-//            improve_bed_offset_and_skew();
-            clean_up_after_endstop_move();
-            current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
-            plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS],current_position[Z_AXIS] , current_position[E_AXIS], homing_feedrate[Z_AXIS]/40, active_extruder);
-            /*
-            current_position[X_AXIS] = X_MIN_POS+0.2;
-            current_position[Y_AXIS] = Y_MIN_POS+0.2;
-            current_position[Z_AXIS] = Z_MIN_POS;
-            plan_buffer_line(current_position[X_AXIS], current_position[X_AXIS], current_position[Z_AXIS], current_position[E_AXIS], XY_AXIS_FEEDRATE, active_extruder);
-            */
-            st_synchronize();
-        }
-    break;
-
     case 46: // M46: mesh_bed_calibration with manual Z up
         {
             // Firstly check if we know where we are
             if ( !( axis_known_position[X_AXIS] && axis_known_position[Y_AXIS]) ){
                 // We don't know where we are! HOME!
-                enquecommand_P((PSTR("G28 X0 Y0 W0"))); // W0 tells G28 to not perform mesh bed leveling.
-                enquecommand_P((PSTR("M46")));
+                // Push the commands to the front of the message queue in the reverse order!
+                // There shall be always enough space reserved for these commands.
+                enquecommand_front_P((PSTR("M46")));
+                enquecommand_front_P((PSTR("G28 X Y")));
                 break;
             }
 
             lcd_update_enable(false);
             if (lcd_calibrate_z_end_stop_manual()) {
-              mbl.reset();
-              setup_for_endstop_move();
-              find_bed_offset_and_skew();
-//              improve_bed_offset_and_skew(1);
-              clean_up_after_endstop_move();
-              // Print head up.
-              current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
-              plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS],current_position[Z_AXIS] , current_position[E_AXIS], homing_feedrate[Z_AXIS]/40, active_extruder);
-              st_synchronize();
+                mbl.reset();
+                setup_for_endstop_move();
+                find_bed_offset_and_skew();
+                clean_up_after_endstop_move();
+                // Print head up.
+                current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
+                plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS],current_position[Z_AXIS] , current_position[E_AXIS], homing_feedrate[Z_AXIS]/40, active_extruder);
+                st_synchronize();
+                // Push the commands to the front of the message queue in the reverse order!
+                // There shall be always enough space reserved for these commands.
+                enquecommand_front_P((PSTR("M47")));
+                enquecommand_front_P((PSTR("G28 X Y")));
+              } else {
+                // User canceled the operation. Give up.
+                lcd_update_enable(true);
+                lcd_implementation_clear();
+                // lcd_return_to_status();
+                lcd_update();
             }
-            // lcd_update_enable(true);
-            //lcd_implementation_clear();
-            //lcd_return_to_status();
-            // lcd_update();
-            // Mesh bed leveling.
-            // enquecommand_P((PSTR("G80")));
-            // The iprovement.
-            
-            //enquecommand_P((PSTR("G80")));
-            enquecommand_P((PSTR("G28 X0 Y0 W0")));
-            enquecommand_P((PSTR("M47")));
         }
     break;
 
@@ -2786,8 +2682,10 @@ void process_commands()
           // Firstly check if we know where we are
           if ( !( axis_known_position[X_AXIS] && axis_known_position[Y_AXIS]) ) {
               // We don't know where we are! HOME!
-              enquecommand_P((PSTR("G28 X0 Y0 W0"))); // W0 tells G28 to not perform mesh bed leveling.
-              enquecommand_P((PSTR("M47")));
+              // Push the commands to the front of the message queue in the reverse order!
+              // There shall be always enough space reserved for these commands.
+              enquecommand_front_P((PSTR("M47")));
+              enquecommand_front_P((PSTR("G28 X Y")));
               break;
           }
           lcd_update_enable(false);
@@ -2801,19 +2699,22 @@ void process_commands()
           st_synchronize();
           lcd_update_enable(true);
           lcd_update();
-          if (success)
+          if (success) {
               // Mesh bed leveling.
-              enquecommand_P((PSTR("G80")));
+              // Push the commands to the front of the message queue in the reverse order!
+              // There shall be always enough space reserved for these commands.
+              enquecommand_front_P((PSTR("G80")));
+          }
           break;
     }
 
- //   case 47:
- //       lcd_diag_show_end_stops();
- //       break;
+    case 48:
+        lcd_diag_show_end_stops();
+        break;
 
 // M48 Z-Probe repeatability measurement function.
 //
-// Usage:   M48 <n #_samples> <X X_position_for_samples> <Y Y_position_for_samples> <V Verbose_Level> <Engage_probe_for_each_reading> <L legs_of_movement_prior_to_doing_probe>
+// Usage:   M48 <n #_samples> <X X_position_for_samples> <Y Y_position_for_samples> <V Verbose_Level> <L legs_of_movement_prior_to_doing_probe>
 //	
 // This function assumes the bed has been homed.  Specificaly, that a G28 command
 // as been issued prior to invoking the M48 Z-Probe repeatability measurement function.
@@ -2838,7 +2739,7 @@ void process_commands()
 	double mean=0.0; 
 	double sigma=0.0;
 	double sample_set[50];
-	int verbose_level=1, n=0, j, n_samples = 10, n_legs=0, engage_probe_for_each_reading=0 ;
+	int verbose_level=1, n=0, j, n_samples = 10, n_legs=0;
 	double X_current, Y_current, Z_current;
 	double X_probe_location, Y_probe_location, Z_start_location, ext_position;
 	
@@ -2869,9 +2770,6 @@ void process_commands()
 	Z_start_location = st_get_position_mm(Z_AXIS) + Z_RAISE_BEFORE_PROBING;
 	ext_position	 = st_get_position_mm(E_AXIS);
 
-	if (code_seen('E') || code_seen('e') ) 
-		engage_probe_for_each_reading++;
-
 	if (code_seen('X') || code_seen('x') ) {
         	X_probe_location = code_value() -  X_PROBE_OFFSET_FROM_EXTRUDER;
 		if (X_probe_location<X_MIN_POS || X_probe_location>X_MAX_POS ) {
@@ -2934,8 +2832,6 @@ void process_commands()
 // Then retrace the right amount and use that in subsequent probes
 //
 
-        engage_z_probe();	
-
 	setup_for_endstop_move();
 	run_z_probe();
 
@@ -2949,9 +2845,6 @@ void process_commands()
         st_synchronize();
 	current_position[Z_AXIS] = Z_current = st_get_position_mm(Z_AXIS);
 
-	if (engage_probe_for_each_reading)
-        	retract_z_probe();
-
         for( n=0; n<n_samples; n++) {
 
 		do_blocking_move_to( X_probe_location, Y_probe_location, Z_start_location); // Make sure we are at the probe location
@@ -3003,11 +2896,6 @@ void process_commands()
 			do_blocking_move_to( X_probe_location, Y_probe_location, Z_start_location); // Go back to the probe location
 		}
 
-		if (engage_probe_for_each_reading)  {
-        		engage_z_probe();	
-          		delay(1000);
-		}
-
 		setup_for_endstop_move();
                 run_z_probe();
 
@@ -3055,13 +2943,8 @@ void process_commands()
 				  current_position[E_AXIS], homing_feedrate[Z_AXIS]/60, active_extruder);
         	st_synchronize();
 
-		if (engage_probe_for_each_reading)  {
-        		retract_z_probe();	
-          		delay(1000);
-		}
 	}
 
-        retract_z_probe();
 	delay(1000);
 
         clean_up_after_endstop_move();
@@ -3321,37 +3204,6 @@ Sigma_Exit:
         fanSpeed = 0;
         break;
     #endif //FAN_PIN
-    #ifdef BARICUDA
-      // PWM for HEATER_1_PIN
-      #if defined(HEATER_1_PIN) && HEATER_1_PIN > -1
-        case 126: //M126 valve open
-          if (code_seen('S')){
-             ValvePressure=constrain(code_value(),0,255);
-          }
-          else {
-            ValvePressure=255;
-          }
-          break;
-        case 127: //M127 valve closed
-          ValvePressure = 0;
-          break;
-      #endif //HEATER_1_PIN
-
-      // PWM for HEATER_2_PIN
-      #if defined(HEATER_2_PIN) && HEATER_2_PIN > -1
-        case 128: //M128 valve open
-          if (code_seen('S')){
-             EtoPPressure=constrain(code_value(),0,255);
-          }
-          else {
-            EtoPPressure=255;
-          }
-          break;
-        case 129: //M129 valve closed
-          EtoPPressure = 0;
-          break;
-      #endif //HEATER_2_PIN
-    #endif
 
     #if defined(PS_ON_PIN) && PS_ON_PIN > -1
       case 80: // M80 - Turn on Power Supply
@@ -3496,26 +3348,6 @@ Sigma_Exit:
       SERIAL_PROTOCOL(float(st_get_position(Z_AXIS))/axis_steps_per_unit[Z_AXIS]);
 
       SERIAL_PROTOCOLLN("");
-#ifdef SCARA
-	  SERIAL_PROTOCOLPGM("SCARA Theta:");
-      SERIAL_PROTOCOL(delta[X_AXIS]);
-      SERIAL_PROTOCOLPGM("   Psi+Theta:");
-      SERIAL_PROTOCOL(delta[Y_AXIS]);
-      SERIAL_PROTOCOLLN("");
-      
-      SERIAL_PROTOCOLPGM("SCARA Cal - Theta:");
-      SERIAL_PROTOCOL(delta[X_AXIS]+add_homing[X_AXIS]);
-      SERIAL_PROTOCOLPGM("   Psi+Theta (90):");
-      SERIAL_PROTOCOL(delta[Y_AXIS]-delta[X_AXIS]-90+add_homing[Y_AXIS]);
-      SERIAL_PROTOCOLLN("");
-      
-      SERIAL_PROTOCOLPGM("SCARA step Cal - Theta:");
-      SERIAL_PROTOCOL(delta[X_AXIS]/90*axis_steps_per_unit[X_AXIS]);
-      SERIAL_PROTOCOLPGM("   Psi+Theta:");
-      SERIAL_PROTOCOL((delta[Y_AXIS]-delta[X_AXIS])/90*axis_steps_per_unit[Y_AXIS]);
-      SERIAL_PROTOCOLLN("");
-      SERIAL_PROTOCOLLN("");
-#endif
       break;
     case 120: // M120
       enable_endstops(false) ;
@@ -3680,39 +3512,8 @@ Sigma_Exit:
       for(int8_t i=0; i < 3; i++)
       {
         if(code_seen(axis_codes[i])) add_homing[i] = code_value();
-      }
-	  #ifdef SCARA
-	   if(code_seen('T'))       // Theta
-      {
-        add_homing[X_AXIS] = code_value() ;
-      }
-      if(code_seen('P'))       // Psi
-      {
-        add_homing[Y_AXIS] = code_value() ;
-      }
-	  #endif
-      break;
-    #ifdef DELTA
-	case 665: // M665 set delta configurations L<diagonal_rod> R<delta_radius> S<segments_per_sec>
-		if(code_seen('L')) {
-			delta_diagonal_rod= code_value();
-		}
-		if(code_seen('R')) {
-			delta_radius= code_value();
-		}
-		if(code_seen('S')) {
-			delta_segments_per_second= code_value();
-		}
-		
-		recalc_delta_settings(delta_radius, delta_diagonal_rod);
-		break;
-    case 666: // M666 set delta endstop adjustemnt
-      for(int8_t i=0; i < 3; i++)
-      {
-        if(code_seen(axis_codes[i])) endstop_adj[i] = code_value();
       }
       break;
-    #endif
     #ifdef FWRETRACT
     case 207: //M207 - set retract length S[positive mm] F[feedrate mm/min] Z[additional zlift/hop]
     {
@@ -3772,7 +3573,7 @@ Sigma_Exit:
           default:
             SERIAL_ECHO_START;
             SERIAL_ECHORPGM(MSG_UNKNOWN_COMMAND);
-            SERIAL_ECHO(cmdbuffer[bufindr]);
+            SERIAL_ECHO(CMDBUFFER_CURRENT_STRING);
             SERIAL_ECHOLNPGM("\"");
         }
       }
@@ -4070,123 +3871,11 @@ Sigma_Exit:
       PID_autotune(temp, e, c);
     }
     break;
-	#ifdef SCARA
-	case 360:  // M360 SCARA Theta pos1
-      SERIAL_ECHOLN(" Cal: Theta 0 ");
-      //SoftEndsEnabled = false;              // Ignore soft endstops during calibration
-      //SERIAL_ECHOLN(" Soft endstops disabled ");
-      if(Stopped == false) {
-        //get_coordinates(); // For X Y Z E F
-        delta[X_AXIS] = 0;
-        delta[Y_AXIS] = 120;
-        calculate_SCARA_forward_Transform(delta);
-        destination[X_AXIS] = delta[X_AXIS]/axis_scaling[X_AXIS];
-        destination[Y_AXIS] = delta[Y_AXIS]/axis_scaling[Y_AXIS];
-        
-        prepare_move();
-        //ClearToSend();
-        return;
-      }
-    break;
-
-    case 361:  // SCARA Theta pos2
-      SERIAL_ECHOLN(" Cal: Theta 90 ");
-      //SoftEndsEnabled = false;              // Ignore soft endstops during calibration
-      //SERIAL_ECHOLN(" Soft endstops disabled ");
-      if(Stopped == false) {
-        //get_coordinates(); // For X Y Z E F
-        delta[X_AXIS] = 90;
-        delta[Y_AXIS] = 130;
-        calculate_SCARA_forward_Transform(delta);
-        destination[X_AXIS] = delta[X_AXIS]/axis_scaling[X_AXIS];
-        destination[Y_AXIS] = delta[Y_AXIS]/axis_scaling[Y_AXIS];
-        
-        prepare_move();
-        //ClearToSend();
-        return;
-      }
-    break;
-    case 362:  // SCARA Psi pos1
-      SERIAL_ECHOLN(" Cal: Psi 0 ");
-      //SoftEndsEnabled = false;              // Ignore soft endstops during calibration
-      //SERIAL_ECHOLN(" Soft endstops disabled ");
-      if(Stopped == false) {
-        //get_coordinates(); // For X Y Z E F
-        delta[X_AXIS] = 60;
-        delta[Y_AXIS] = 180;
-        calculate_SCARA_forward_Transform(delta);
-        destination[X_AXIS] = delta[X_AXIS]/axis_scaling[X_AXIS];
-        destination[Y_AXIS] = delta[Y_AXIS]/axis_scaling[Y_AXIS];
-        
-        prepare_move();
-        //ClearToSend();
-        return;
-      }
-    break;
-    case 363:  // SCARA Psi pos2
-      SERIAL_ECHOLN(" Cal: Psi 90 ");
-      //SoftEndsEnabled = false;              // Ignore soft endstops during calibration
-      //SERIAL_ECHOLN(" Soft endstops disabled ");
-      if(Stopped == false) {
-        //get_coordinates(); // For X Y Z E F
-        delta[X_AXIS] = 50;
-        delta[Y_AXIS] = 90;
-        calculate_SCARA_forward_Transform(delta);
-        destination[X_AXIS] = delta[X_AXIS]/axis_scaling[X_AXIS];
-        destination[Y_AXIS] = delta[Y_AXIS]/axis_scaling[Y_AXIS];
-        
-        prepare_move();
-        //ClearToSend();
-        return;
-      }
-    break;
-    case 364:  // SCARA Psi pos3 (90 deg to Theta)
-      SERIAL_ECHOLN(" Cal: Theta-Psi 90 ");
-     // SoftEndsEnabled = false;              // Ignore soft endstops during calibration
-      //SERIAL_ECHOLN(" Soft endstops disabled ");
-      if(Stopped == false) {
-        //get_coordinates(); // For X Y Z E F
-        delta[X_AXIS] = 45;
-        delta[Y_AXIS] = 135;
-        calculate_SCARA_forward_Transform(delta);
-        destination[X_AXIS] = delta[X_AXIS]/axis_scaling[X_AXIS];
-        destination[Y_AXIS] = delta[Y_AXIS]/axis_scaling[Y_AXIS]; 
-        
-        prepare_move();
-        //ClearToSend();
-        return;
-      }
-    break;
-    case 365: // M364  Set SCARA scaling for X Y Z
-      for(int8_t i=0; i < 3; i++) 
-      {
-        if(code_seen(axis_codes[i])) 
-        {
-          
-            axis_scaling[i] = code_value();
-          
-        }
-      }
-      break;
-	#endif
     case 400: // M400 finish all moves
     {
       st_synchronize();
     }
     break;
-#if defined(ENABLE_AUTO_BED_LEVELING) && defined(SERVO_ENDSTOPS) && not defined(Z_PROBE_SLED)
-    case 401:
-    {
-        engage_z_probe();    // Engage Z Servo endstop if available
-    }
-    break;
-
-    case 402:
-    {
-        retract_z_probe();    // Retract Z Servo endstop if enabled
-    }
-    break;
-#endif
 
 #ifdef FILAMENT_SENSOR
 case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or display nominal filament width 
@@ -4654,7 +4343,7 @@ case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp
       FlushSerialRequestResend();
     break;
     }
-  }
+  } // end if(code_seen('M')) (end of M codes)
 
   else if(code_seen('T'))
   {
@@ -4739,16 +4428,7 @@ case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp
         // Set the new active extruder and position
         active_extruder = tmp_extruder;
       #endif //else DUAL_X_CARRIAGE
-#ifdef DELTA 
-
-  calculate_delta(current_position); // change cartesian kinematic  to  delta kinematic;
-   //sent position to plan_set_position();
-  plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS],current_position[E_AXIS]);
-            
-#else
         plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-
-#endif
         // Move to the old position if 'F' was in the parameters
         if(make_move && Stopped == false) {
            prepare_move();
@@ -4759,13 +4439,13 @@ case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp
       SERIAL_ECHO(MSG_ACTIVE_EXTRUDER);
       SERIAL_PROTOCOLLN((int)active_extruder);
     }
-  }
+  } // end if(code_seen('T')) (end of T codes)
 
   else
   {
     SERIAL_ECHO_START;
     SERIAL_ECHORPGM(MSG_UNKNOWN_COMMAND);
-    SERIAL_ECHO(cmdbuffer[bufindr]);
+    SERIAL_ECHO(CMDBUFFER_CURRENT_STRING);
     SERIAL_ECHOLNPGM("\"");
   }
 
@@ -4781,14 +4461,13 @@ void FlushSerialRequestResend()
   ClearToSend();
 }
 
+// Confirm the execution of a command, if sent from a serial line.
+// Execution of a command from a SD card will not be confirmed.
 void ClearToSend()
 {
-  previous_millis_cmd = millis();
-  #ifdef SDSUPPORT
-  if(fromsd[bufindr])
-    return;
-  #endif //SDSUPPORT
-  SERIAL_PROTOCOLLNRPGM(MSG_OK);
+    previous_millis_cmd = millis();
+    if (CMDBUFFER_CURRENT_TYPE == CMDBUFFER_CURRENT_TYPE_USB)
+        SERIAL_PROTOCOLLNRPGM(MSG_OK);
 }
 
 void get_coordinates()
@@ -4855,45 +4534,6 @@ void clamp_to_software_endstops(float target[3])
   }
 }
 
-#ifdef DELTA
-void recalc_delta_settings(float radius, float diagonal_rod)
-{
-	 delta_tower1_x= -SIN_60*radius; // front left tower
-	 delta_tower1_y= -COS_60*radius;	   
-	 delta_tower2_x=  SIN_60*radius; // front right tower
-	 delta_tower2_y= -COS_60*radius;	   
-	 delta_tower3_x= 0.0;                  // back middle tower
-	 delta_tower3_y= radius;
-	 delta_diagonal_rod_2= sq(diagonal_rod);
-}
-
-void calculate_delta(float cartesian[3])
-{
-  delta[X_AXIS] = sqrt(delta_diagonal_rod_2
-                       - sq(delta_tower1_x-cartesian[X_AXIS])
-                       - sq(delta_tower1_y-cartesian[Y_AXIS])
-                       ) + cartesian[Z_AXIS];
-  delta[Y_AXIS] = sqrt(delta_diagonal_rod_2
-                       - sq(delta_tower2_x-cartesian[X_AXIS])
-                       - sq(delta_tower2_y-cartesian[Y_AXIS])
-                       ) + cartesian[Z_AXIS];
-  delta[Z_AXIS] = sqrt(delta_diagonal_rod_2
-                       - sq(delta_tower3_x-cartesian[X_AXIS])
-                       - sq(delta_tower3_y-cartesian[Y_AXIS])
-                       ) + cartesian[Z_AXIS];
-  /*
-  SERIAL_ECHOPGM("cartesian x="); SERIAL_ECHO(cartesian[X_AXIS]);
-  SERIAL_ECHOPGM(" y="); SERIAL_ECHO(cartesian[Y_AXIS]);
-  SERIAL_ECHOPGM(" z="); SERIAL_ECHOLN(cartesian[Z_AXIS]);
-
-  SERIAL_ECHOPGM("delta x="); SERIAL_ECHO(delta[X_AXIS]);
-  SERIAL_ECHOPGM(" y="); SERIAL_ECHO(delta[Y_AXIS]);
-  SERIAL_ECHOPGM(" z="); SERIAL_ECHOLN(delta[Z_AXIS]);
-  */
-}
-#endif
-
-
 #ifdef MESH_BED_LEVELING
     void mesh_plan_buffer_line(const float &x, const float &y, const float &z, const float &e, const float &feed_rate, const uint8_t extruder) {
         float dx = x - current_position[X_AXIS];
@@ -4929,73 +4569,7 @@ void prepare_move()
 {
   clamp_to_software_endstops(destination);
   previous_millis_cmd = millis();
-  
-  #ifdef SCARA //for now same as delta-code
-
-float difference[NUM_AXIS];
-for (int8_t i=0; i < NUM_AXIS; i++) {
-	difference[i] = destination[i] - current_position[i];
-}
-
-float cartesian_mm = sqrt(	sq(difference[X_AXIS]) +
-							sq(difference[Y_AXIS]) +
-							sq(difference[Z_AXIS]));
-if (cartesian_mm < 0.000001) { cartesian_mm = abs(difference[E_AXIS]); }
-if (cartesian_mm < 0.000001) { return; }
-float seconds = 6000 * cartesian_mm / feedrate / feedmultiply;
-int steps = max(1, int(scara_segments_per_second * seconds));
- //SERIAL_ECHOPGM("mm="); SERIAL_ECHO(cartesian_mm);
- //SERIAL_ECHOPGM(" seconds="); SERIAL_ECHO(seconds);
- //SERIAL_ECHOPGM(" steps="); SERIAL_ECHOLN(steps);
-for (int s = 1; s <= steps; s++) {
-	float fraction = float(s) / float(steps);
-	for(int8_t i=0; i < NUM_AXIS; i++) {
-		destination[i] = current_position[i] + difference[i] * fraction;
-	}
-
-	
-	calculate_delta(destination);
-         //SERIAL_ECHOPGM("destination[X_AXIS]="); SERIAL_ECHOLN(destination[X_AXIS]);
-         //SERIAL_ECHOPGM("destination[Y_AXIS]="); SERIAL_ECHOLN(destination[Y_AXIS]);
-         //SERIAL_ECHOPGM("destination[Z_AXIS]="); SERIAL_ECHOLN(destination[Z_AXIS]);
-         //SERIAL_ECHOPGM("delta[X_AXIS]="); SERIAL_ECHOLN(delta[X_AXIS]);
-         //SERIAL_ECHOPGM("delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]);
-         //SERIAL_ECHOPGM("delta[Z_AXIS]="); SERIAL_ECHOLN(delta[Z_AXIS]);
-         
-	plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS],
-	destination[E_AXIS], feedrate*feedmultiply/60/100.0,
-	active_extruder);
-}
-#endif // SCARA
-  
-#ifdef DELTA
-  float difference[NUM_AXIS];
-  for (int8_t i=0; i < NUM_AXIS; i++) {
-    difference[i] = destination[i] - current_position[i];
-  }
-  float cartesian_mm = sqrt(sq(difference[X_AXIS]) +
-                            sq(difference[Y_AXIS]) +
-                            sq(difference[Z_AXIS]));
-  if (cartesian_mm < 0.000001) { cartesian_mm = abs(difference[E_AXIS]); }
-  if (cartesian_mm < 0.000001) { return; }
-  float seconds = 6000 * cartesian_mm / feedrate / feedmultiply;
-  int steps = max(1, int(delta_segments_per_second * seconds));
-  // SERIAL_ECHOPGM("mm="); SERIAL_ECHO(cartesian_mm);
-  // SERIAL_ECHOPGM(" seconds="); SERIAL_ECHO(seconds);
-  // SERIAL_ECHOPGM(" steps="); SERIAL_ECHOLN(steps);
-  for (int s = 1; s <= steps; s++) {
-    float fraction = float(s) / float(steps);
-    for(int8_t i=0; i < NUM_AXIS; i++) {
-      destination[i] = current_position[i] + difference[i] * fraction;
-    }
-    calculate_delta(destination);
-    plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS],
-                     destination[E_AXIS], feedrate*feedmultiply/60/100.0,
-                     active_extruder);
-  }
-  
-#endif // DELTA
-
+    
 #ifdef DUAL_X_CARRIAGE
   if (active_extruder_parked)
   {
@@ -5037,7 +4611,6 @@ for (int s = 1; s <= steps; s++) {
   }
 #endif //DUAL_X_CARRIAGE
 
-#if ! (defined DELTA || defined SCARA)
   // Do not use feedmultiply for E or Z only moves
   if( (current_position[X_AXIS] == destination [X_AXIS]) && (current_position[Y_AXIS] == destination [Y_AXIS])) {
 #ifdef MESH_BED_LEVELING
@@ -5053,7 +4626,6 @@ for (int s = 1; s <= steps; s++) {
      plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder);
 #endif
   }
-#endif // !(DELTA || SCARA)
 
   for(int8_t i=0; i < NUM_AXIS; i++) {
     current_position[i] = destination[i];
@@ -5122,84 +4694,6 @@ void controllerFan()
 }
 #endif
 
-#ifdef SCARA
-void calculate_SCARA_forward_Transform(float f_scara[3])
-{
-  // Perform forward kinematics, and place results in delta[3]
-  // The maths and first version has been done by QHARLEY . Integrated into masterbranch 06/2014 and slightly restructured by Joachim Cerny in June 2014
-  
-  float x_sin, x_cos, y_sin, y_cos;
-  
-    //SERIAL_ECHOPGM("f_delta x="); SERIAL_ECHO(f_scara[X_AXIS]);
-    //SERIAL_ECHOPGM(" y="); SERIAL_ECHO(f_scara[Y_AXIS]);
-  
-    x_sin = sin(f_scara[X_AXIS]/SCARA_RAD2DEG) * Linkage_1;
-    x_cos = cos(f_scara[X_AXIS]/SCARA_RAD2DEG) * Linkage_1;
-    y_sin = sin(f_scara[Y_AXIS]/SCARA_RAD2DEG) * Linkage_2;
-    y_cos = cos(f_scara[Y_AXIS]/SCARA_RAD2DEG) * Linkage_2;
-   
-  //  SERIAL_ECHOPGM(" x_sin="); SERIAL_ECHO(x_sin);
-  //  SERIAL_ECHOPGM(" x_cos="); SERIAL_ECHO(x_cos);
-  //  SERIAL_ECHOPGM(" y_sin="); SERIAL_ECHO(y_sin);
-  //  SERIAL_ECHOPGM(" y_cos="); SERIAL_ECHOLN(y_cos);
-  
-    delta[X_AXIS] = x_cos + y_cos + SCARA_offset_x;  //theta
-    delta[Y_AXIS] = x_sin + y_sin + SCARA_offset_y;  //theta+phi
-	
-    //SERIAL_ECHOPGM(" delta[X_AXIS]="); SERIAL_ECHO(delta[X_AXIS]);
-    //SERIAL_ECHOPGM(" delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]);
-}  
-
-void calculate_delta(float cartesian[3]){
-  //reverse kinematics.
-  // Perform reversed kinematics, and place results in delta[3]
-  // The maths and first version has been done by QHARLEY . Integrated into masterbranch 06/2014 and slightly restructured by Joachim Cerny in June 2014
-  
-  float SCARA_pos[2];
-  static float SCARA_C2, SCARA_S2, SCARA_K1, SCARA_K2, SCARA_theta, SCARA_psi; 
-  
-  SCARA_pos[X_AXIS] = cartesian[X_AXIS] * axis_scaling[X_AXIS] - SCARA_offset_x;  //Translate SCARA to standard X Y
-  SCARA_pos[Y_AXIS] = cartesian[Y_AXIS] * axis_scaling[Y_AXIS] - SCARA_offset_y;  // With scaling factor.
-  
-  #if (Linkage_1 == Linkage_2)
-    SCARA_C2 = ( ( sq(SCARA_pos[X_AXIS]) + sq(SCARA_pos[Y_AXIS]) ) / (2 * (float)L1_2) ) - 1;
-  #else
-    SCARA_C2 =   ( sq(SCARA_pos[X_AXIS]) + sq(SCARA_pos[Y_AXIS]) - (float)L1_2 - (float)L2_2 ) / 45000; 
-  #endif
-  
-  SCARA_S2 = sqrt( 1 - sq(SCARA_C2) );
-  
-  SCARA_K1 = Linkage_1 + Linkage_2 * SCARA_C2;
-  SCARA_K2 = Linkage_2 * SCARA_S2;
-  
-  SCARA_theta = ( atan2(SCARA_pos[X_AXIS],SCARA_pos[Y_AXIS])-atan2(SCARA_K1, SCARA_K2) ) * -1;
-  SCARA_psi   =   atan2(SCARA_S2,SCARA_C2);
-  
-  delta[X_AXIS] = SCARA_theta * SCARA_RAD2DEG;  // Multiply by 180/Pi  -  theta is support arm angle
-  delta[Y_AXIS] = (SCARA_theta + SCARA_psi) * SCARA_RAD2DEG;  //       -  equal to sub arm angle (inverted motor)
-  delta[Z_AXIS] = cartesian[Z_AXIS];
-  
-  /*
-  SERIAL_ECHOPGM("cartesian x="); SERIAL_ECHO(cartesian[X_AXIS]);
-  SERIAL_ECHOPGM(" y="); SERIAL_ECHO(cartesian[Y_AXIS]);
-  SERIAL_ECHOPGM(" z="); SERIAL_ECHOLN(cartesian[Z_AXIS]);
-  
-  SERIAL_ECHOPGM("scara x="); SERIAL_ECHO(SCARA_pos[X_AXIS]);
-  SERIAL_ECHOPGM(" y="); SERIAL_ECHOLN(SCARA_pos[Y_AXIS]);
-  
-  SERIAL_ECHOPGM("delta x="); SERIAL_ECHO(delta[X_AXIS]);
-  SERIAL_ECHOPGM(" y="); SERIAL_ECHO(delta[Y_AXIS]);
-  SERIAL_ECHOPGM(" z="); SERIAL_ECHOLN(delta[Z_AXIS]);
-  
-  SERIAL_ECHOPGM("C2="); SERIAL_ECHO(SCARA_C2);
-  SERIAL_ECHOPGM(" S2="); SERIAL_ECHO(SCARA_S2);
-  SERIAL_ECHOPGM(" Theta="); SERIAL_ECHO(SCARA_theta);
-  SERIAL_ECHOPGM(" Psi="); SERIAL_ECHOLN(SCARA_psi);
-  SERIAL_ECHOLN(" ");*/
-}
-
-#endif
-
 #ifdef TEMP_STAT_LEDS
 static bool blue_led = false;
 static bool red_led = false;
@@ -5240,12 +4734,6 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) //default argument s
 	static int killCount = 0;   // make the inactivity button a bit less responsive
    const int KILL_DELAY = 10000;
 #endif
-
-#if defined(HOME_PIN) && HOME_PIN > -1
-   static int homeDebounceCount = 0;   // poor man's debouncing count
-   const int HOME_DEBOUNCE_DELAY = 10000;
-#endif
-   
 	
   if(buflen < (BUFSIZE-1))
     get_command();
@@ -5296,28 +4784,6 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) //default argument s
        kill();
     }
   #endif
-
-#if defined(HOME_PIN) && HOME_PIN > -1
-    // Check to see if we have to home, use poor man's debouncer
-    // ---------------------------------------------------------
-    if ( 0 == READ(HOME_PIN) )
-    {
-       if (homeDebounceCount == 0)
-       {
-          enquecommand_P((PSTR("G28")));
-          homeDebounceCount++;
-          LCD_ALERTMESSAGERPGM(MSG_AUTO_HOME);
-       }
-       else if (homeDebounceCount < HOME_DEBOUNCE_DELAY)
-       {
-          homeDebounceCount++;
-       }
-       else
-       {
-          homeDebounceCount = 0;
-       }
-    }
-#endif
     
   #if defined(CONTROLLERFAN_PIN) && CONTROLLERFAN_PIN > -1
     controllerFan(); //Check if fan should be turned on to cool stepper drivers down

Firmware/cardreader.cpp

@@ -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;
     }

Firmware/mesh_bed_calibration.cpp

@@ -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

Firmware/mesh_bed_calibration.h

@@ -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();
 

Firmware/mesh_bed_leveling.cpp

@@ -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
 

Firmware/pins.h

@@ -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

Firmware/planner.cpp

@@ -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];
         }
-}
+}

Firmware/planner.h

@@ -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);
 
 

Firmware/stepper.cpp

@@ -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;
   }

Firmware/ultralcd.cpp

@@ -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);