From 5abee3d3e51acaabe2ec25f1ab717c3a835c3590 Mon Sep 17 00:00:00 2001
From: vintagepc <53943260+vintagepc@users.noreply.github.com>
Date: Tue, 11 Feb 2020 19:44:26 -0500
Subject: [PATCH] Better display handling

---
 Firmware/ultralcd.cpp | 42 ++++++++++++++++++++----------------------
 1 file changed, 20 insertions(+), 22 deletions(-)

diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index 3dbb6831..ed1b91d4 100755
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -7439,39 +7439,37 @@ static void lcd_belttest_v()
 
 void lcd_belttest()
 {
-    bool _result = true;
-    
+    lcd_clear();
 	// Belttest requires high power mode. Enable it.
 	FORCE_HIGH_POWER_START;
     
     uint16_t   X = eeprom_read_word((uint16_t*)(EEPROM_BELTSTATUS_X));
     uint16_t   Y = eeprom_read_word((uint16_t*)(EEPROM_BELTSTATUS_Y));
-	lcd_puts_at_P(0,0,_i("Checking X..."));
-
+	lcd_puts_at_P(0,0,_i("Checking X axis  ")); // share message with selftest
+	lcd_set_cursor(0,1), lcd_printf_P(PSTR("X: %d "),X);
     KEEPALIVE_STATE(IN_HANDLER);
     
-    _result = lcd_selfcheck_axis_sg(X_AXIS);
-    X = eeprom_read_word((uint16_t*)(EEPROM_BELTSTATUS_X));
-	lcd_set_cursor(0,1), lcd_printf_P(PSTR("X: %d "),X); // Trailing space for done/error spacing if !_result
-    if (_result){
-		lcd_printf_P(_i("Done"));
-        lcd_puts_at_P(0,2,_i("Checking Y..."));
-        _result = lcd_selfcheck_axis_sg(Y_AXIS);
-        Y = eeprom_read_word((uint16_t*)(EEPROM_BELTSTATUS_Y));
+	// N.B: it doesn't make sense to handle !lcd_selfcheck...() because selftest_sg throws its own error screen
+	// that clobbers ours, with more info than we could provide. So on fail we just fall through to take us back to status.
+    if (lcd_selfcheck_axis_sg(X_AXIS)){
+		X = eeprom_read_word((uint16_t*)(EEPROM_BELTSTATUS_X));
+		lcd_printf_P(PSTR("-> %d"),X); // Show new X value next to old one.
+        lcd_puts_at_P(0,2,_i("Checking Y axis  "));
 		lcd_set_cursor(0,3), lcd_printf_P(PSTR("Y: %d "),Y);
+		if (lcd_selfcheck_axis_sg(Y_AXIS))
+		{
+			Y = eeprom_read_word((uint16_t*)(EEPROM_BELTSTATUS_Y));
+			lcd_printf_P(PSTR("-> %d"),Y);
+			lcd_set_custom_characters_nextpage();
+			lcd_set_cursor(19, 3);
+			lcd_print(char(2));
+			lcd_wait_for_click_delay(10);
+		}
     }
-    
-    if (!_result) { // If error on X, error appears after X measurement, else done or error after Y measurement.
-        lcd_printf_P(_i("Error"));
-    } else {
-        lcd_printf_P(_i("Done"));
-    }   
-
-	lcd_puts_at_P(19,3,char(2)); // Checkmark
+	
 	FORCE_HIGH_POWER_END;
-    
     KEEPALIVE_STATE(NOT_BUSY);
-    _delay(3000);
+	lcd_set_custom_characters(); // restore status screen chars.
 }
 #endif //TMC2130