selftest

7 files changed, 239 insertions, 56 deletions

diff --git a/Firmware/language_all.cpp b/Firmware/language_all.cpp
index c0f3fe8ba..c1268a1eb 100644
--- a/Firmware/language_all.cpp
+++ b/Firmware/language_all.cpp
@@ -2396,6 +2396,16 @@ const char * const MSG_SELFTEST_LANG_TABLE[LANG_NUM] PROGMEM = {
 	MSG_SELFTEST_DE
 };
 
+const char MSG_SELFTEST_AXIS_EN[] PROGMEM = "Axis";
+const char * const MSG_SELFTEST_AXIS_LANG_TABLE[1] PROGMEM = {
+	MSG_SELFTEST_AXIS_EN
+};
+
+const char MSG_SELFTEST_AXIS_LENGTH_EN[] PROGMEM = "Axis length";
+const char * const MSG_SELFTEST_AXIS_LENGTH_LANG_TABLE[1] PROGMEM = {
+	MSG_SELFTEST_AXIS_LENGTH_EN
+};
+
 const char MSG_SELFTEST_BEDHEATER_EN[] PROGMEM = "Bed / Heater";
 const char MSG_SELFTEST_BEDHEATER_IT[] PROGMEM = "Letto/Riscald.";
 const char MSG_SELFTEST_BEDHEATER_ES[] PROGMEM = "Cama/Calentador";
diff --git a/Firmware/language_all.h b/Firmware/language_all.h
index 9a7e1f8fa..21e5871d8 100644
--- a/Firmware/language_all.h
+++ b/Firmware/language_all.h
@@ -468,6 +468,10 @@ extern const char* const MSG_SD_WRITE_TO_FILE_LANG_TABLE[1];
 #define MSG_SD_WRITE_TO_FILE LANG_TABLE_SELECT_EXPLICIT(MSG_SD_WRITE_TO_FILE_LANG_TABLE, 0)
 extern const char* const MSG_SELFTEST_LANG_TABLE[LANG_NUM];
 #define MSG_SELFTEST LANG_TABLE_SELECT(MSG_SELFTEST_LANG_TABLE)
+extern const char* const MSG_SELFTEST_AXIS_LANG_TABLE[1];
+#define MSG_SELFTEST_AXIS LANG_TABLE_SELECT_EXPLICIT(MSG_SELFTEST_AXIS_LANG_TABLE, 0)
+extern const char* const MSG_SELFTEST_AXIS_LENGTH_LANG_TABLE[1];
+#define MSG_SELFTEST_AXIS_LENGTH LANG_TABLE_SELECT_EXPLICIT(MSG_SELFTEST_AXIS_LENGTH_LANG_TABLE, 0)
 extern const char* const MSG_SELFTEST_BEDHEATER_LANG_TABLE[LANG_NUM];
 #define MSG_SELFTEST_BEDHEATER LANG_TABLE_SELECT(MSG_SELFTEST_BEDHEATER_LANG_TABLE)
 extern const char* const MSG_SELFTEST_CHECK_ALLCORRECT_LANG_TABLE[LANG_NUM];
diff --git a/Firmware/language_en.h b/Firmware/language_en.h
index bbc77ef9b..7cc3ff7e2 100644
--- a/Firmware/language_en.h
+++ b/Firmware/language_en.h
@@ -173,6 +173,8 @@
 #define MSG_SELFTEST_ENDSTOPS				"Endstops"
 #define MSG_SELFTEST_MOTOR					"Motor"
 #define MSG_SELFTEST_ENDSTOP				"Endstop"
+#define MSG_SELFTEST_AXIS					"Axis"
+#define MSG_SELFTEST_AXIS_LENGTH			"Axis length"
 #define(length=20,lines=1) MSG_SELFTEST_ENDSTOP_NOTHIT			"Endstop not hit"
 #define MSG_SELFTEST_OK						"Self test OK"
 #define(length=20,lines=1) MSG_LOOSE_PULLEY					"Loose pulley"
diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp
index d0aa34fda..8798470f6 100644
--- a/Firmware/stepper.cpp
+++ b/Firmware/stepper.cpp
@@ -41,8 +41,10 @@
 //=============================public variables  ============================
 //===========================================================================
 block_t *current_block;  // A pointer to the block currently being traced
-
-
+bool x_min_endstop = false;
+bool x_max_endstop = false;
+bool y_min_endstop = false;
+bool y_max_endstop = false;
 //===========================================================================
 //=============================private variables ============================
 //===========================================================================
@@ -407,9 +409,9 @@ ISR(TIMER1_COMPA_vect)
         {
           #if defined(X_MIN_PIN) && X_MIN_PIN > -1
 			#ifndef TMC2130_SG_HOMING_SW
-				bool x_min_endstop = (READ(X_MIN_PIN) != X_MIN_ENDSTOP_INVERTING);
+				x_min_endstop = (READ(X_MIN_PIN) != X_MIN_ENDSTOP_INVERTING);
 			#else //TMC2130_SG_HOMING_SW
-				bool x_min_endstop = tmc2130_axis_stalled[X_AXIS];
+				x_min_endstop = tmc2130_axis_stalled[X_AXIS];
 			#endif //TMC2130_SG_HOMING_SW
             if(x_min_endstop && old_x_min_endstop && (current_block->steps_x > 0)) {
               endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
@@ -427,9 +429,9 @@ ISR(TIMER1_COMPA_vect)
         {
           #if defined(X_MAX_PIN) && X_MAX_PIN > -1
 			#ifndef TMC2130_SG_HOMING_SW
-				bool x_max_endstop = (READ(X_MAX_PIN) != X_MAX_ENDSTOP_INVERTING);
+				x_max_endstop = (READ(X_MAX_PIN) != X_MAX_ENDSTOP_INVERTING);
 			#else //TMC2130_SG_HOMING_SW
-				bool x_max_endstop = tmc2130_axis_stalled[X_AXIS];
+				x_max_endstop = tmc2130_axis_stalled[X_AXIS];
 			#endif //TMC2130_SG_HOMING_SW
             if(x_max_endstop && old_x_max_endstop && (current_block->steps_x > 0)){
               endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
@@ -451,9 +453,9 @@ ISR(TIMER1_COMPA_vect)
       {
         #if defined(Y_MIN_PIN) && Y_MIN_PIN > -1
 			#ifndef TMC2130_SG_HOMING_SW
-				bool y_min_endstop=(READ(Y_MIN_PIN) != Y_MIN_ENDSTOP_INVERTING);
+				y_min_endstop=(READ(Y_MIN_PIN) != Y_MIN_ENDSTOP_INVERTING);
 			#else //TMC2130_SG_HOMING_SW
-				bool y_min_endstop = tmc2130_axis_stalled[Y_AXIS];
+				y_min_endstop = tmc2130_axis_stalled[Y_AXIS];
 			#endif //TMC2130_SG_HOMING_SW
           if(y_min_endstop && old_y_min_endstop && (current_block->steps_y > 0)) {
             endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
@@ -469,9 +471,9 @@ ISR(TIMER1_COMPA_vect)
       {
         #if defined(Y_MAX_PIN) && Y_MAX_PIN > -1
 			#ifndef TMC2130_SG_HOMING_SW
-				bool y_max_endstop=(READ(Y_MAX_PIN) != Y_MAX_ENDSTOP_INVERTING);
+				y_max_endstop=(READ(Y_MAX_PIN) != Y_MAX_ENDSTOP_INVERTING);
 			#else //TMC2130_SG_HOMING_SW
-				bool y_max_endstop = tmc2130_axis_stalled[Y_AXIS];
+				y_max_endstop = tmc2130_axis_stalled[Y_AXIS];
 			#endif //TMC2130_SG_HOMING_SW
           if(y_max_endstop && old_y_max_endstop && (current_block->steps_y > 0)){
             endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
diff --git a/Firmware/stepper.h b/Firmware/stepper.h
index c9c4f3640..70d1bce5e 100644
--- a/Firmware/stepper.h
+++ b/Firmware/stepper.h
@@ -79,6 +79,10 @@ void checkStepperErrors(); //Print errors detected by the stepper
 void finishAndDisableSteppers();
 
 extern block_t *current_block;  // A pointer to the block currently being traced
+extern bool x_min_endstop;
+extern bool x_max_endstop;
+extern bool y_min_endstop;
+extern bool y_max_endstop;
 
 void quickStop();
 
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index 52b1d6cbb..10a0765bc 100644
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -2012,14 +2012,14 @@ void lcd_bed_calibration_show_result(BedSkewOffsetDetectionResultType result, ui
 }
 
 static void lcd_show_end_stops() {
-    lcd.setCursor(0, 0);
-    lcd_printPGM((PSTR("End stops diag")));
-    lcd.setCursor(0, 1);
-    lcd_printPGM((READ(X_MIN_PIN) ^ X_MIN_ENDSTOP_INVERTING == 1) ? (PSTR("X1")) : (PSTR("X0")));
-    lcd.setCursor(0, 2);
-    lcd_printPGM((READ(Y_MIN_PIN) ^ Y_MIN_ENDSTOP_INVERTING == 1) ? (PSTR("Y1")) : (PSTR("Y0")));
-    lcd.setCursor(0, 3);
-    lcd_printPGM((READ(Z_MIN_PIN) ^ Z_MIN_ENDSTOP_INVERTING == 1) ? (PSTR("Z1")) : (PSTR("Z0")));
+	lcd.setCursor(0, 0);
+	lcd_printPGM((PSTR("End stops diag")));
+	lcd.setCursor(0, 1);
+	lcd_printPGM((READ(X_MIN_PIN) ^ X_MIN_ENDSTOP_INVERTING == 1) ? (PSTR("X1")) : (PSTR("X0")));
+	lcd.setCursor(0, 2);
+	lcd_printPGM((READ(Y_MIN_PIN) ^ Y_MIN_ENDSTOP_INVERTING == 1) ? (PSTR("Y1")) : (PSTR("Y0")));
+	lcd.setCursor(0, 3);
+	lcd_printPGM((READ(Z_MIN_PIN) ^ Z_MIN_ENDSTOP_INVERTING == 1) ? (PSTR("Z1")) : (PSTR("Z0")));
 }
 
 static void menu_show_end_stops() {
@@ -4246,16 +4246,14 @@ static void lcd_selftest()
 	if (_result)
 	{
 		_progress = lcd_selftest_screen(3, _progress, 3, true, 1000);
-		//_result = lcd_selfcheck_check_heater(false);
+		_result = lcd_selfcheck_check_heater(false);
 	}
 
 	if (_result)
 	{
 		//current_position[Z_AXIS] += 15;									//move Z axis higher to avoid false triggering of Z end stop in case that we are very low - just above heatbed
 		_progress = lcd_selftest_screen(4, _progress, 3, true, 2000);
-		//_result = lcd_selfcheck_axis(X_AXIS, X_MAX_POS);
-		homeaxis(X_AXIS);
-
+		_result = lcd_selfcheck_axis_sg(X_AXIS);//, X_MAX_POS);
 	}
 
 	if (_result)
@@ -4263,13 +4261,14 @@ static void lcd_selftest()
 		_progress = lcd_selftest_screen(4, _progress, 3, true, 0);
 
 
-		_result = lcd_selfcheck_pulleys(X_AXIS);
+		//_result = lcd_selfcheck_pulleys(X_AXIS);
 	}
 
 
 	if (_result)
 	{
 		_progress = lcd_selftest_screen(5, _progress, 3, true, 1500);
+		_result = lcd_selfcheck_axis_sg(Y_AXIS);
 		//_result = lcd_selfcheck_axis(Y_AXIS, Y_MAX_POS);
 	}
 
@@ -4282,17 +4281,28 @@ static void lcd_selftest()
 
 	if (_result)
 	{
-		current_position[X_AXIS] = current_position[X_AXIS] - 3;
-		current_position[Y_AXIS] = current_position[Y_AXIS] - 14;
+		//current_position[X_AXIS] = current_position[X_AXIS] + 14;
+		//current_position[Y_AXIS] = current_position[Y_AXIS] + 12;
+#ifdef HAVE_TMC2130_DRIVERS
+		tmc2130_home_exit();
+#endif
+		current_position[X_AXIS] = current_position[X_AXIS] + 14;
+		current_position[Y_AXIS] = current_position[Y_AXIS] + 12;
+
+		//homeaxis(X_AXIS);
+		//homeaxis(Y_AXIS);
+		current_position[Z_AXIS] = current_position[Z_AXIS] + 10;
+		plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[3], manual_feedrate[0] / 60, active_extruder);
+		st_synchronize();
 		_progress = lcd_selftest_screen(6, _progress, 3, true, 1500);
-		//_result = lcd_selfcheck_axis(2, Z_MAX_POS);
-		//enquecommand_P(PSTR("G28 W"));
+		_result = lcd_selfcheck_axis(2, Z_MAX_POS);
+		enquecommand_P(PSTR("G28 W"));
 	}
 
 	if (_result)
 	{
 		_progress = lcd_selftest_screen(7, _progress, 3, true, 2000);
-		//_result = lcd_selfcheck_check_heater(true);
+		_result = lcd_selfcheck_check_heater(true);
 	}
 	if (_result)
 	{
@@ -4318,8 +4328,114 @@ static void lcd_selftest()
 }
 
 
+static bool lcd_selfcheck_axis_sg(char axis) {
+	
+	float axis_length, current_position_init, current_position_final;
+	float measured_axis_length[2];
+	float margin = 100;
+	float max_error_mm = 10;
+	switch (axis) {
+	case 0: axis_length = X_MAX_POS; break;
+	case 1: axis_length = Y_MAX_POS + 8; break;
+	default: axis_length = 210; break;
+	}
+	
+
+	for (char i = 0; i < 2; i++) {
+		SERIAL_ECHOPGM("Current position:");
+		MYSERIAL.println(current_position[axis]);
+		if (i == 0) {
+			current_position[axis] -= (axis_length + margin);
+			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[3], manual_feedrate[0] / 60, active_extruder);
+		}
+		else {
+			current_position[axis] -= margin;
+			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[3], manual_feedrate[0] / 60, active_extruder);
+			st_synchronize();
+			current_position[axis] -= axis_length;
+			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[3], manual_feedrate[0] / 60, active_extruder);
+		}
+
+
+#ifdef HAVE_TMC2130_DRIVERS
+		tmc2130_home_enter(axis);
+#endif
+		st_synchronize();
+#ifdef HAVE_TMC2130_DRIVERS
+		tmc2130_home_exit();
+#endif
+		//current_position[axis] = st_get_position_mm(axis);
+		//plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+
+		SERIAL_ECHOPGM("Current position:");
+		MYSERIAL.println(current_position[axis]);
+		current_position_init = st_get_position_mm(axis);
+		if (i == 0) {
+			current_position[axis] += margin;
+			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[3], manual_feedrate[0] / 60, active_extruder);
+			st_synchronize();
+			current_position[axis] += axis_length;
+			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[3], manual_feedrate[0] / 60, active_extruder);
+#ifdef HAVE_TMC2130_DRIVERS
+			tmc2130_home_enter(axis);
+#endif
+			st_synchronize();
+#ifdef HAVE_TMC2130_DRIVERS
+			tmc2130_home_exit();
+#endif
+			//current_position[axis] = st_get_position_mm(axis);
+			//plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+			SERIAL_ECHOPGM("Current position:");
+			MYSERIAL.println(current_position[axis]);
+			current_position_final = st_get_position_mm(axis);
+		}
+		measured_axis_length[i] = abs(current_position_final - current_position_init);
+		SERIAL_ECHOPGM("Measured axis length:");
+		MYSERIAL.println(measured_axis_length[i]);
+
+
+		if (abs(measured_axis_length[i] - axis_length) > max_error_mm) {
+			//axis length
+#ifdef HAVE_TMC2130_DRIVERS
+			tmc2130_home_exit();
+#endif
+			const char *_error_1;
+			const char *_error_2;
+
+			if (axis == X_AXIS) _error_1 = "X";
+			if (axis == Y_AXIS) _error_1 = "Y";
+			if (axis == Z_AXIS) _error_1 = "Z";
+
+			lcd_selftest_error(9, _error_1, _error_2);
+			return false;
+		}
+	}
+
+	SERIAL_ECHOPGM("Axis length difference:");
+	MYSERIAL.println(abs(measured_axis_length[0] - measured_axis_length[1]));
+	
+		if (abs(measured_axis_length[0] - measured_axis_length[1]) > 1) {
+			//loose pulleys
+			const char *_error_1;
+			const char *_error_2;
+
+			if (axis == X_AXIS) _error_1 = "X";
+			if (axis == Y_AXIS) _error_1 = "Y";
+			if (axis == Z_AXIS) _error_1 = "Z";
+
+			lcd_selftest_error(8, _error_1, _error_2);
+			return false;
+		}
+
+		return true;
+}
+	
+
+
+
 static bool lcd_selfcheck_axis(int _axis, int _travel)
 {
+	
 	bool _stepdone = false;
 	bool _stepresult = false;
 	int _progress = 0;
@@ -4327,37 +4443,41 @@ static bool lcd_selfcheck_axis(int _axis, int _travel)
 	int _err_endstop = 0;
 	int _lcd_refresh = 0;
 	_travel = _travel + (_travel / 10);
-
 	do {
+
 		current_position[_axis] = current_position[_axis] - 1;
 
+
 		plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[3], manual_feedrate[0] / 60, active_extruder);
 		st_synchronize();
-
-		if (READ(X_MIN_PIN) ^ X_MIN_ENDSTOP_INVERTING == 1 || READ(Y_MIN_PIN) ^ Y_MIN_ENDSTOP_INVERTING == 1 || READ(Z_MIN_PIN) ^ Z_MIN_ENDSTOP_INVERTING == 1)
+		
+		if (/*x_min_endstop || y_min_endstop || */(READ(Z_MIN_PIN) ^ Z_MIN_ENDSTOP_INVERTING == 1))
 		{
 			if (_axis == 0)
 			{
-				_stepresult = (READ(X_MIN_PIN) ^ X_MIN_ENDSTOP_INVERTING == 1) ? true : false;
-				_err_endstop = (READ(Y_MIN_PIN) ^ Y_MIN_ENDSTOP_INVERTING == 1) ? 1 : 2;
+				_stepresult = (x_min_endstop) ? true : false;
+				_err_endstop = (y_min_endstop) ? 1 : 2;
 				
 			}
 			if (_axis == 1)
 			{
-				_stepresult = (READ(Y_MIN_PIN) ^ Y_MIN_ENDSTOP_INVERTING == 1) ? true : false;
-				_err_endstop = (READ(X_MIN_PIN) ^ X_MIN_ENDSTOP_INVERTING == 1) ? 0 : 2;
+				_stepresult = (y_min_endstop) ? true : false;
+				_err_endstop = (x_min_endstop) ? 0 : 2;
 				
 			}
 			if (_axis == 2)
 			{
 				_stepresult = (READ(Z_MIN_PIN) ^ Z_MIN_ENDSTOP_INVERTING == 1) ? true : false;
-				_err_endstop = (READ(X_MIN_PIN) ^ X_MIN_ENDSTOP_INVERTING == 1) ? 0 : 1;
+				_err_endstop = (x_min_endstop) ? 0 : 1;
 				/*disable_x();
 				disable_y();
 				disable_z();*/
 			}
 			_stepdone = true;
 		}
+#ifdef HAVE_TMC2130_DRIVERS
+		tmc2130_home_exit();
+#endif
 
 		if (_lcd_refresh < 6)
 		{
@@ -4374,7 +4494,7 @@ static bool lcd_selfcheck_axis(int _axis, int _travel)
 
 		//delay(100);
 		(_travel_done <= _travel) ? _travel_done++ : _stepdone = true;
-
+		
 	} while (!_stepdone);
 
 
@@ -4404,6 +4524,7 @@ static bool lcd_selfcheck_axis(int _axis, int _travel)
 		}
 	}
 
+
 	return _stepresult;
 }
 
@@ -4411,7 +4532,7 @@ static bool lcd_selfcheck_pulleys(int axis)
 {
 	float tmp_motor_loud[3] = DEFAULT_PWM_MOTOR_CURRENT_LOUD;
 	float tmp_motor[3] = DEFAULT_PWM_MOTOR_CURRENT;
-	float current_position_init;
+	float current_position_init, current_position_final;
 	float move;
 	bool endstop_triggered = false;
 	bool result = true;
@@ -4423,80 +4544,111 @@ static bool lcd_selfcheck_pulleys(int axis)
 	if (axis == 0) move = 50; //X_AXIS 
 		else move = 50; //Y_AXIS
 
-		current_position_init = current_position[axis];
-				
-		current_position[axis] += 2;
+		//current_position_init = current_position[axis];
+		current_position_init = st_get_position_mm(axis);
+		current_position[axis] += 5;
 		plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[3], manual_feedrate[0] / 60, active_extruder);
 		for (i = 0; i < 5; i++) {
 			refresh_cmd_timeout();
 			current_position[axis] = current_position[axis] + move;
-			digipot_current(0, 850); //set motor current higher
+			//digipot_current(0, 850); //set motor current higher
 			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[3], 200, active_extruder);
 			st_synchronize();
-			if (SilentModeMenu == 1) digipot_current(0, tmp_motor[0]); //set back to normal operation currents
-			else digipot_current(0, tmp_motor_loud[0]); //set motor current back			
+			//if (SilentModeMenu == 1) digipot_current(0, tmp_motor[0]); //set back to normal operation currents
+			//else digipot_current(0, tmp_motor_loud[0]); //set motor current back			
 			current_position[axis] = current_position[axis] - move;
+#ifdef HAVE_TMC2130_DRIVERS
+			tmc2130_home_enter(axis);
+#endif
 			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[3], 50, active_extruder);
+			
 			st_synchronize();
-			if ((READ(X_MIN_PIN) ^ X_MIN_ENDSTOP_INVERTING == 1) || (READ(Y_MIN_PIN) ^ Y_MIN_ENDSTOP_INVERTING == 1)) {
+			if ((x_min_endstop) || (y_min_endstop)) {
 				lcd_selftest_error(8, (axis == 0) ? "X" : "Y", "");
 				return(false);
 			}
+#ifdef HAVE_TMC2130_DRIVERS
+			tmc2130_home_exit();
+#endif
 		}
 		timeout_counter = millis() + 2500;
 		endstop_triggered = false;
 		manage_inactivity(true);
 		while (!endstop_triggered) {
-			if ((READ(X_MIN_PIN) ^ X_MIN_ENDSTOP_INVERTING == 1) || (READ(Y_MIN_PIN) ^ Y_MIN_ENDSTOP_INVERTING == 1)) {
+			if ((x_min_endstop) || (y_min_endstop)) {
+#ifdef HAVE_TMC2130_DRIVERS
+				tmc2130_home_exit();
+#endif
 				endstop_triggered = true;
-				if (current_position_init - 1 <= current_position[axis] && current_position_init + 1 >= current_position[axis]) {
+				current_position_final = st_get_position_mm(axis);
+
+				SERIAL_ECHOPGM("current_pos_init:");
+				MYSERIAL.println(current_position_init);
+				SERIAL_ECHOPGM("current_pos:");
+				MYSERIAL.println(current_position_final);
+				lcd_selftest_error(8, (axis == 0) ? "X" : "Y", "");
+				
+				if (current_position_init - 1 <= current_position_final && current_position_init + 1 >= current_position_final) {
 					current_position[axis] += 15;
 					plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[3], manual_feedrate[0] / 60, active_extruder);
 					st_synchronize();
 					return(true);
 				}
 				else {
-					lcd_selftest_error(8, (axis == 0) ? "X" : "Y", "");
+
 					return(false);
 				}
 			}
 			else {
-				current_position[axis] -= 1;
+#ifdef HAVE_TMC2130_DRIVERS
+				tmc2130_home_exit();
+#endif
+				//current_position[axis] -= 1;
+				current_position[axis] += 50;
+				plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[3], manual_feedrate[0] / 60, active_extruder);
+				current_position[axis] -= 100;
+#ifdef HAVE_TMC2130_DRIVERS
+				tmc2130_home_enter(axis);
+#endif
 				plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[3], manual_feedrate[0] / 60, active_extruder);
 				st_synchronize();
+				
 				if (millis() > timeout_counter) {
 					lcd_selftest_error(8, (axis == 0) ? "X" : "Y", "");
 					return(false);
 				}
 			}
 		}		
+
+
 }
 
 static bool lcd_selfcheck_endstops()
-{
+{/*
 	bool _result = true;
 
-	if (READ(X_MIN_PIN) ^ X_MIN_ENDSTOP_INVERTING == 1 || READ(Y_MIN_PIN) ^ Y_MIN_ENDSTOP_INVERTING == 1 || READ(Z_MIN_PIN) ^ Z_MIN_ENDSTOP_INVERTING == 1)
+	if (x_min_endstop || y_min_endstop || READ(Z_MIN_PIN) ^ Z_MIN_ENDSTOP_INVERTING == 1)
 	{
-		current_position[0] = (READ(X_MIN_PIN) ^ X_MIN_ENDSTOP_INVERTING == 1) ? current_position[0] = current_position[0] + 10 : current_position[0];
-		current_position[1] = (READ(Y_MIN_PIN) ^ Y_MIN_ENDSTOP_INVERTING == 1) ? current_position[1] = current_position[1] + 10 : current_position[1];
+		current_position[0] = (x_min_endstop) ? current_position[0] = current_position[0] + 10 : current_position[0];
+		current_position[1] = (y_min_endstop) ? current_position[1] = current_position[1] + 10 : current_position[1];
 		current_position[2] = (READ(Z_MIN_PIN) ^ Z_MIN_ENDSTOP_INVERTING == 1) ? current_position[2] = current_position[2] + 10 : current_position[2];
 	}
 	plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[0] / 60, active_extruder);
 	delay(500);
 
-	if (READ(X_MIN_PIN) ^ X_MIN_ENDSTOP_INVERTING == 1 || READ(Y_MIN_PIN) ^ Y_MIN_ENDSTOP_INVERTING == 1 || READ(Z_MIN_PIN) ^ Z_MIN_ENDSTOP_INVERTING == 1)
+	if (x_min_endstop || y_min_endstop || READ(Z_MIN_PIN) ^ Z_MIN_ENDSTOP_INVERTING == 1)
 	{
 		_result = false;
 		char _error[4] = "";
-		if (READ(X_MIN_PIN) ^ X_MIN_ENDSTOP_INVERTING == 1) strcat(_error, "X");
-		if (READ(Y_MIN_PIN) ^ Y_MIN_ENDSTOP_INVERTING == 1) strcat(_error, "Y");
+		if (x_min_endstop) strcat(_error, "X");
+		if (y_min_endstop) strcat(_error, "Y");
 		if (READ(Z_MIN_PIN) ^ Z_MIN_ENDSTOP_INVERTING == 1) strcat(_error, "Z");
 		lcd_selftest_error(3, _error, "");
 	}
 	manage_heater();
 	manage_inactivity(true);
 	return _result;
+	*/
 }
 
 static bool lcd_selfcheck_check_heater(bool _isbed)
@@ -4647,6 +4799,14 @@ static void lcd_selftest_error(int _error_no, const char *_error_1, const char *
 		lcd.setCursor(18, 3);
 		lcd.print(_error_1);
 		break;
+	case 9:
+		lcd.setCursor(0, 2);
+		lcd_printPGM(MSG_SELFTEST_AXIS_LENGTH);
+		lcd.setCursor(0, 3);
+		lcd_printPGM(MSG_SELFTEST_AXIS);
+		lcd.setCursor(18, 3);
+		lcd.print(_error_1);
+		break;
 	}
 
 	delay(1000);
diff --git a/Firmware/ultralcd.h b/Firmware/ultralcd.h
index a9a1cbf05..f17894339 100644
--- a/Firmware/ultralcd.h
+++ b/Firmware/ultralcd.h
@@ -35,6 +35,7 @@ void lcd_mylang();
   static void lcd_selftest();
   static bool lcd_selfcheck_endstops();
   static bool lcd_selfcheck_axis(int _axis, int _travel);
+  static bool lcd_selfcheck_axis_sg(char axis);
   static bool lcd_selfcheck_check_heater(bool _isbed);
   static int  lcd_selftest_screen(int _step, int _progress, int _progress_scale, bool _clear, int _delay);
   static void lcd_selftest_screen_step(int _row, int _col, int _state, const char *_name, const char *_indicator);