diff --git a/Firmware/temperature.h b/Firmware/temperature.h
index da88a53c0..0bf944724 100755
--- a/Firmware/temperature.h
+++ b/Firmware/temperature.h
@@ -99,13 +99,10 @@ extern bool bedPWMDisabled;
   float unscalePID_d(float d);
 
 #endif
-  
-  
-#ifdef BABYSTEPPING
-  extern volatile int babystepsTodo[3];
-#endif
 
-void resetPID(uint8_t extruder);
+
+#ifdef BABYSTEPPING
+extern volatile int babystepsTodo[3];
 
 inline void babystepsTodoZadd(int n)
 {
@@ -115,15 +112,9 @@ inline void babystepsTodoZadd(int n)
         CRITICAL_SECTION_END
     }
 }
+#endif
 
-inline void babystepsTodoZsubtract(int n)
-{
-    if (n != 0) {
-        CRITICAL_SECTION_START
-        babystepsTodo[Z_AXIS] -= n;
-        CRITICAL_SECTION_END
-    }
-}
+void resetPID(uint8_t extruder);
 
 //high level conversion routines, for use outside of temperature.cpp
 //inline so that there is no performance decrease.
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index 25cd55d93..d7c7a25d9 100755
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -3113,12 +3113,7 @@ static void lcd_babystep_z()
 
         if (_md->babystepMemZ < Z_BABYSTEP_MIN) _md->babystepMemZ = Z_BABYSTEP_MIN; //-3999 -> -9.99 mm
         else if (_md->babystepMemZ > Z_BABYSTEP_MAX) _md->babystepMemZ = Z_BABYSTEP_MAX; //0
-        else
-        {
-            CRITICAL_SECTION_START
-            babystepsTodo[Z_AXIS] += (int)lcd_encoder;
-            CRITICAL_SECTION_END
-        }
+        else babystepsTodoZadd(lcd_encoder);
 
 		_md->babystepMemMMZ = _md->babystepMemZ/cs.axis_steps_per_unit[Z_AXIS];
 		_delay(50);