diff --git a/Marlin/cardreader.cpp b/Marlin/cardreader.cpp
index 5aad0194279c5492f827b45f4a3c0983fbb5ec35..673e9d55a7bc17ce21b8f7c92379abbfd8800ad3 100644
--- a/Marlin/cardreader.cpp
+++ b/Marlin/cardreader.cpp
@@ -91,10 +91,10 @@ void CardReader::lsDive(const char *prepend, SdFile parent, const char * const m
     {
       char pn0 = p.name[0];
       if (pn0 == DIR_NAME_FREE) break;
-      if (pn0 == DIR_NAME_DELETED || pn0 == '.'|| pn0 == '_') continue;
-      if (longFilename[0] != '\0' &&
-          (longFilename[0] == '.' || longFilename[0] == '_')) continue;
-      
+      if (pn0 == DIR_NAME_DELETED || pn0 == '.' || pn0 == '_') continue;
+      char lf0 = longFilename[0];
+      if (lf0 == '.' || lf0 == '_') continue;
+
       if (!DIR_IS_FILE_OR_SUBDIR(&p)) continue;
       filenameIsDir=DIR_IS_SUBDIR(&p);
 
diff --git a/Marlin/dogm_lcd_implementation.h b/Marlin/dogm_lcd_implementation.h
index 0bfc8c144a328c8922ee0931d41fd205b564dd3c..ba7b9865b1085fa7d253deaa3807a9e066a50c9e 100644
--- a/Marlin/dogm_lcd_implementation.h
+++ b/Marlin/dogm_lcd_implementation.h
@@ -39,7 +39,6 @@
 #include "ultralcd.h"
 #include "ultralcd_st7920_u8glib_rrd.h"
 
-
 /* Russian language not supported yet, needs custom font
 
 #if LANGUAGE_CHOICE == 6
@@ -57,20 +56,18 @@
 #define DOG_CHAR_WIDTH_LARGE	9
 #define DOG_CHAR_HEIGHT_LARGE	18
 
-
 #define START_ROW				0
 
-
 /* Custom characters defined in font font_6x10_marlin.c */
-#define LCD_STR_BEDTEMP     "\xFE"
 #define LCD_STR_DEGREE      "\xB0"
-#define LCD_STR_THERMOMETER "\xFF"
-#define LCD_STR_UPLEVEL     "\xFB"
 #define LCD_STR_REFRESH     "\xF8"
 #define LCD_STR_FOLDER      "\xF9"
-#define LCD_STR_FEEDRATE    "\xFD"
-#define LCD_STR_CLOCK       "\xFC"
 #define LCD_STR_ARROW_RIGHT "\xFA"
+#define LCD_STR_UPLEVEL     "\xFB"
+#define LCD_STR_CLOCK       "\xFC"
+#define LCD_STR_FEEDRATE    "\xFD"
+#define LCD_STR_BEDTEMP     "\xFE"
+#define LCD_STR_THERMOMETER "\xFF"
 
 #define FONT_STATUSMENU	u8g_font_6x9
 
@@ -167,6 +164,25 @@ static void lcd_printPGM(const char* str)
     }
 }
 
+static void _draw_heater_status(int x, int heater) {
+  bool isBed = heater < 0;
+  int y = 17 + (isBed ? 1 : 0);
+  u8g.setFont(FONT_STATUSMENU);
+  u8g.setPrintPos(x,6);
+  u8g.print(itostr3(int((heater >= 0 ? degTargetHotend(heater) : degTargetBed()) + 0.5)));
+  lcd_printPGM(PSTR(LCD_STR_DEGREE " "));
+  u8g.setPrintPos(x,27);
+  u8g.print(itostr3(int(heater >= 0 ? degHotend(heater) : degBed()) + 0.5));
+  lcd_printPGM(PSTR(LCD_STR_DEGREE " "));
+  if (!isHeatingHotend(0)) {
+    u8g.drawBox(x+7,y,2,2);
+  }
+  else {
+    u8g.setColorIndex(0); // white on black
+    u8g.drawBox(x+7,y,2,2);
+    u8g.setColorIndex(1); // black on white
+  }
+}
 
 static void lcd_implementation_status_screen()
 {
@@ -176,8 +192,7 @@ static void lcd_implementation_status_screen()
  u8g.setColorIndex(1);	// black on white
  
  // Symbols menu graphics, animated fan
- if ((blink % 2) &&  fanSpeed )	u8g.drawBitmapP(9,1,STATUS_SCREENBYTEWIDTH,STATUS_SCREENHEIGHT,status_screen0_bmp);
-	else u8g.drawBitmapP(9,1,STATUS_SCREENBYTEWIDTH,STATUS_SCREENHEIGHT,status_screen1_bmp);
+ u8g.drawBitmapP(9,1,STATUS_SCREENBYTEWIDTH,STATUS_SCREENHEIGHT, (blink % 2) && fanSpeed ? status_screen0_bmp : status_screen1_bmp);
  
  #ifdef SDSUPPORT
  //SD Card Symbol
@@ -213,73 +228,17 @@ static void lcd_implementation_status_screen()
 		 }
  #endif
  
- // Extruder 1
- u8g.setFont(FONT_STATUSMENU);
- u8g.setPrintPos(6,6);
- u8g.print(itostr3(int(degTargetHotend(0) + 0.5)));
- lcd_printPGM(PSTR(LCD_STR_DEGREE " "));
- u8g.setPrintPos(6,27);
- u8g.print(itostr3(int(degHotend(0) + 0.5)));
- lcd_printPGM(PSTR(LCD_STR_DEGREE " "));
- if (!isHeatingHotend(0)) u8g.drawBox(13,17,2,2);
-	else
-		{
-		 u8g.setColorIndex(0);	// white on black
-		 u8g.drawBox(13,17,2,2);
-		 u8g.setColorIndex(1);	// black on white
-		}
- 
- // Extruder 2
- u8g.setFont(FONT_STATUSMENU);
- #if EXTRUDERS > 1
- u8g.setPrintPos(31,6);
- u8g.print(itostr3(int(degTargetHotend(1) + 0.5)));
- lcd_printPGM(PSTR(LCD_STR_DEGREE " "));
- u8g.setPrintPos(31,27);
- u8g.print(itostr3(int(degHotend(1) + 0.5)));
- lcd_printPGM(PSTR(LCD_STR_DEGREE " "));
- if (!isHeatingHotend(1)) u8g.drawBox(38,17,2,2);
-	else
-		{
-		 u8g.setColorIndex(0);	// white on black
-		 u8g.drawBox(38,17,2,2);
-		 u8g.setColorIndex(1);	// black on white
-		}
- #endif
- 
- // Extruder 3
- u8g.setFont(FONT_STATUSMENU);
- # if EXTRUDERS > 2
- u8g.setPrintPos(55,6);
- u8g.print(itostr3(int(degTargetHotend(2) + 0.5)));
- lcd_printPGM(PSTR(LCD_STR_DEGREE " "));
- u8g.setPrintPos(55,27);
- u8g.print(itostr3(int(degHotend(2) + 0.5)));
- lcd_printPGM(PSTR(LCD_STR_DEGREE " "));
- if (!isHeatingHotend(2)) u8g.drawBox(62,17,2,2);
-	else
-		{
-		 u8g.setColorIndex(0);	// white on black
-		 u8g.drawBox(62,17,2,2);
-		 u8g.setColorIndex(1);	// black on white
-		}
- #endif
- 
- // Heatbed
- u8g.setFont(FONT_STATUSMENU);
- u8g.setPrintPos(81,6);
- u8g.print(itostr3(int(degTargetBed() + 0.5)));
- lcd_printPGM(PSTR(LCD_STR_DEGREE " "));
- u8g.setPrintPos(81,27);
- u8g.print(itostr3(int(degBed() + 0.5)));
- lcd_printPGM(PSTR(LCD_STR_DEGREE " "));
- if (!isHeatingBed()) u8g.drawBox(88,18,2,2);
-	else
-		{
-		 u8g.setColorIndex(0);	// white on black
-		 u8g.drawBox(88,18,2,2);
-		 u8g.setColorIndex(1);	// black on white
-		}
+  // Extruders
+  _draw_heater_status(6, 0);
+  #if EXTRUDERS > 1
+    _draw_heater_status(31, 1);
+    #if EXTRUDERS > 2
+      _draw_heater_status(55, 2);
+    #endif
+  #endif
+
+  // Heatbed
+  _draw_heater_status(81, -1);
  
  // Fan
  u8g.setFont(FONT_STATUSMENU);
@@ -360,7 +319,7 @@ static void lcd_implementation_drawmenu_generic(uint8_t row, const char* pstr, c
 		   } else u8g.setColorIndex(1); // unmarked text is black on white
 		
 		u8g.setPrintPos(0 * DOG_CHAR_WIDTH, (row + 1) * DOG_CHAR_HEIGHT);
-		if (pre_char != '>') u8g.print(pre_char); else u8g.print(' ');	// Row selector is obsolete
+		u8g.print(pre_char == '>' ? ' ' : pre_char);	// Row selector is obsolete
 
 
     while( (c = pgm_read_byte(pstr)) != '\0' )
@@ -378,57 +337,29 @@ static void lcd_implementation_drawmenu_generic(uint8_t row, const char* pstr, c
 		u8g.setColorIndex(1);		// restore settings to black on white
 }
 
-static void lcd_implementation_drawmenu_setting_edit_generic(uint8_t row, const char* pstr, char pre_char, char* data)
-{
-    static unsigned int fkt_cnt = 0;
-	char c;
-    uint8_t n = LCD_WIDTH - 1 - 2 - strlen(data);
-		
-		u8g.setPrintPos(0 * DOG_CHAR_WIDTH, (row + 1) * DOG_CHAR_HEIGHT);
-		u8g.print(pre_char);
-	
-    while( (c = pgm_read_byte(pstr)) != '\0' )
-    {
-			u8g.print(c);
+static void _drawmenu_setting_edit_generic(uint8_t row, const char* pstr, char pre_char, char* data, bool pgm) {
+  char c;
+  uint8_t n = LCD_WIDTH - 1 - 2 - (pgm ? strlen_P(data) : strlen(data));
 		
-        pstr++;
-        n--;
-    }
-	
-		u8g.print(':');
-
-    while(n--){
-					u8g.print(' ');
-			  }
+  u8g.setPrintPos(0 * DOG_CHAR_WIDTH, (row + 1) * DOG_CHAR_HEIGHT);
+  u8g.print(pre_char);
 
-		u8g.print(data);
-}
-
-static void lcd_implementation_drawmenu_setting_edit_generic_P(uint8_t row, const char* pstr, char pre_char, const char* data)
-{
-    char c;
-    uint8_t n= LCD_WIDTH - 1 - 2 - strlen_P(data);
+  while( (c = pgm_read_byte(pstr)) != '\0' ) {
+    u8g.print(c);
+    pstr++;
+    n--;
+  }
 
-		u8g.setPrintPos(0 * DOG_CHAR_WIDTH, (row + 1) * DOG_CHAR_HEIGHT);
-		u8g.print(pre_char);
-	
-    while( (c = pgm_read_byte(pstr)) != '\0' )
-    {
-			u8g.print(c);
-		
-        pstr++;
-        n--;
-    }
+  u8g.print(':');
 
-		u8g.print(':');
-	
-    while(n--){
-					u8g.print(' ');
-			  }
+  while(n--) u8g.print(' ');
 
-		lcd_printPGM(data);
+  if (pgm) { lcd_printPGM(data); } else { u8g.print(data); }
 }
 
+#define lcd_implementation_drawmenu_setting_edit_generic(row, pstr, pre_char, data) _drawmenu_setting_edit_generic(row, pstr, pre_char, data, false)
+#define lcd_implementation_drawmenu_setting_edit_generic_P(row, pstr, pre_char, data) _drawmenu_setting_edit_generic(row, pstr, pre_char, data, true)
+
 #define lcd_implementation_drawmenu_setting_edit_int3_selected(row, pstr, pstr2, data, minValue, maxValue) lcd_implementation_drawmenu_setting_edit_generic(row, pstr, '>', itostr3(*(data)))
 #define lcd_implementation_drawmenu_setting_edit_int3(row, pstr, pstr2, data, minValue, maxValue) lcd_implementation_drawmenu_setting_edit_generic(row, pstr, ' ', itostr3(*(data)))
 #define lcd_implementation_drawmenu_setting_edit_float3_selected(row, pstr, pstr2, data, minValue, maxValue) lcd_implementation_drawmenu_setting_edit_generic(row, pstr, '>', ftostr3(*(data)))
@@ -474,118 +405,40 @@ void lcd_implementation_drawedit(const char* pstr, char* value)
 		u8g.print(value);
 }
 
-static void lcd_implementation_drawmenu_sdfile_selected(uint8_t row, const char* pstr, const char* filename, char* longFilename)
-{
-    char c;
-    uint8_t n = LCD_WIDTH - 1;
+static void _drawmenu_sd(uint8_t row, const char* pstr, const char* filename, char * const longFilename, bool isDir, bool isSelected) {
+  char c;
+  uint8_t n = LCD_WIDTH - 1;
 
-    if (longFilename[0] != '\0')
-    {
-        filename = longFilename;
-        longFilename[LCD_WIDTH-1] = '\0';
-    }
+  if (longFilename[0] != '\0') {
+    filename = longFilename;
+    longFilename[n] = '\0';
+  }
 
-		u8g.setColorIndex(1);		// black on white
-		u8g.drawBox (0, row*DOG_CHAR_HEIGHT + 3, 128, DOG_CHAR_HEIGHT);
-		u8g.setColorIndex(0);		// following text must be white on black
-		u8g.setPrintPos(0 * DOG_CHAR_WIDTH, (row + 1) * DOG_CHAR_HEIGHT);
-		u8g.print(' ');	// Indent by 1 char
-	   
-    while((c = *filename) != '\0')
-    {
-		u8g.print(c);
-        filename++;
-        n--;
-    }
-    while(n--){
-					u8g.print(' ');
-			   }
-	u8g.setColorIndex(1);		// black on white
-}
-
-static void lcd_implementation_drawmenu_sdfile(uint8_t row, const char* pstr, const char* filename, char* longFilename)
-{
-    char c;
-    uint8_t n = LCD_WIDTH - 1;
+  if (isSelected) {
+    u8g.setColorIndex(1); // black on white
+    u8g.drawBox (0, row*DOG_CHAR_HEIGHT + 3, 128, DOG_CHAR_HEIGHT);
+    u8g.setColorIndex(0); // following text must be white on black
+  }
 
-    if (longFilename[0] != '\0')
-    {
-        filename = longFilename;
-        longFilename[LCD_WIDTH-1] = '\0';
-    }
+  u8g.setPrintPos(0 * DOG_CHAR_WIDTH, (row + 1) * DOG_CHAR_HEIGHT);
+  u8g.print(' ');	// Indent by 1 char
 
-		u8g.setPrintPos(0 * DOG_CHAR_WIDTH, (row + 1) * DOG_CHAR_HEIGHT);
-		u8g.print(' ');
-		
-while((c = *filename) != '\0')
-    {
-			u8g.print(c);
-		
-        filename++;
-        n--;
-    }
-    while(n--){
-					u8g.print(' ');
-			   }
+  if (isDir) u8g.print(LCD_STR_FOLDER[0]);
 
-}
+  while((c = *filename) != '\0') {
+    u8g.print(c);
+    filename++;
+    n--;
+  }
+  while(n--) u8g.print(' ');
 
-static void lcd_implementation_drawmenu_sddirectory_selected(uint8_t row, const char* pstr, const char* filename, char* longFilename)
-{
-    char c;
-    uint8_t n = LCD_WIDTH - 2;
-		
-    if (longFilename[0] != '\0')
-    {
-        filename = longFilename;
-        longFilename[LCD_WIDTH-2] = '\0';
-    }
-		u8g.setColorIndex(1);		// black on white
-		u8g.drawBox (0, row*DOG_CHAR_HEIGHT + 3, 128, DOG_CHAR_HEIGHT);
-		u8g.setColorIndex(0);		// following text must be white on black
-		u8g.setPrintPos(0 * DOG_CHAR_WIDTH, (row + 1) * DOG_CHAR_HEIGHT);
-		u8g.print(' ');	// Indent by 1 char
-		u8g.print(LCD_STR_FOLDER[0]);		
-	   
-    while((c = *filename) != '\0')
-    {
-			u8g.print(c);
-		
-        filename++;
-        n--;
-    }
-    while(n--){
-					u8g.print(' ');
-			   }
-	u8g.setColorIndex(1);		// black on white
+  if (isSelected) u8g.setColorIndex(1); // black on white
 }
 
-static void lcd_implementation_drawmenu_sddirectory(uint8_t row, const char* pstr, const char* filename, char* longFilename)
-{
-    char c;
-    uint8_t n = LCD_WIDTH - 2;
-
-    if (longFilename[0] != '\0')
-    {
-        filename = longFilename;
-        longFilename[LCD_WIDTH-2] = '\0';
-    }
-
-		u8g.setPrintPos(0 * DOG_CHAR_WIDTH, (row + 1) * DOG_CHAR_HEIGHT);
-		u8g.print(' ');
-		u8g.print(LCD_STR_FOLDER[0]);
-
-    while((c = *filename) != '\0')
-    {
-			u8g.print(c);
-		
-        filename++;
-        n--;
-    }
-    while(n--){
-					u8g.print(' ');
-			   }
-}
+#define lcd_implementation_drawmenu_sdfile_selected(row, pstr, filename, longFilename) _drawmenu_sd(row, pstr, filename, longFilename, false, true)
+#define lcd_implementation_drawmenu_sdfile(row, pstr, filename, longFilename) _drawmenu_sd(row, pstr, filename, longFilename, false, false)
+#define lcd_implementation_drawmenu_sddirectory_selected(row, pstr, filename, longFilename) _drawmenu_sd(row, pstr, filename, longFilename, true, true)
+#define lcd_implementation_drawmenu_sddirectory(row, pstr, filename, longFilename) _drawmenu_sd(row, pstr, filename, longFilename, true, false)
 
 #define lcd_implementation_drawmenu_back_selected(row, pstr, data) lcd_implementation_drawmenu_generic(row, pstr, LCD_STR_UPLEVEL[0], LCD_STR_UPLEVEL[0])
 #define lcd_implementation_drawmenu_back(row, pstr, data) lcd_implementation_drawmenu_generic(row, pstr, ' ', LCD_STR_UPLEVEL[0])
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index 3fdbb6cbfe9268a19267726bb62e73cf7972194a..c0bcc8f3aa4b93f18d9ced73e06c3662b3f0a679 100644
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -184,6 +184,14 @@ menuFunc_t callbackFunc;
 // place-holders for Ki and Kd edits
 float raw_Ki, raw_Kd;
 
+static void lcd_goto_menu(menuFunc_t menu, const uint32_t encoder=0, const bool feedback=true) {
+  if (currentMenu != menu) {
+    currentMenu = menu;
+    encoderPosition = encoder;
+    if (feedback) lcd_quick_feedback();
+  }
+}
+
 /* Main status screen. It's up to the implementation specific part to show what is needed. As this is very display dependent */
 static void lcd_status_screen()
 {
@@ -218,9 +226,7 @@ static void lcd_status_screen()
 
     if (current_click)
     {
-        currentMenu = lcd_main_menu;
-        encoderPosition = 0;
-        lcd_quick_feedback();
+        lcd_goto_menu(lcd_main_menu);
         lcd_implementation_init(); // to maybe revive the LCD if static electricity killed it.
 #ifdef FILAMENT_LCD_DISPLAY
         message_millis=millis();  //get status message to show up for a while
@@ -255,7 +261,7 @@ static void lcd_status_screen()
 
     if (feedmultiply < 10)
         feedmultiply = 10;
-    if (feedmultiply > 999)
+    else if (feedmultiply > 999)
         feedmultiply = 999;
 #endif//ULTIPANEL
 }
@@ -263,10 +269,8 @@ static void lcd_status_screen()
 #ifdef ULTIPANEL
 static void lcd_return_to_status()
 {
-    encoderPosition = 0;
-    currentMenu = lcd_status_screen;
+    lcd_goto_menu(lcd_status_screen, 0, false);
 }
-
 static void lcd_sdcard_pause()
 {
     card.pauseSDPrint();
@@ -357,65 +361,20 @@ void lcd_set_home_offsets()
 
 
 #ifdef BABYSTEPPING
-static void lcd_babystep_x()
-{
-    if (encoderPosition != 0)
-    {
-        babystepsTodo[X_AXIS]+=(int)encoderPosition;
-        encoderPosition=0;
-        lcdDrawUpdate = 1;
-    }
-    if (lcdDrawUpdate)
-    {
-        lcd_implementation_drawedit(PSTR(MSG_BABYSTEPPING_X),"");
-    }
-    if (LCD_CLICKED)
-    {
-        lcd_quick_feedback();
-        currentMenu = lcd_tune_menu;
-        encoderPosition = 0;
-    }
-}
 
-static void lcd_babystep_y()
-{
-    if (encoderPosition != 0)
-    {
-        babystepsTodo[Y_AXIS]+=(int)encoderPosition;
-        encoderPosition=0;
-        lcdDrawUpdate = 1;
-    }
-    if (lcdDrawUpdate)
-    {
-        lcd_implementation_drawedit(PSTR(MSG_BABYSTEPPING_Y),"");
+  static void _lcd_babystep(int axis, const char *msg) {
+    if (encoderPosition != 0) {
+      babystepsTodo[axis] += (int)encoderPosition;
+      encoderPosition = 0;
+      lcdDrawUpdate = 1;
     }
-    if (LCD_CLICKED)
-    {
-        lcd_quick_feedback();
-        currentMenu = lcd_tune_menu;
-        encoderPosition = 0;
-    }
-}
+    if (lcdDrawUpdate) lcd_implementation_drawedit(PSTR(msg), "");
+    if (LCD_CLICKED) lcd_goto_menu(lcd_tune_menu);
+  }
+  static void lcd_babystep_x() { _lcd_babystep(X_AXIS, MSG_BABYSTEPPING_X); }
+  static void lcd_babystep_y() { _lcd_babystep(Y_AXIS, MSG_BABYSTEPPING_Y); }
+  static void lcd_babystep_z() { _lcd_babystep(Z_AXIS, MSG_BABYSTEPPING_Z); }
 
-static void lcd_babystep_z()
-{
-    if (encoderPosition != 0)
-    {
-        babystepsTodo[Z_AXIS]+=BABYSTEP_Z_MULTIPLICATOR*(int)encoderPosition;
-        encoderPosition=0;
-        lcdDrawUpdate = 1;
-    }
-    if (lcdDrawUpdate)
-    {
-        lcd_implementation_drawedit(PSTR(MSG_BABYSTEPPING_Z),"");
-    }
-    if (LCD_CLICKED)
-    {
-        lcd_quick_feedback();
-        currentMenu = lcd_tune_menu;
-        encoderPosition = 0;
-    }
-}
 #endif //BABYSTEPPING
 
 static void lcd_tune_menu()
@@ -644,96 +603,28 @@ static void lcd_prepare_menu()
 float move_menu_scale;
 static void lcd_move_menu_axis();
 
-static void lcd_move_x()
-{
-    if (encoderPosition != 0)
-    {
-        refresh_cmd_timeout();
-        current_position[X_AXIS] += float((int)encoderPosition) * move_menu_scale;
-        if (min_software_endstops && current_position[X_AXIS] < X_MIN_POS)
-            current_position[X_AXIS] = X_MIN_POS;
-        if (max_software_endstops && current_position[X_AXIS] > X_MAX_POS)
-            current_position[X_AXIS] = X_MAX_POS;
-        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[X_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[X_AXIS]/60, active_extruder);
-        #endif
-        lcdDrawUpdate = 1;
-    }
-    if (lcdDrawUpdate)
-    {
-        lcd_implementation_drawedit(PSTR("X"), ftostr31(current_position[X_AXIS]));
-    }
-    if (LCD_CLICKED)
-    {
-        lcd_quick_feedback();
-        currentMenu = lcd_move_menu_axis;
-        encoderPosition = 0;
-    }
-}
-static void lcd_move_y()
-{
-    if (encoderPosition != 0)
-    {
-        refresh_cmd_timeout();
-        current_position[Y_AXIS] += float((int)encoderPosition) * move_menu_scale;
-        if (min_software_endstops && current_position[Y_AXIS] < Y_MIN_POS)
-            current_position[Y_AXIS] = Y_MIN_POS;
-        if (max_software_endstops && current_position[Y_AXIS] > Y_MAX_POS)
-            current_position[Y_AXIS] = Y_MAX_POS;
-        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[Y_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[Y_AXIS]/60, active_extruder);
-        #endif
-        lcdDrawUpdate = 1;
-    }
-    if (lcdDrawUpdate)
-    {
-        lcd_implementation_drawedit(PSTR("Y"), ftostr31(current_position[Y_AXIS]));
-    }
-    if (LCD_CLICKED)
-    {
-        lcd_quick_feedback();
-        currentMenu = lcd_move_menu_axis;
-        encoderPosition = 0;
-    }
-}
-static void lcd_move_z()
-{
-    if (encoderPosition != 0)
-    {
-        refresh_cmd_timeout();
-        current_position[Z_AXIS] += float((int)encoderPosition) * move_menu_scale;
-        if (min_software_endstops && current_position[Z_AXIS] < Z_MIN_POS)
-            current_position[Z_AXIS] = Z_MIN_POS;
-        if (max_software_endstops && current_position[Z_AXIS] > Z_MAX_POS)
-            current_position[Z_AXIS] = Z_MAX_POS;
-        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[Z_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[Z_AXIS]/60, active_extruder);
-        #endif
-        lcdDrawUpdate = 1;
-    }
-    if (lcdDrawUpdate)
-    {
-        lcd_implementation_drawedit(PSTR("Z"), ftostr31(current_position[Z_AXIS]));
-    }
-    if (LCD_CLICKED)
-    {
-        lcd_quick_feedback();
-        currentMenu = lcd_move_menu_axis;
-        encoderPosition = 0;
-    }
+static void _lcd_move(const char *name, int axis, int min, int max) {
+  if (encoderPosition != 0) {
+    refresh_cmd_timeout();
+    current_position[axis] += float((int)encoderPosition) * move_menu_scale;
+    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]));
+  if (LCD_CLICKED) lcd_goto_menu(lcd_move_menu_axis);
 }
+static void lcd_move_x() { _lcd_move(PSTR("X"), X_AXIS, X_MIN_POS, X_MAX_POS); }
+static void lcd_move_y() { _lcd_move(PSTR("Y"), Y_AXIS, Y_MIN_POS, Y_MAX_POS); }
+static void lcd_move_z() { _lcd_move(PSTR("Z"), Z_AXIS, Z_MIN_POS, Z_MAX_POS); }
+
 static void lcd_move_e()
 {
     if (encoderPosition != 0)
@@ -752,12 +643,7 @@ static void lcd_move_e()
     {
         lcd_implementation_drawedit(PSTR("Extruder"), ftostr31(current_position[E_AXIS]));
     }
-    if (LCD_CLICKED)
-    {
-        lcd_quick_feedback();
-        currentMenu = lcd_move_menu_axis;
-        encoderPosition = 0;
-    }
+    if (LCD_CLICKED) lcd_goto_menu(lcd_move_menu_axis);
 }
 
 static void lcd_move_menu_axis()
@@ -951,12 +837,7 @@ static void lcd_set_contrast()
     {
         lcd_implementation_drawedit(PSTR(MSG_CONTRAST), itostr2(lcd_contrast));
     }
-    if (LCD_CLICKED)
-    {
-        lcd_quick_feedback();
-        currentMenu = lcd_control_menu;
-        encoderPosition = 0;
-    }
+    if (LCD_CLICKED) lcd_goto_menu(lcd_control_menu);
 }
 #endif
 
@@ -979,7 +860,8 @@ static void lcd_control_retract_menu()
     MENU_ITEM_EDIT(float3, MSG_CONTROL_RETRACT_RECOVERF, &retract_recover_feedrate, 1, 999);
     END_MENU();
 }
-#endif
+
+#endif //FWRETRACT
 
 #if SDCARDDETECT == -1
 static void lcd_sd_refresh()
@@ -1052,27 +934,12 @@ void lcd_sdcard_menu()
         if (LCD_CLICKED) \
         { \
             *((_type*)editValue) = ((_type)((int32_t)encoderPosition + minEditValue)) / scale; \
-            lcd_quick_feedback(); \
-            currentMenu = prevMenu; \
-            encoderPosition = prevEncoderPosition; \
+            lcd_goto_menu(prevMenu, prevEncoderPosition); \
         } \
     } \
-    void menu_edit_callback_ ## _name () \
-    { \
-        if ((int32_t)encoderPosition < 0) \
-            encoderPosition = 0; \
-        if ((int32_t)encoderPosition > maxEditValue) \
-            encoderPosition = maxEditValue; \
-        if (lcdDrawUpdate) \
-            lcd_implementation_drawedit(editLabel, _strFunc(((_type)((int32_t)encoderPosition + minEditValue)) / scale)); \
-        if (LCD_CLICKED) \
-        { \
-            *((_type*)editValue) = ((_type)((int32_t)encoderPosition + minEditValue)) / scale; \
-            lcd_quick_feedback(); \
-            currentMenu = prevMenu; \
-            encoderPosition = prevEncoderPosition; \
-            (*callbackFunc)();\
-        } \
+    void menu_edit_callback_ ## _name () { \
+        menu_edit_ ## _name (); \
+        if (LCD_CLICKED) (*callbackFunc)(); \
     } \
     static void menu_action_setting_edit_ ## _name (const char* pstr, _type* ptr, _type minValue, _type maxValue) \
     { \
@@ -1157,24 +1024,10 @@ static void lcd_quick_feedback()
 }
 
 /** Menu action functions **/
-static void menu_action_back(menuFunc_t data)
-{
-    currentMenu = data;
-    encoderPosition = 0;
-}
-static void menu_action_submenu(menuFunc_t data)
-{
-    currentMenu = data;
-    encoderPosition = 0;
-}
-static void menu_action_gcode(const char* pgcode)
-{
-    enquecommand_P(pgcode);
-}
-static void menu_action_function(menuFunc_t data)
-{
-    (*data)();
-}
+static void menu_action_back(menuFunc_t data) { lcd_goto_menu(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_function(menuFunc_t data) { (*data)(); }
 static void menu_action_sdfile(const char* filename, char* longFilename)
 {
     char cmd[30];
@@ -1459,10 +1312,8 @@ void lcd_buttons_update()
 
     //manage encoder rotation
     uint8_t enc=0;
-    if(buttons&EN_A)
-        enc|=(1<<0);
-    if(buttons&EN_B)
-        enc|=(1<<1);
+    if (buttons & EN_A) enc |= B01;
+    if (buttons & EN_B) enc |= B10;
     if(enc != lastEncoderBits)
     {
         switch(enc)
@@ -1611,6 +1462,7 @@ char *itostr31(const int &xx)
   return conv;
 }
 
+// Convert int to rj string with 123 or -12 format
 char *itostr3(const int &x)
 {
   int xx = x;
@@ -1653,47 +1505,25 @@ char *itostr3left(const int &xx)
   return conv;
 }
 
-char *itostr4(const int &xx)
-{
-  if (xx >= 1000)
-    conv[0]=(xx/1000)%10+'0';
-  else
-    conv[0]=' ';
-  if (xx >= 100)
-    conv[1]=(xx/100)%10+'0';
-  else
-    conv[1]=' ';
-  if (xx >= 10)
-    conv[2]=(xx/10)%10+'0';
-  else
-    conv[2]=' ';
-  conv[3]=(xx)%10+'0';
-  conv[4]=0;
+// Convert int to rj string with 1234 format
+char *itostr4(const int &xx) {
+  conv[0] = xx >= 1000 ? (xx / 1000) % 10 + '0' : ' ';
+  conv[1] = xx >= 100 ? (xx / 100) % 10 + '0' : ' ';
+  conv[2] = xx >= 10 ? (xx / 10) % 10 + '0' : ' ';
+  conv[3] = xx % 10 + '0';
+  conv[4] = 0;
   return conv;
 }
 
-//  convert float to string with 12345 format
-char *ftostr5(const float &x)
-{
-  long xx=abs(x);
-  if (xx >= 10000)
-    conv[0]=(xx/10000)%10+'0';
-  else
-    conv[0]=' ';
-  if (xx >= 1000)
-    conv[1]=(xx/1000)%10+'0';
-  else
-    conv[1]=' ';
-  if (xx >= 100)
-    conv[2]=(xx/100)%10+'0';
-  else
-    conv[2]=' ';
-  if (xx >= 10)
-    conv[3]=(xx/10)%10+'0';
-  else
-    conv[3]=' ';
-  conv[4]=(xx)%10+'0';
-  conv[5]=0;
+//  convert float to rj string with 12345 format
+char *ftostr5(const float &x) {
+  long xx = abs(x);
+  conv[0] = xx >= 10000 ? (xx / 10000) % 10 + '0' : ' ';
+  conv[1] = xx >= 1000 ? (xx / 1000) % 10 + '0' : ' ';
+  conv[2] = xx >= 100 ? (xx / 100) % 10 + '0' : ' ';
+  conv[3] = xx >= 10 ? (xx / 10) % 10 + '0' : ' ';
+  conv[4] = xx % 10 + '0';
+  conv[5] = 0;
   return conv;
 }
 
diff --git a/Marlin/ultralcd.h b/Marlin/ultralcd.h
index 652693ce813a9d7213aab0a1477b5e5accf25153..339955e592c52ac7f3d45fda00682bec5f913019 100644
--- a/Marlin/ultralcd.h
+++ b/Marlin/ultralcd.h
@@ -105,7 +105,8 @@
 
   #define LCD_MESSAGEPGM(x) 
   #define LCD_ALERTMESSAGEPGM(x) 
-#endif 
+
+#endif //ULTRA_LCD
 
 char *itostr2(const uint8_t &x);
 char *itostr31(const int &xx);
@@ -122,4 +123,4 @@ char *ftostr5(const float &x);
 char *ftostr51(const float &x);
 char *ftostr52(const float &x);
 
-#endif //ULTRALCD
+#endif //ULTRALCD_H