diff --git a/doc/DeskControl UI mockup - LCD version.psd b/doc/DeskControl UI mockup - LCD version.psd
index 7c7baed..7ad3a60 100644
Binary files a/doc/DeskControl UI mockup - LCD version.psd and b/doc/DeskControl UI mockup - LCD version.psd differ
diff --git a/doc/testscript.md b/doc/testscript.md
index 953e73e..bff95b4 100644
--- a/doc/testscript.md
+++ b/doc/testscript.md
@@ -3,7 +3,7 @@ As I really do not want to explain to my employer how I broke my desk, this docu
 
 ## Startup
 
-1. Height sensor must report at least 3 values within 5mm of each other in 500 ms intervals to be considered stable.
+1. Height sensor must report at least 3 values within 10mm of each other to be considered stable.
 1. If the EEPROM has not been initialized yet, the height offset setup screen must be displayed.
 1. Otherwise, the home screen must be displayed.
 
diff --git a/src/fonts/FreeSansBold18pt7b.trimmed.h b/src/fonts/FreeSansBold18pt7b.trimmed.h
index 81efb2d..7612649 100644
--- a/src/fonts/FreeSansBold18pt7b.trimmed.h
+++ b/src/fonts/FreeSansBold18pt7b.trimmed.h
@@ -4,7 +4,7 @@
   https: //github.com/MvRens/AdafruitGFXFontTools
 
   Source: FreeSansBold18pt7b.h
-  Filter: [0-9\.mSTOPMenu]
+  Filter: [ 0-9\.mSTOPMenuERCalibrt]
 
 */
 
@@ -52,54 +52,101 @@ const uint8_t FreeSansBold18pt7bTrimmedBitmaps[] PROGMEM = {
   0xE0, 0x3D, 0xF0, 0x1F, 0xF8, 0x0F, 0xFC, 0x07, 0xFE, 0x03, 0xFF, 0x83,
   0xF7, 0xE3, 0xFB, 0xFF, 0xFC, 0xFF, 0xFE, 0x3F, 0xDF, 0x07, 0xCF, 0x80,
   0x07, 0x80, 0x03, 0xDF, 0x03, 0xE7, 0xC3, 0xE3, 0xFF, 0xF0, 0xFF, 0xF0,
-  0x3F, 0xF0, 0x07, 0xE0, 0x00, 0xFF, 0x00, 0xFF, 0xFF, 0x00, 0xFF, 0xFF,
-  0x00, 0xFF, 0xFF, 0x01, 0xFF, 0xFF, 0x81, 0xFF, 0xFF, 0x81, 0xFF, 0xFF,
-  0x81, 0xFF, 0xFF, 0x81, 0xFF, 0xFB, 0xC3, 0xDF, 0xFB, 0xC3, 0xDF, 0xFB,
-  0xC3, 0xDF, 0xFB, 0xC3, 0xDF, 0xF9, 0xC7, 0xDF, 0xF9, 0xE7, 0x9F, 0xF9,
-  0xE7, 0x9F, 0xF9, 0xE7, 0x9F, 0xF9, 0xE7, 0x9F, 0xF8, 0xFF, 0x1F, 0xF8,
-  0xFF, 0x1F, 0xF8, 0xFF, 0x1F, 0xF8, 0xFF, 0x1F, 0xF8, 0x7F, 0x1F, 0xF8,
-  0x7E, 0x1F, 0xF8, 0x7E, 0x1F, 0xF8, 0x7E, 0x1F, 0xF8, 0x3E, 0x1F, 0x00,
-  0x7F, 0x00, 0x01, 0xFF, 0xF0, 0x01, 0xFF, 0xFC, 0x03, 0xFF, 0xFF, 0x01,
-  0xFC, 0x1F, 0xC1, 0xF8, 0x03, 0xF1, 0xF8, 0x00, 0xFC, 0xF8, 0x00, 0x3E,
-  0x7C, 0x00, 0x1F, 0x7C, 0x00, 0x07, 0xFE, 0x00, 0x03, 0xFF, 0x00, 0x01,
-  0xFF, 0x80, 0x00, 0xFF, 0xC0, 0x00, 0x7F, 0xE0, 0x00, 0x3F, 0xF0, 0x00,
-  0x1F, 0xF8, 0x00, 0x0F, 0xBE, 0x00, 0x0F, 0x9F, 0x00, 0x07, 0xCF, 0xC0,
-  0x07, 0xE3, 0xF0, 0x07, 0xE0, 0xFE, 0x0F, 0xE0, 0x7F, 0xFF, 0xE0, 0x0F,
-  0xFF, 0xE0, 0x03, 0xFF, 0xE0, 0x00, 0x3F, 0x80, 0x00, 0xFF, 0xFC, 0x1F,
-  0xFF, 0xE3, 0xFF, 0xFE, 0x7F, 0xFF, 0xEF, 0x80, 0xFF, 0xF0, 0x0F, 0xFE,
-  0x00, 0xFF, 0xC0, 0x1F, 0xF8, 0x03, 0xFF, 0x00, 0x7F, 0xE0, 0x1F, 0xFC,
-  0x07, 0xEF, 0xFF, 0xFD, 0xFF, 0xFF, 0x3F, 0xFF, 0xC7, 0xFF, 0xE0, 0xF8,
-  0x00, 0x1F, 0x00, 0x03, 0xE0, 0x00, 0x7C, 0x00, 0x0F, 0x80, 0x01, 0xF0,
-  0x00, 0x3E, 0x00, 0x07, 0xC0, 0x00, 0xF8, 0x00, 0x1F, 0x00, 0x00, 0x07,
-  0xF8, 0x01, 0xFF, 0xF0, 0x3F, 0xFF, 0x87, 0xFF, 0xFC, 0x7E, 0x0F, 0xCF,
-  0xC0, 0x7E, 0xF8, 0x03, 0xEF, 0x80, 0x3E, 0xF8, 0x00, 0x0F, 0xC0, 0x00,
-  0xFF, 0x00, 0x07, 0xFF, 0xC0, 0x3F, 0xFF, 0x81, 0xFF, 0xFC, 0x03, 0xFF,
-  0xE0, 0x01, 0xFF, 0x00, 0x03, 0xF0, 0x00, 0x1F, 0xF8, 0x01, 0xFF, 0x80,
-  0x1F, 0xFC, 0x03, 0xFF, 0xE0, 0x7E, 0x7F, 0xFF, 0xE3, 0xFF, 0xFC, 0x1F,
-  0xFF, 0x00, 0x3F, 0xC0, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
-  0xFF, 0xF0, 0x1F, 0x00, 0x03, 0xE0, 0x00, 0x7C, 0x00, 0x0F, 0x80, 0x01,
-  0xF0, 0x00, 0x3E, 0x00, 0x07, 0xC0, 0x00, 0xF8, 0x00, 0x1F, 0x00, 0x03,
-  0xE0, 0x00, 0x7C, 0x00, 0x0F, 0x80, 0x01, 0xF0, 0x00, 0x3E, 0x00, 0x07,
-  0xC0, 0x00, 0xF8, 0x00, 0x1F, 0x00, 0x03, 0xE0, 0x00, 0x7C, 0x00, 0x0F,
-  0x80, 0x01, 0xF0, 0x00, 0x3E, 0x00, 0x07, 0xE0, 0x07, 0xFE, 0x03, 0xFF,
-  0xE0, 0xFF, 0xF8, 0x7E, 0x1F, 0x1F, 0x03, 0xCF, 0x80, 0xFB, 0xE0, 0x1E,
-  0xFF, 0xFF, 0xBF, 0xFF, 0xEF, 0xFF, 0xFB, 0xE0, 0x00, 0xF8, 0x00, 0x3F,
-  0x03, 0xE7, 0xE1, 0xF9, 0xFF, 0xFC, 0x3F, 0xFE, 0x07, 0xFF, 0x00, 0x7F,
-  0x00, 0xF8, 0xF8, 0x3F, 0x1F, 0x7F, 0x9F, 0xF3, 0xFF, 0xFF, 0xFF, 0x7F,
-  0xFF, 0xFF, 0xFF, 0xC3, 0xF8, 0x7F, 0xF8, 0x3F, 0x07, 0xFE, 0x07, 0xC0,
-  0xFF, 0xC0, 0xF8, 0x1F, 0xF8, 0x1F, 0x03, 0xFF, 0x03, 0xE0, 0x7F, 0xE0,
-  0x7C, 0x0F, 0xFC, 0x0F, 0x81, 0xFF, 0x81, 0xF0, 0x3F, 0xF0, 0x3E, 0x07,
-  0xFE, 0x07, 0xC0, 0xFF, 0xC0, 0xF8, 0x1F, 0xF8, 0x1F, 0x03, 0xFF, 0x03,
-  0xE0, 0x7F, 0xE0, 0x7C, 0x0F, 0x80, 0xF8, 0xF8, 0x7D, 0xFF, 0x3F, 0xFF,
-  0xDF, 0xFF, 0xEF, 0xE1, 0xFF, 0xE0, 0x7F, 0xE0, 0x3F, 0xF0, 0x1F, 0xF8,
-  0x0F, 0xFC, 0x07, 0xFE, 0x03, 0xFF, 0x01, 0xFF, 0x80, 0xFF, 0xC0, 0x7F,
-  0xE0, 0x3F, 0xF0, 0x1F, 0xF8, 0x0F, 0xFC, 0x07, 0xFE, 0x03, 0xE0, 0xF8,
-  0x0F, 0xFC, 0x07, 0xFE, 0x03, 0xFF, 0x01, 0xFF, 0x80, 0xFF, 0xC0, 0x7F,
-  0xE0, 0x3F, 0xF0, 0x1F, 0xF8, 0x0F, 0xFC, 0x07, 0xFE, 0x03, 0xFF, 0x01,
-  0xFF, 0x80, 0xFF, 0xC0, 0xFF, 0xF0, 0xFF, 0xFF, 0xFF, 0x7F, 0xFF, 0x9F,
-  0xF7, 0xC7, 0xE3, 0xE0 };
+  0x3F, 0xF0, 0x07, 0xE0, 0x00, 0x00, 0xFF, 0x00, 0x07, 0xFF, 0x80, 0x3F,
+  0xFF, 0xC0, 0xFF, 0xFF, 0xC3, 0xF8, 0x1F, 0x87, 0xE0, 0x1F, 0x9F, 0x80,
+  0x1F, 0x3E, 0x00, 0x1F, 0x7C, 0x00, 0x3F, 0xF0, 0x00, 0x03, 0xE0, 0x00,
+  0x07, 0xC0, 0x00, 0x0F, 0x80, 0x00, 0x1F, 0x00, 0x00, 0x3E, 0x00, 0x00,
+  0x7C, 0x00, 0x00, 0xF8, 0x00, 0x00, 0xF8, 0x00, 0x7D, 0xF0, 0x00, 0xFB,
+  0xF0, 0x03, 0xF3, 0xF0, 0x0F, 0xC7, 0xF0, 0x3F, 0x87, 0xFF, 0xFE, 0x07,
+  0xFF, 0xF8, 0x03, 0xFF, 0xC0, 0x01, 0xFE, 0x00, 0xFF, 0xFF, 0xDF, 0xFF,
+  0xFB, 0xFF, 0xFF, 0x7F, 0xFF, 0xEF, 0x80, 0x01, 0xF0, 0x00, 0x3E, 0x00,
+  0x07, 0xC0, 0x00, 0xF8, 0x00, 0x1F, 0x00, 0x03, 0xE0, 0x00, 0x7F, 0xFF,
+  0xCF, 0xFF, 0xF9, 0xFF, 0xFF, 0x3F, 0xFF, 0xE7, 0xC0, 0x00, 0xF8, 0x00,
+  0x1F, 0x00, 0x03, 0xE0, 0x00, 0x7C, 0x00, 0x0F, 0x80, 0x01, 0xF0, 0x00,
+  0x3F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC, 0xFF, 0x00,
+  0xFF, 0xFF, 0x00, 0xFF, 0xFF, 0x00, 0xFF, 0xFF, 0x01, 0xFF, 0xFF, 0x81,
+  0xFF, 0xFF, 0x81, 0xFF, 0xFF, 0x81, 0xFF, 0xFF, 0x81, 0xFF, 0xFB, 0xC3,
+  0xDF, 0xFB, 0xC3, 0xDF, 0xFB, 0xC3, 0xDF, 0xFB, 0xC3, 0xDF, 0xF9, 0xC7,
+  0xDF, 0xF9, 0xE7, 0x9F, 0xF9, 0xE7, 0x9F, 0xF9, 0xE7, 0x9F, 0xF9, 0xE7,
+  0x9F, 0xF8, 0xFF, 0x1F, 0xF8, 0xFF, 0x1F, 0xF8, 0xFF, 0x1F, 0xF8, 0xFF,
+  0x1F, 0xF8, 0x7F, 0x1F, 0xF8, 0x7E, 0x1F, 0xF8, 0x7E, 0x1F, 0xF8, 0x7E,
+  0x1F, 0xF8, 0x3E, 0x1F, 0x00, 0x7F, 0x00, 0x01, 0xFF, 0xF0, 0x01, 0xFF,
+  0xFC, 0x03, 0xFF, 0xFF, 0x01, 0xFC, 0x1F, 0xC1, 0xF8, 0x03, 0xF1, 0xF8,
+  0x00, 0xFC, 0xF8, 0x00, 0x3E, 0x7C, 0x00, 0x1F, 0x7C, 0x00, 0x07, 0xFE,
+  0x00, 0x03, 0xFF, 0x00, 0x01, 0xFF, 0x80, 0x00, 0xFF, 0xC0, 0x00, 0x7F,
+  0xE0, 0x00, 0x3F, 0xF0, 0x00, 0x1F, 0xF8, 0x00, 0x0F, 0xBE, 0x00, 0x0F,
+  0x9F, 0x00, 0x07, 0xCF, 0xC0, 0x07, 0xE3, 0xF0, 0x07, 0xE0, 0xFE, 0x0F,
+  0xE0, 0x7F, 0xFF, 0xE0, 0x0F, 0xFF, 0xE0, 0x03, 0xFF, 0xE0, 0x00, 0x3F,
+  0x80, 0x00, 0xFF, 0xFC, 0x1F, 0xFF, 0xE3, 0xFF, 0xFE, 0x7F, 0xFF, 0xEF,
+  0x80, 0xFF, 0xF0, 0x0F, 0xFE, 0x00, 0xFF, 0xC0, 0x1F, 0xF8, 0x03, 0xFF,
+  0x00, 0x7F, 0xE0, 0x1F, 0xFC, 0x07, 0xEF, 0xFF, 0xFD, 0xFF, 0xFF, 0x3F,
+  0xFF, 0xC7, 0xFF, 0xE0, 0xF8, 0x00, 0x1F, 0x00, 0x03, 0xE0, 0x00, 0x7C,
+  0x00, 0x0F, 0x80, 0x01, 0xF0, 0x00, 0x3E, 0x00, 0x07, 0xC0, 0x00, 0xF8,
+  0x00, 0x1F, 0x00, 0x00, 0xFF, 0xFF, 0x07, 0xFF, 0xFE, 0x3F, 0xFF, 0xF9,
+  0xFF, 0xFF, 0xCF, 0x80, 0x3F, 0x7C, 0x00, 0xFB, 0xE0, 0x07, 0xDF, 0x00,
+  0x3E, 0xF8, 0x01, 0xF7, 0xC0, 0x0F, 0x3E, 0x00, 0xF9, 0xFF, 0xFF, 0x8F,
+  0xFF, 0xF8, 0x7F, 0xFF, 0xC3, 0xFF, 0xFF, 0x1F, 0x00, 0xFC, 0xF8, 0x03,
+  0xE7, 0xC0, 0x1F, 0x3E, 0x00, 0xF9, 0xF0, 0x07, 0xCF, 0x80, 0x3E, 0x7C,
+  0x01, 0xF3, 0xE0, 0x0F, 0x9F, 0x00, 0x7C, 0xF8, 0x03, 0xF7, 0xC0, 0x0F,
+  0xC0, 0x07, 0xF8, 0x01, 0xFF, 0xF0, 0x3F, 0xFF, 0x87, 0xFF, 0xFC, 0x7E,
+  0x0F, 0xCF, 0xC0, 0x7E, 0xF8, 0x03, 0xEF, 0x80, 0x3E, 0xF8, 0x00, 0x0F,
+  0xC0, 0x00, 0xFF, 0x00, 0x07, 0xFF, 0xC0, 0x3F, 0xFF, 0x81, 0xFF, 0xFC,
+  0x03, 0xFF, 0xE0, 0x01, 0xFF, 0x00, 0x03, 0xF0, 0x00, 0x1F, 0xF8, 0x01,
+  0xFF, 0x80, 0x1F, 0xFC, 0x03, 0xFF, 0xE0, 0x7E, 0x7F, 0xFF, 0xE3, 0xFF,
+  0xFC, 0x1F, 0xFF, 0x00, 0x3F, 0xC0, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+  0xFF, 0xFF, 0xFF, 0xF0, 0x1F, 0x00, 0x03, 0xE0, 0x00, 0x7C, 0x00, 0x0F,
+  0x80, 0x01, 0xF0, 0x00, 0x3E, 0x00, 0x07, 0xC0, 0x00, 0xF8, 0x00, 0x1F,
+  0x00, 0x03, 0xE0, 0x00, 0x7C, 0x00, 0x0F, 0x80, 0x01, 0xF0, 0x00, 0x3E,
+  0x00, 0x07, 0xC0, 0x00, 0xF8, 0x00, 0x1F, 0x00, 0x03, 0xE0, 0x00, 0x7C,
+  0x00, 0x0F, 0x80, 0x01, 0xF0, 0x00, 0x3E, 0x00, 0x07, 0xF8, 0x07, 0xFF,
+  0x83, 0xFF, 0xF1, 0xFF, 0xFE, 0x7C, 0x1F, 0xBE, 0x03, 0xE0, 0x00, 0xF8,
+  0x01, 0xFE, 0x0F, 0xFF, 0x8F, 0xFF, 0xE7, 0xF8, 0xFB, 0xF0, 0x3E, 0xF8,
+  0x0F, 0xBE, 0x07, 0xEF, 0xC3, 0xFB, 0xFF, 0xFE, 0x7F, 0xFF, 0x8F, 0xFB,
+  0xF1, 0xF8, 0xFC, 0xF8, 0x00, 0x3E, 0x00, 0x0F, 0x80, 0x03, 0xE0, 0x00,
+  0xF8, 0x00, 0x3E, 0x00, 0x0F, 0x80, 0x03, 0xE7, 0xE0, 0xFB, 0xFC, 0x3F,
+  0xFF, 0xCF, 0xFF, 0xF3, 0xF8, 0x7E, 0xFC, 0x0F, 0xBF, 0x03, 0xFF, 0x80,
+  0x7F, 0xE0, 0x1F, 0xF8, 0x07, 0xFE, 0x01, 0xFF, 0x80, 0x7F, 0xF0, 0x3F,
+  0xFC, 0x0F, 0xBF, 0x87, 0xEF, 0xFF, 0xF3, 0xFF, 0xFC, 0xFB, 0xFC, 0x3E,
+  0x7E, 0x00, 0x07, 0xE0, 0x07, 0xFE, 0x03, 0xFF, 0xE0, 0xFF, 0xF8, 0x7E,
+  0x1F, 0x1F, 0x03, 0xCF, 0x80, 0xFB, 0xE0, 0x1E, 0xFF, 0xFF, 0xBF, 0xFF,
+  0xEF, 0xFF, 0xFB, 0xE0, 0x00, 0xF8, 0x00, 0x3F, 0x03, 0xE7, 0xE1, 0xF9,
+  0xFF, 0xFC, 0x3F, 0xFE, 0x07, 0xFF, 0x00, 0x7F, 0x00, 0xFF, 0xFF, 0xF0,
+  0x00, 0x1F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+  0xFF, 0xC0, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+  0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xC0, 0xF8, 0xF8, 0x3F, 0x1F, 0x7F,
+  0x9F, 0xF3, 0xFF, 0xFF, 0xFF, 0x7F, 0xFF, 0xFF, 0xFF, 0xC3, 0xF8, 0x7F,
+  0xF8, 0x3F, 0x07, 0xFE, 0x07, 0xC0, 0xFF, 0xC0, 0xF8, 0x1F, 0xF8, 0x1F,
+  0x03, 0xFF, 0x03, 0xE0, 0x7F, 0xE0, 0x7C, 0x0F, 0xFC, 0x0F, 0x81, 0xFF,
+  0x81, 0xF0, 0x3F, 0xF0, 0x3E, 0x07, 0xFE, 0x07, 0xC0, 0xFF, 0xC0, 0xF8,
+  0x1F, 0xF8, 0x1F, 0x03, 0xFF, 0x03, 0xE0, 0x7F, 0xE0, 0x7C, 0x0F, 0x80,
+  0xF8, 0xF8, 0x7D, 0xFF, 0x3F, 0xFF, 0xDF, 0xFF, 0xEF, 0xE1, 0xFF, 0xE0,
+  0x7F, 0xE0, 0x3F, 0xF0, 0x1F, 0xF8, 0x0F, 0xFC, 0x07, 0xFE, 0x03, 0xFF,
+  0x01, 0xFF, 0x80, 0xFF, 0xC0, 0x7F, 0xE0, 0x3F, 0xF0, 0x1F, 0xF8, 0x0F,
+  0xFC, 0x07, 0xFE, 0x03, 0xE0, 0xF8, 0xFF, 0x7F, 0xFF, 0xFF, 0xFF, 0xE1,
+  0xF8, 0x3E, 0x07, 0xC0, 0xF8, 0x1F, 0x03, 0xE0, 0x7C, 0x0F, 0x81, 0xF0,
+  0x3E, 0x07, 0xC0, 0xF8, 0x1F, 0x03, 0xE0, 0x00, 0x3E, 0x1F, 0x0F, 0x87,
+  0xC3, 0xE7, 0xFF, 0xFF, 0xFF, 0x3E, 0x1F, 0x0F, 0x87, 0xC3, 0xE1, 0xF0,
+  0xF8, 0x7C, 0x3E, 0x1F, 0x0F, 0x87, 0xF3, 0xF8, 0xFC, 0x3E, 0xF8, 0x0F,
+  0xFC, 0x07, 0xFE, 0x03, 0xFF, 0x01, 0xFF, 0x80, 0xFF, 0xC0, 0x7F, 0xE0,
+  0x3F, 0xF0, 0x1F, 0xF8, 0x0F, 0xFC, 0x07, 0xFE, 0x03, 0xFF, 0x01, 0xFF,
+  0x80, 0xFF, 0xC0, 0xFF, 0xF0, 0xFF, 0xFF, 0xFF, 0x7F, 0xFF, 0x9F, 0xF7,
+  0xC7, 0xE3, 0xE0 };
 
 const GFXglyph FreeSansBold18pt7bTrimmedGlyphs[] PROGMEM = {
+  {     0,   0,   0,  10,    0,    1 },   // 0x20 ' '
+  {     0,   0,   0,   0,    0,    0 },   // 0x21 '!'
+  {     0,   0,   0,   0,    0,    0 },   // 0x22 '"'
+  {     0,   0,   0,   0,    0,    0 },   // 0x23 '#'
+  {     0,   0,   0,   0,    0,    0 },   // 0x24 '$'
+  {     0,   0,   0,   0,    0,    0 },   // 0x25 '%'
+  {     0,   0,   0,   0,    0,    0 },   // 0x26 '&'
+  {     0,   0,   0,   0,    0,    0 },   // 0x27 '''
+  {     0,   0,   0,   0,    0,    0 },   // 0x28 '('
+  {     0,   0,   0,   0,    0,    0 },   // 0x29 ')'
+  {     0,   0,   0,   0,    0,    0 },   // 0x2A '*'
+  {     0,   0,   0,   0,    0,    0 },   // 0x2B '+'
+  {     0,   0,   0,   0,    0,    0 },   // 0x2C ','
+  {     0,   0,   0,   0,    0,    0 },   // 0x2D '-'
   {     0,   5,   5,   9,    2,   -4 },   // 0x2E '.'
   {     0,   0,   0,   0,    0,    0 },   // 0x2F '/'
   {     4,  17,  25,  19,    1,  -24 },   // 0x30 '0'
@@ -121,9 +168,9 @@ const GFXglyph FreeSansBold18pt7bTrimmedGlyphs[] PROGMEM = {
   {     0,   0,   0,   0,    0,    0 },   // 0x40 '@'
   {     0,   0,   0,   0,    0,    0 },   // 0x41 'A'
   {     0,   0,   0,   0,    0,    0 },   // 0x42 'B'
-  {     0,   0,   0,   0,    0,    0 },   // 0x43 'C'
+  {   521,  23,  26,  25,    1,  -25 },   // 0x43 'C'
   {     0,   0,   0,   0,    0,    0 },   // 0x44 'D'
-  {     0,   0,   0,   0,    0,    0 },   // 0x45 'E'
+  {   596,  19,  26,  23,    3,  -25 },   // 0x45 'E'
   {     0,   0,   0,   0,    0,    0 },   // 0x46 'F'
   {     0,   0,   0,   0,    0,    0 },   // 0x47 'G'
   {     0,   0,   0,   0,    0,    0 },   // 0x48 'H'
@@ -131,14 +178,14 @@ const GFXglyph FreeSansBold18pt7bTrimmedGlyphs[] PROGMEM = {
   {     0,   0,   0,   0,    0,    0 },   // 0x4A 'J'
   {     0,   0,   0,   0,    0,    0 },   // 0x4B 'K'
   {     0,   0,   0,   0,    0,    0 },   // 0x4C 'L'
-  {   521,  24,  26,  30,    3,  -25 },   // 0x4D 'M'
+  {   658,  24,  26,  30,    3,  -25 },   // 0x4D 'M'
   {     0,   0,   0,   0,    0,    0 },   // 0x4E 'N'
-  {   599,  25,  26,  27,    1,  -25 },   // 0x4F 'O'
-  {   681,  19,  26,  24,    3,  -25 },   // 0x50 'P'
+  {   736,  25,  26,  27,    1,  -25 },   // 0x4F 'O'
+  {   818,  19,  26,  24,    3,  -25 },   // 0x50 'P'
   {     0,   0,   0,   0,    0,    0 },   // 0x51 'Q'
-  {     0,   0,   0,   0,    0,    0 },   // 0x52 'R'
-  {   743,  20,  26,  24,    2,  -25 },   // 0x53 'S'
-  {   808,  19,  26,  23,    2,  -25 },   // 0x54 'T'
+  {   880,  21,  26,  25,    3,  -25 },   // 0x52 'R'
+  {   949,  20,  26,  24,    2,  -25 },   // 0x53 'S'
+  {  1014,  19,  26,  23,    2,  -25 },   // 0x54 'T'
   {     0,   0,   0,   0,    0,    0 },   // 0x55 'U'
   {     0,   0,   0,   0,    0,    0 },   // 0x56 'V'
   {     0,   0,   0,   0,    0,    0 },   // 0x57 'W'
@@ -151,31 +198,31 @@ const GFXglyph FreeSansBold18pt7bTrimmedGlyphs[] PROGMEM = {
   {     0,   0,   0,   0,    0,    0 },   // 0x5E '^'
   {     0,   0,   0,   0,    0,    0 },   // 0x5F '_'
   {     0,   0,   0,   0,    0,    0 },   // 0x60 '`'
-  {     0,   0,   0,   0,    0,    0 },   // 0x61 'a'
-  {     0,   0,   0,   0,    0,    0 },   // 0x62 'b'
+  {  1076,  18,  19,  20,    1,  -18 },   // 0x61 'a'
+  {  1119,  18,  26,  22,    2,  -25 },   // 0x62 'b'
   {     0,   0,   0,   0,    0,    0 },   // 0x63 'c'
   {     0,   0,   0,   0,    0,    0 },   // 0x64 'd'
-  {   870,  18,  19,  20,    1,  -18 },   // 0x65 'e'
+  {  1178,  18,  19,  20,    1,  -18 },   // 0x65 'e'
   {     0,   0,   0,   0,    0,    0 },   // 0x66 'f'
   {     0,   0,   0,   0,    0,    0 },   // 0x67 'g'
   {     0,   0,   0,   0,    0,    0 },   // 0x68 'h'
-  {     0,   0,   0,   0,    0,    0 },   // 0x69 'i'
+  {  1221,   5,  26,  10,    2,  -25 },   // 0x69 'i'
   {     0,   0,   0,   0,    0,    0 },   // 0x6A 'j'
   {     0,   0,   0,   0,    0,    0 },   // 0x6B 'k'
-  {     0,   0,   0,   0,    0,    0 },   // 0x6C 'l'
-  {   913,  27,  19,  31,    2,  -18 },   // 0x6D 'm'
-  {   978,  17,  19,  21,    2,  -18 },   // 0x6E 'n'
+  {  1238,   5,  26,   9,    2,  -25 },   // 0x6C 'l'
+  {  1255,  27,  19,  31,    2,  -18 },   // 0x6D 'm'
+  {  1320,  17,  19,  21,    2,  -18 },   // 0x6E 'n'
   {     0,   0,   0,   0,    0,    0 },   // 0x6F 'o'
   {     0,   0,   0,   0,    0,    0 },   // 0x70 'p'
   {     0,   0,   0,   0,    0,    0 },   // 0x71 'q'
-  {     0,   0,   0,   0,    0,    0 },   // 0x72 'r'
+  {  1361,  11,  19,  14,    2,  -18 },   // 0x72 'r'
   {     0,   0,   0,   0,    0,    0 },   // 0x73 's'
-  {     0,   0,   0,   0,    0,    0 },   // 0x74 't'
-  {  1019,  17,  19,  21,    2,  -18 } }; // 0x75 'u'
+  {  1388,   9,  23,  12,    1,  -22 },   // 0x74 't'
+  {  1414,  17,  19,  21,    2,  -18 } }; // 0x75 'u'
 
 const GFXfont FreeSansBold18pt7bTrimmed PROGMEM = {
   (uint8_t  *)FreeSansBold18pt7bTrimmedBitmaps,
   (GFXglyph *)FreeSansBold18pt7bTrimmedGlyphs,
-  0x2E, 0x75, 42 };
+  0x20, 0x75, 42 };
 
-// Approx. 1571 bytes
+// Approx. 2064 bytes
diff --git a/src/fonts/trim.bat b/src/fonts/trim.bat
index c1e7361..31ac2e5 100644
--- a/src/fonts/trim.bat
+++ b/src/fonts/trim.bat
@@ -2,6 +2,6 @@
 
 REM It is a nuisance to keep the filter up-to-date with whatever is printed
 REM in the actual source, but the space savings are worth it.
-AdafruitGFXFontTrim [ 0-9\.mSTOPMenuER] FreeSansBold18pt7b.h
+AdafruitGFXFontTrim "[ 0-9\.mSTOPMenuERCalibrt]" FreeSansBold18pt7b.h
 
 pause
\ No newline at end of file
diff --git a/src/include/config.h b/src/include/config.h
index 740eef7..fbe2365 100644
--- a/src/include/config.h
+++ b/src/include/config.h
@@ -12,9 +12,9 @@ class Config
       Buttons
 
     */
-    static const uint8_t ButtonPinUp = 3;
-    static const uint8_t ButtonPinMenu = 5;
-    static const uint8_t ButtonPinDown = 6;
+    static const uint8_t ButtonPinTop = 3;
+    static const uint8_t ButtonPinMiddle = 5;
+    static const uint8_t ButtonPinBottom = 6;
 
 
     /*
@@ -38,7 +38,7 @@ class Config
 
 
     static const uint16_t DisplayIdleTime = 10000;
-    static const uint16_t DisplayMoveRefreshRate = 1000;
+    static const uint16_t DisplayRefreshRate = 1000;
 
 
     /*
@@ -59,6 +59,7 @@ class Config
     // How much the measurements can change to still be considered "stable"
     static const uint8_t HeightMeasurementDeltaStable = 10;
     static const uint8_t HeightMeasurementDeltaStableCount = 3;
+    static const uint16_t HeightMeasurementDeltaStableMoveTimeout = 2000;
 
     // How far in advance to stop the motor
     static const uint8_t HeightMeasurementDeltaStop = 0;
@@ -99,10 +100,13 @@ class Config
     #define ColorDarkerGray 0x2965
 
     #define ColorStopRed 0xF9A6
+    #define ColorDarkRed 0x4861
     #define ColorSoftGreen 0x2BE7
     #define ColorSoftBlue 0x3376
     #define ColorDarkBlue 0x0907
 
+    #define ColorOrange 0xF443
+
 
     // Init sequence
     static const uint16_t ColorInitSeqBackground = ColorBlack;
@@ -139,7 +143,8 @@ class Config
 
 
     // Move error / overcurrent
-    static const uint16_t ColorMoveErrorText = ColorStopRed;
+    static const uint16_t ColorErrorBackground = ColorDarkRed;
+    static const uint16_t ColorErrorText = ColorStopRed;
 
 
     // Menu
@@ -149,8 +154,11 @@ class Config
     static const uint16_t ColorMenuSelectedText = ColorWhite;
     static const uint16_t ColorMenuSelectedBackground = ColorDarkBlue;
 
+    static const uint16_t ColorCalibrateBackground = ColorBlack;
     static const uint16_t ColorCalibrateIndicators = ColorWhite;
     static const uint16_t ColorCalibrateValue = ColorWhite;
+    static const uint16_t ColorCalibrateInvalidValue = ColorOrange;
+    static const uint16_t ColorCalibrateDigitMarker = ColorWhite;
 };
 
 #endif
\ No newline at end of file
diff --git a/src/include/metrics.h b/src/include/metrics.h
index fffd229..eec9ba6 100644
--- a/src/include/metrics.h
+++ b/src/include/metrics.h
@@ -2,7 +2,7 @@
 #define __metrics
 
 #include <stdint.h>
-#include "fonts/FreeSansBold18pt7b.trimmed.h"
+//#include "fonts/FreeSansBold18pt7b.trimmed.h"
 
 class Metrics
 {
@@ -14,6 +14,7 @@ class Metrics
 
     */
     static constexpr const GFXfont* SmallFont = nullptr;
+    static const uint8_t SmallFontTextSize = 1;
 
     static const uint16_t SmallFontBaseline = 0;
     static const uint16_t SmallFontHeight = 8;
@@ -23,15 +24,21 @@ class Metrics
     /*
 
       Large font
-        Uses a trimmed version of the FreeSansBold 18pt font.
-
+        Originally used a trimmed version of the FreeSansBold 18pt font.
+        Unfortunately due to program size constraints (I already ordered
+        PCBs based on the ATMega328P) I'll use a scaled version of the
+        built-in font for now.
     */
-    static constexpr const GFXfont* LargeFont = &FreeSansBold18pt7bTrimmed;
+    static constexpr const GFXfont* LargeFont = nullptr;//&FreeSansBold18pt7bTrimmed;
+    static const uint8_t LargeFontTextSize = 3;
 
-    static const uint16_t LargeFontBaseline = 25;
-    static const uint16_t LargeFontHeight = Metrics::LargeFontBaseline;
-    static const uint16_t LargeFontMaxHeight = 42;
+    //static const uint16_t LargeFontBaseline = 25;
+    //static const uint16_t LargeFontHeight = Metrics::LargeFontBaseline;
+    //static const uint16_t LargeFontMaxHeight = 42;
 
+    static const uint16_t LargeFontBaseline = 0;
+    static const uint16_t LargeFontHeight = 24;
+    static const uint16_t LargeFontMaxHeight = 24;
 
 
     /*
diff --git a/src/include/screenids.h b/src/include/screenids.h
new file mode 100644
index 0000000..44942e8
--- /dev/null
+++ b/src/include/screenids.h
@@ -0,0 +1,15 @@
+#ifndef __screenids
+#define __screenids
+
+enum class ScreenId
+{
+  Home,
+  Calibrate,
+  Move,
+  MoveOvercurrent,
+  MoveSensorError,
+  Menu,
+  Manual
+};
+
+#endif
\ No newline at end of file
diff --git a/src/lib/control.cpp b/src/lib/control.cpp
index 516e01d..f92522e 100644
--- a/src/lib/control.cpp
+++ b/src/lib/control.cpp
@@ -1,9 +1,9 @@
 #include "./control.h"
 #include <Wire.h>
-#include "./motor.h"
-#include "./Control.h"
-#include "./settings.h"
 #include "./debug.h"
+#include "./motor.h"
+#include "./state.h"
+#include "./settings.h"
 #include "include/config.h"
 
 
@@ -12,6 +12,7 @@ ControlManager Control = ControlManager();
 
 VL53L0XInitResult ControlManager::init()
 {
+  dln("[ CONTROL  ] Initializing");
   Wire.begin();
 
   this->heightSensor.setTimeout(500);
@@ -26,45 +27,182 @@ VL53L0XInitResult ControlManager::init()
 
 ControlUpdateResult ControlManager::update()
 {
+  // Always read the range if in async reading mode, even if we're not supposed to
+  // be moving anymore, so it clears the status.
+  bool asyncReading = heightSensor.asyncReading();
 
+  uint16_t measurement = asyncReading
+    ? heightSensor.asyncReadRangeSingleMillimeters()
+    : VL53L0XRangeNotReady;
+
+
+  vl("[ CONTROL  ] Update: asyncReading = "); vl(asyncReading); vl(", measurement = "); vl(measurement);
+  vl(", moveDirection = "); vl((uint8_t)this->moveDirection); vl(", stabilizationStart = "); vln(this->stabilizationStart);
+
+
+  bool moving = this->moveDirection != MoveDirection::None && this->stabilizationStart == 0;
+  if (!moving && this->stabilizationStart == 0)
+  {
+    vl("[ CONTROL  ] Idle");
+    return ControlUpdateResult::Idle;
+  }
+
+
+  // Check for over-current
+  // TODO: don't check current every update
+  if (moving && motorIsOvercurrent())
+  {
+    dln("[ CONTROL  ] Overcurrent detected!");
+    this->moveStop();
+
+    return ControlUpdateResult::Overcurrent;
+  }
+
+  // Read sensor
+  if (!asyncReading)
+  {
+    vln("[ CONTROL  ] Starting async read");
+    heightSensor.asyncStartReadRangeSingleMillimeters();
+  }
+
+
+  if (measurement != VL53L0XRangeNotReady)
+  {
+    if (measurement > Config::HeightMeasurementMax)
+      // Treat invalid results as a timeout, so the checks further on are more readable
+      measurement = VL53L0XRangeTimeout;
+
+
+    dl("[ CONTROL  ] Measurement: "); dln(measurement);
+    this->lastMeasurement = measurement != VL53L0XRangeTimeout ? measurement : 0;
+  }
+
+
+  if (measurement != VL53L0XRangeNotReady && measurement != VL53L0XRangeTimeout)
+  {
+    this->currentHeight = measurement;
+    this->lastValidMeasurement = CurrentTime;
+
+    // Check if target has been reached
+    if (moving && this->targetReached())
+    {
+      this->moveStop();
+
+      return ControlUpdateResult::TargetReached;
+    }
+  }
+
+  if (measurement == VL53L0XRangeTimeout && moving)
+  {
+    // While moving, allow incidental invalid results
+    if (CurrentTime - lastValidMeasurement >= Config::HeightMeasurementAbortTimeout)
+    {
+      dln("[ CONTROL  ] Timeout while moving!");
+      this->moveStop();
+
+      return ControlUpdateResult::SensorError;
+    }
+  }
+
+  if (measurement == VL53L0XRangeTimeout && this->moveDirection == MoveDirection::None)
+  {
+    // In pure stabilization (not pre-move), immediately return the sensor error
+    return ControlUpdateResult::SensorError;
+  }
+
+
+
+  if (this->stabilizationStart && this->moveDirection != MoveDirection::None)
+  {
+    // Pre-move stabilization
+    if (this->stabilized())
+    {
+      dln("[ CONTROL  ] Pre-move stabilization successful");
+
+      // Sensor looks good, let's go!
+      motorStart(this->moveDirection == MoveDirection::Up ? MotorDirection::Up : MotorDirection::Down);
+      return ControlUpdateResult::Moving;
+    }
+    else if (CurrentTime - this->stabilizationStart >= Config::HeightMeasurementDeltaStableMoveTimeout)
+    {
+      dln("[ CONTROL  ] Timeout in pre-move stabilization!");
+
+      // Timeout expired, abort the move
+      this->stabilizationStart = 0;
+      this->moveDirection = MoveDirection::None;
+
+      return ControlUpdateResult::SensorError;
+    }
+  }
+
+  return moving ? ControlUpdateResult::Moving : ControlUpdateResult::Stabilizing;
 }
 
 
 void ControlManager::stabilizeStart()
 {
-  this->stabilizing = true;
+  dln("[ CONTROL  ] Starting stabilization");
+
+  this->stabilizationStart = CurrentTime;
+  this->stabilizeTarget = 0;
 }
 
 
 bool ControlManager::stabilized()
 {
-  // TODO: stabilized
+  if (this->stabilizeTarget == 0)
+  {
+    this->stabilizeTarget = this->currentHeight;
+    this->stabilizationLastCheck = this->lastValidMeasurement;
+    return false;
+  }
+
+  // Only count if we have a new measurement
+  if (this->stabilizationLastCheck == this->lastValidMeasurement)
+    return false;
+
+  this->stabilizationLastCheck = this->lastValidMeasurement;
+
+
+  int16_t delta = this->currentHeight - this->stabilizeTarget;
+  if (abs(delta) <= Config::HeightMeasurementDeltaStable)
+  {
+    this->stabilizeCount++;
+
+    if (this->stabilizeCount >= Config::HeightMeasurementDeltaStableCount)
+    {
+      dln("[ CONTROL  ] Stable");
+      this->stabilizationStart = 0;
+      return true;
+    }
+  }
+  else
+  {
+    // If it's this much off, chances are the actual value is somewhere in the middle
+    this->stabilizeTarget += (delta / 2);
+    this->stabilizeCount = 0;
+  }
+
+  return false;
 }
 
 
-void ControlManager::movePrepare(uint16_t height)
+void ControlManager::moveStart(uint16_t height)
 {
+  dl("[ CONTROL  ] Starting move to: "); dln(height);
+
   this->moveTarget = height;
   this->moveDirection = height > this->currentHeight ? MoveDirection::Up : MoveDirection::Down;
-}
 
-
-bool ControlManager::moveStart()
-{
-  // TODO: moveStart - wait for stable / timeout
-
-  this->moveDirection = MoveDirection::None;
+  // Wait for a stable result
   this->stabilizeStart();
-  //this->sensorError = true;
-  return false;
-
-  motorStart(this->moveDirection == MoveDirection::Up ? MotorDirection::Up : MotorDirection::Down);
-  return true;
 }
 
 
 void ControlManager::moveStop()
 {
+  dln("[ CONTROL  ] Stopping move");
+
   motorStop();
   this->moveDirection = MoveDirection::None;
 }
@@ -84,8 +222,52 @@ void ControlManager::snapToPreset()
 }
 
 
+bool ControlManager::targetReached()
+{
+  // Checks depend on the direction, so it returns true immediately if we've overshot the target
+  switch (this->moveDirection)
+  {
+    case MoveDirection::Up:
+      if (this->currentHeight >= this->moveTarget - Config::HeightMeasurementDeltaStop)
+      {
+        // Snap to the target
+        if (this->currentHeight - this->moveTarget <= Config::HeightMeasurementDeltaOnTarget)
+          this->currentHeight = this->moveTarget;
+
+        dln("[ CONTROL  ] Target reached");
+        return true;
+      }
+      break;
+
+    case MoveDirection::Down:
+      if (this->currentHeight <= this->moveTarget + Config::HeightMeasurementDeltaStop)
+      {
+        // Snap to the target
+        if (this->moveTarget - this->currentHeight <= Config::HeightMeasurementDeltaOnTarget)
+          this->currentHeight = this->moveTarget;
+
+        dln("[ CONTROL  ] Target reached");
+        return true;
+      }
+      break;
+
+    default:
+      break;
+  }
+
+  return false;
+}
+
+
 void ControlManager::getDisplayHeight(char* buffer, uint16_t value)
 {
+  if (value == 0)
+  {
+    buffer[0] = '-';
+    buffer[1] = 0;
+    return;
+  }
+
   uint8_t displayValue = (value + Settings.Height.Offset) / 10;
 
   if (displayValue > 99)
@@ -99,148 +281,3 @@ void ControlManager::getDisplayHeight(char* buffer, uint16_t value)
   buffer[4] = 'm';
   buffer[5] = 0;
 }
-
-
-/*
-bool controlCheckTargetReached()
-{
-  dl("controlCheckTargetReached: direction = "); dl((uint8_t)Control.getMoveDirection()); dl(", currentHeight = "); dln(Control.getCurrentHeight());
-
-  switch (Control.getMoveDirection())
-  {
-    case Direction::Up:
-      if (Control.getCurrentHeight() >= Control.getMoveTarget() - Config::HeightMeasurementDeltaStop)
-      {
-        if (Control.getCurrentHeight() - Control.getMoveTarget() <= Config::HeightMeasurementDeltaOnTarget)
-          Control.getCurrentHeight() = Control.getMoveTarget();
-
-        controlStop();
-        return true;
-      }
-      break;
-
-    case Direction::Down:
-      if (Control.getCurrentHeight() <= Control.getMoveTarget() + Config::HeightMeasurementDeltaStop)
-      {
-        if (Control.getMoveTarget() - Control.getCurrentHeight() <= Config::HeightMeasurementDeltaOnTarget)
-          Control.getCurrentHeight() = Control.getMoveTarget();
-
-        controlStop();
-        return true;
-      }
-      break;
-
-    default:
-      break;
-  }
-
-  return false;
-}
-
-
-bool controlCheckOverCurrent()
-{
-  if (motorIsOverCurrent())
-  {
-    dln("controlCheckOverCurrent: overcurrent detected!");
-
-    controlStop();
-    return true;
-  }
-
-  return false;
-}
-
-
-
-
-  if (Control.getMoveDirection() != Direction::None)
-  {
-    if (controlCheckOverCurrent())
-      screenManager.show<MoveOverCurrentScreen>();
-    else
-      updateHeight();
-  }
-  else if (Control.SensorError)
-    updateHeight();
-
-
-
-
-void updateHeight()
-{
-  uint16_t measurement;
-
-  if (heightSensorGetRange(&measurement))
-  {
-    Control.getCurrentHeight() = measurement;
-
-    if (Control.getMoveDirection() != Direction::None)
-    {
-      lastValidMeasurement = CurrentTime;
-
-      if (controlCheckTargetReached())
-        screenManager.show<HomeScreen>();
-    }
-  }
-  else if (Control.getMoveDirection() != Direction::None && CurrentTime - lastValidMeasurement >= Config::HeightMeasurementAbortTimeout)
-  {
-    dln("Out of range timeout!");
-    Control.moveStop();
-
-    screenManager.show<MoveSensorErrorScreen>();
-  }
-}
-
-
-bool heightSensorGetRange(uint16_t* measurement)
-{
-  *measurement = heightSensor.readRangeSingleMillimeters();
-  dl("Range: "); dln(*measurement);
-
-  return *measurement <= Config::HeightMeasurementMax;
-}
-
-
-
-
-
-
-
-
-  uint16_t reference = 0;
-  uint8_t closeCount = 0;
-  uint16_t measurement;
-
-  while (true)
-  {
-    if (heightSensorGetRange(&measurement))
-    {
-      initSequenceDisplayHeight(measurement);
-
-      if (abs(measurement - reference) <= Config::HeightMeasurementDeltaStable)
-        closeCount++;
-      else
-      {
-        reference = measurement;
-        closeCount = 0;
-      }
-    }
-    else
-    {
-      initSequenceDisplayHeight(0);
-
-      reference = 0;
-      closeCount = 0;
-    }
-
-    if (closeCount < Config::HeightMeasurementDeltaStableCount)
-      delay(500);
-    else
-      break;
-  }
-
-  initSequenceSuccess(InitSequenceStep::HeightSensorTest);
-  return reference;
-
-*/
\ No newline at end of file
diff --git a/src/lib/control.h b/src/lib/control.h
index df27946..5b5ff57 100644
--- a/src/lib/control.h
+++ b/src/lib/control.h
@@ -7,19 +7,20 @@
 
 enum class ControlUpdateResult
 {
-  Idle = 0,
-  Moving = 1,
-  TargetReached = 2,
-  SensorError = 3,
-  OverCurrent = 4
+  Idle,
+  Stabilizing,
+  Moving,
+  TargetReached,
+  SensorError,
+  Overcurrent
 };
 
 
 enum class MoveDirection
 {
-  None = 0,
-  Up = 1,
-  Down = 2
+  None,
+  Up,
+  Down
 };
 
 
@@ -34,8 +35,7 @@ class ControlManager
     void stabilizeStart();
     bool stabilized();
 
-    void movePrepare(uint16_t height);
-    bool moveStart();
+    void moveStart(uint16_t height);
     void moveStop();
 
     void snapToPreset();
@@ -48,18 +48,26 @@ class ControlManager
 
 
     uint16_t getCurrentHeight() { return this->currentHeight; }
+    uint16_t getLastMeasurement() { return this->lastMeasurement; }
     MoveDirection getMoveDirection() { return this->moveDirection; }
     uint16_t getMoveTarget() { return this->moveTarget; }
+    bool getIsStabilizing() { return this->stabilizationStart != 0; }
 
   private:
+    bool targetReached();
+
     VL53L0X heightSensor = VL53L0X();
     uint16_t currentHeight;
+    uint16_t lastMeasurement = 0;
+    uint32_t lastValidMeasurement = 0;
 
     MoveDirection moveDirection;
     uint16_t moveTarget;
 
-    bool stabilizing = false;
-    uint32_t lastValidMeasurement = 0;
+    uint32_t stabilizationStart = 0;
+    uint8_t stabilizeCount = 0;
+    uint16_t stabilizeTarget = 0;
+    uint32_t stabilizationLastCheck = 0;
 };
 
 
diff --git a/src/lib/debug.h b/src/lib/debug.h
index 5d76651..b4e19dc 100644
--- a/src/lib/debug.h
+++ b/src/lib/debug.h
@@ -3,12 +3,12 @@
 
 #include <Arduino.h>
 
-#define SerialDebug
+#define DebugLog
+//#define VerboseLog
 
 
-
-#ifdef SerialDebug
-  #define DebugInit() Serial.begin(9600)
+#ifdef DebugLog
+  #define DebugInit() Serial.begin(115200)
   #define dl(value) Serial.print(value)
   #define dln(value) Serial.println(value)
 #else
@@ -18,4 +18,13 @@
 #endif
 
 
+#ifdef VerboseLog
+  #define vl(value) Serial.print(value)
+  #define vln(value) Serial.println(value)
+#else
+  #define vl(value)
+  #define vln(value)
+#endif
+
+
 #endif
\ No newline at end of file
diff --git a/src/lib/motor.cpp b/src/lib/motor.cpp
index 5df1f3d..76e255a 100644
--- a/src/lib/motor.cpp
+++ b/src/lib/motor.cpp
@@ -31,9 +31,9 @@ void motorStop()
 }
 
 
-bool motorIsOverCurrent()
+bool motorIsOvercurrent()
 {
-  // TODO: implement motorIsOverCurrent
+  // TODO: implement motorIsOvercurrent
   return false;
 }
 
diff --git a/src/lib/motor.h b/src/lib/motor.h
index 48b6162..2f44430 100644
--- a/src/lib/motor.h
+++ b/src/lib/motor.h
@@ -5,14 +5,14 @@
 // Low-level functions to control the motor
 enum class MotorDirection
 {
-  Up = 0,
-  Down = 1
+  Up,
+  Down
 };
 
 
 extern void motorInit();
 extern void motorStart(MotorDirection direction);
 extern void motorStop();
-extern bool motorIsOverCurrent();
+extern bool motorIsOvercurrent();
 
 #endif
\ No newline at end of file
diff --git a/src/lib/screen.cpp b/src/lib/screen.cpp
index 1b8e64e..0f52fb8 100644
--- a/src/lib/screen.cpp
+++ b/src/lib/screen.cpp
@@ -3,43 +3,53 @@
 #include "include/metrics.h"
 
 
-Adafruit_GFX* BaseScreen::getDisplay()
-{
-  return this->screenManager->getDisplay();
-}
-
-
 uint16_t BaseScreen::printCentered(const char* text, int16_t y)
 {
-  auto display = this->getDisplay();
-
   int16_t textX;
   int16_t textY;
   uint16_t textW;
   uint16_t textH;
-  display->getTextBounds(text, 0, 0, &textX, &textY, &textW, &textH);
+  this->display->getTextBounds(text, 0, 0, &textX, &textY, &textW, &textH);
 
   textX = (Config::DisplayWidth - textW) / 2;
-  display->setCursor(textX, y);
-  display->print(text);
+  this->display->setCursor(textX, y);
+  this->display->print(text);
 
   return textW;
 }
 
 
+void BaseScreen::drawLargeTextLineCentered(const char* text, int16_t y, uint16_t textColor, uint16_t backgroundColor)
+{
+  this->display->fillRect(0, y, Config::DisplayWidth, Metrics::LargeTextLineHeight, backgroundColor);
+  this->display->setTextColor(textColor);
+  this->printCentered(text, y + Metrics::LargeTextLineYOffset);
+}
+
+
 void BaseScreen::drawArrowLeft(int16_t x, int16_t y, uint16_t color)
 {
-  this->getDisplay()->fillTriangle(
-    x + Metrics::HArrowWidth, y,                           // Top right
-    x, y + (Metrics::HArrowHeight / 2),                    // Middle left
-    x + Metrics::HArrowWidth, y + Metrics::HArrowHeight,   // Bottom right
+  this->display->fillTriangle(
+    x + Metrics::HArrowWidth, y,                          // Top right
+    x, y + (Metrics::HArrowHeight / 2),                   // Middle left
+    x + Metrics::HArrowWidth, y + Metrics::HArrowHeight,  // Bottom right
+    color);
+}
+
+
+void BaseScreen::drawArrowRight(int16_t x, int16_t y, uint16_t color)
+{
+  this->display->fillTriangle(
+    x, y,                                                         // Top left
+    x +  + Metrics::HArrowWidth, y + (Metrics::HArrowHeight / 2), // Middle right
+    x, y + Metrics::HArrowHeight,                                 // Bottom left
     color);
 }
 
 
 void BaseScreen::drawArrowUp(int16_t x, int16_t y, uint16_t color)
 {
-  this->getDisplay()->fillTriangle(
+  this->display->fillTriangle(
     x + (Metrics::VArrowWidth / 2), y,                    // Top middle
     x, y + Metrics::VArrowHeight,                         // Bottom left
     x + Metrics::VArrowWidth, y + Metrics::VArrowHeight,  // Bottom right
@@ -49,7 +59,7 @@ void BaseScreen::drawArrowUp(int16_t x, int16_t y, uint16_t color)
 
 void BaseScreen::drawArrowDown(int16_t x, int16_t y, uint16_t color)
 {
-  this->getDisplay()->fillTriangle(
+  this->display->fillTriangle(
     x, y,                                                       // Top left
     x + Metrics::VArrowWidth, y,                                // Top right
     x + (Metrics::VArrowWidth / 2), y + Metrics::VArrowHeight,  // Bottom middle
diff --git a/src/lib/screen.h b/src/lib/screen.h
index 15f08b7..6ac5962 100644
--- a/src/lib/screen.h
+++ b/src/lib/screen.h
@@ -3,6 +3,7 @@
 
 #include <Adafruit_ST7789.h>
 #include "include/config.h"
+#include "include/screenids.h"
 
 
 class ScreenManager;
@@ -10,18 +11,19 @@ class ScreenManager;
 
 enum class Button
 {
-  Up = 0,
-  Menu = 1,
-  Down = 2
+  Top,
+  Middle,
+  Bottom
 };
 
 
 class BaseScreen
 {
   public:
-    BaseScreen(ScreenManager* screenManager)
+    BaseScreen(ScreenManager* screenManager, Adafruit_GFX* display)
     {
       this->screenManager = screenManager;
+      this->display = display;
     }
 
     virtual ~BaseScreen() {}
@@ -30,18 +32,20 @@ class BaseScreen
     virtual void onButton(Button button) = 0;
     virtual void onTick() = 0;
 
-  protected:
-    ScreenManager* getScreenManager() { return this->screenManager; }
-    Adafruit_GFX* getDisplay();
+    virtual ScreenId screenId() = 0;
 
+  protected:
     uint16_t printCentered(const char* text, int16_t y);
 
+    void drawLargeTextLineCentered(const char* text, int16_t y, uint16_t textColor, uint16_t backgroundColor);
+
     void drawArrowLeft(int16_t x, int16_t y, uint16_t color);
+    void drawArrowRight(int16_t x, int16_t y, uint16_t color);
     void drawArrowUp(int16_t x, int16_t y, uint16_t color);
     void drawArrowDown(int16_t x, int16_t y, uint16_t color);
 
-  private:
     ScreenManager* screenManager;
+    Adafruit_GFX* display;
 };
 
 
@@ -60,7 +64,6 @@ class ScreenManager
     inline void tick() { this->getCurrentScreen()->onTick(); }
 
     inline BaseScreen* getCurrentScreen() { return this->currentScreen; }
-    inline Adafruit_GFX* getDisplay() { return this->display; }
 
 
     template<class T> void show()
@@ -71,17 +74,22 @@ class ScreenManager
         delete this->currentScreen;
       }
 
-      this->currentScreen = new T(this);
+      this->currentScreen = new T(this, this->display);
       this->currentScreen->onShow();
     }
 
+    template<class T> void show(ScreenId onlyIfNotOn)
+    {
+      if (this->currentScreen == nullptr || this->currentScreen->screenId() != onlyIfNotOn)
+        this->show<T>();
+    }
+
 
     void displayOff();
     void displayOn();
 
   private:
     Adafruit_ST7789* display;
-
     BaseScreen* currentScreen = nullptr;
 };
 
diff --git a/src/lib/screen/calibrate.cpp b/src/lib/screen/calibrate.cpp
index 6d286c1..495ada3 100644
--- a/src/lib/screen/calibrate.cpp
+++ b/src/lib/screen/calibrate.cpp
@@ -1,19 +1,205 @@
 #include "./calibrate.h"
+#include "./home.h"
+#include "include/metrics.h"
+#include "lib/settings.h"
 
 
 void CalibrateScreen::onShow()
 {
-  //auto display = this->getDisplay();
+  this->display->setFont(Metrics::LargeFont);
+  this->display->setTextSize(Metrics::LargeFontTextSize);
 
-  // TODO: implement CalibrateScreen
+  this->display->fillScreen(Config::ColorHomeBackground);
+
+  this->drawTitle();
+  this->height[CalibrateStepCurrent] = Control.getCurrentHeight() + Settings.Height.Offset;
+  this->height[CalibrateStepMax] = 1969; // TODO: read min/max from settings
+  this->height[CalibrateStepMin] = 0;
+
+  this->drawArrowUp(Metrics::ArrowMargin, Metrics::LargeTextLineHeight + Metrics::LargeTextLineVArrowYOffset, Config::ColorCalibrateIndicators);
+  this->drawArrowRight(Metrics::ArrowMargin, Metrics::MiddleLargeTextLineY + Metrics::LargeTextLineHArrowYOffset, Config::ColorCalibrateIndicators);
+
+  this->display->setTextColor(Config::ColorCalibrateIndicators);
+  this->display->setCursor(Metrics::ArrowMargin, Config::DisplayHeight - Metrics::LargeTextLineHeight + Metrics::LargeTextLineYOffset);
+  this->display->print("OK");
+
+  this->drawSetHeight();
+}
+
+
+// Credit: https://stackoverflow.com/questions/101439/the-most-efficient-way-to-implement-an-integer-based-power-function-powint-int
+uint16_t ipow(uint16_t base, uint8_t exp)
+{
+    int result = 1;
+    for (;;)
+    {
+        if (exp & 1)
+            result *= base;
+
+        exp >>= 1;
+        if (!exp)
+            break;
+
+        base *= base;
+    }
+
+    return result;
 }
 
 
 void CalibrateScreen::onButton(Button button)
 {
+  switch (button)
+  {
+    case Button::Top:
+    {
+      uint16_t height = this->height[this->step];
+      uint16_t increment = ipow(10, 3 - this->editingDigit);
+      uint16_t modulus = increment * 10;
+
+      uint16_t remainder = height % modulus;
+      uint16_t offset = height - remainder;
+
+      height = offset + ((remainder + increment) % modulus);
+      if (height > 1999)
+        height %= 2000;
+
+      this->height[this->step] = height;
+      this->drawSetHeight();
+      break;
+    }
+
+    case Button::Middle:
+    {
+      this->editingDigit++;
+      if (this->editingDigit > 3)
+        this->editingDigit = 0;
+
+      this->drawSetHeight();
+      break;
+    }
+
+    case Button::Bottom:
+    {
+      if (!this->isValidHeight())
+        return;
+
+      this->step = (CalibrateStep)(this->step + 1);
+      if (this->step == CalibrateStepCount)
+      {
+        // TODO: store new settings
+        this->screenManager->show<HomeScreen>();
+      }
+      else
+      {
+        this->drawTitle();
+        this->drawSetHeight();
+      }
+
+      break;
+    }
+  }
 }
 
 
 void CalibrateScreen::onTick()
 {
+}
+
+
+bool CalibrateScreen::isValidHeight()
+{
+  switch (this->step)
+  {
+    case CalibrateStepCurrent:
+      return this->height[CalibrateStepCurrent] >= Control.getCurrentHeight();
+
+    case CalibrateStepMin:
+      return this->height[CalibrateStepMin] <= this->height[CalibrateStepCurrent];
+
+    case CalibrateStepMax:
+      return
+        this->height[CalibrateStepMax] > this->height[CalibrateStepMin] &&
+        this->height[CalibrateStepMax] >= this->height[CalibrateStepCurrent];
+
+    default:
+      return true;
+  }
+}
+
+
+void CalibrateScreen::drawTitle()
+{
+  switch (this->step)
+  {
+    case CalibrateStepCurrent:
+      this->drawLargeTextLineCentered("Calibrate", 0, Config::ColorMenuHeaderText, Config::ColorMenuHeaderBackground);
+      break;
+
+    case CalibrateStepMin:
+      this->drawLargeTextLineCentered("Minimum", 0, Config::ColorMenuHeaderText, Config::ColorMenuHeaderBackground);
+      break;
+
+    case CalibrateStepMax:
+      this->drawLargeTextLineCentered("Maximum", 0, Config::ColorMenuHeaderText, Config::ColorMenuHeaderBackground);
+      break;
+
+    default:
+      break;
+  }
+}
+
+
+void CalibrateScreen::drawSetHeight()
+{
+  char heightText[7];
+  uint16_t height = this->height[this->step];
+
+  if (height > 999)
+    heightText[0] = '0' + ((height / 1000) % 10);
+  else
+    heightText[0] = '0';
+
+  heightText[1] = '.';
+  heightText[2] = '0' + ((height / 100) % 10);
+  heightText[3] = '0' + ((height / 10) % 10);
+  heightText[4] = '0' + (height % 10);
+  heightText[5] = 'm';
+  heightText[6] = 0;
+
+
+  int16_t textX;
+  int16_t textY;
+  uint16_t textW;
+  uint16_t textH;
+  this->display->getTextBounds(&heightText[0], 0, 0, &textX, &textY, &textW, &textH);
+  textX = (Config::DisplayWidth - textW) / 2;
+
+
+
+  uint8_t editingChar = this->editingDigit;
+
+  // Skip the dot
+  if (editingChar > 0)
+    editingChar++;
+
+
+  if (this->lastTextWidth > 0)
+    this->display->fillRect((Config::DisplayWidth - this->lastTextWidth) / 2, Metrics::MiddleLargeTextLineY, this->lastTextWidth, Metrics::LargeTextLineHeight, Config::ColorCalibrateBackground);
+
+  this->lastTextWidth = textW;
+
+  this->display->setTextColor(this->isValidHeight() ? Config::ColorCalibrateValue : Config::ColorCalibrateInvalidValue);
+  this->display->setCursor(textX, Metrics::MiddleLargeTextLineY + Metrics::LargeTextLineYOffset);
+
+  // Draw each character ourselves so we can keep track of where the line should be drawn
+  for (uint8_t i = 0; i < sizeof(heightText); i++)
+  {
+    int16_t cursorStart = this->display->getCursorX();
+
+    this->display->print(heightText[i]);
+
+    if (i == editingChar)
+      this->display->drawFastHLine(cursorStart, Metrics::MiddleLargeTextLineY + Metrics::LargeTextLineHeight - 1, this->display->getCursorX() - cursorStart, Config::ColorCalibrateDigitMarker);
+  }
 }
\ No newline at end of file
diff --git a/src/lib/screen/calibrate.h b/src/lib/screen/calibrate.h
index 6291a42..6c9aa2e 100644
--- a/src/lib/screen/calibrate.h
+++ b/src/lib/screen/calibrate.h
@@ -1,10 +1,21 @@
 #ifndef __screen_calibrate
 #define __screen_calibrate
 
+#include "include/screenids.h"
 #include "../screen.h"
 #include "../control.h"
 
 
+enum CalibrateStep : uint8_t
+{
+    CalibrateStepCurrent = 0,
+    CalibrateStepMax,
+    CalibrateStepMin,
+
+    CalibrateStepCount
+};
+
+
 /*
  * Calibrate screen
  *   Configures the absolute height (calculates the offset) and
@@ -13,13 +24,24 @@
 class CalibrateScreen : public BaseScreen
 {
   public:
-    CalibrateScreen(ScreenManager* screenManager) : BaseScreen(screenManager) { }
+    CalibrateScreen(ScreenManager* screenManager, Adafruit_GFX* display) : BaseScreen(screenManager, display) { }
 
     void onShow();
     void onButton(Button button);
     void onTick();
 
+    ScreenId screenId() { return ScreenId::Calibrate; };
+
   private:
+    bool isValidHeight();
+    void drawTitle();
+    void drawSetHeight();
+
+    CalibrateStep step = CalibrateStepCurrent;
+
+    uint16_t height[CalibrateStepCount];
+    uint16_t lastTextWidth = 0;
+    uint8_t editingDigit = 0;
 };
 
 #endif
diff --git a/src/lib/screen/home.cpp b/src/lib/screen/home.cpp
index 2d87247..d55ede0 100644
--- a/src/lib/screen/home.cpp
+++ b/src/lib/screen/home.cpp
@@ -1,5 +1,6 @@
 #include "./home.h"
 #include "./move.h"
+#include "./move-sensorerror.h"
 #include "include/config.h"
 #include "include/metrics.h"
 #include "lib/settings.h"
@@ -13,10 +14,10 @@ void HomeScreen::onShow()
 {
   this->showTime = CurrentTime;
 
-  auto display = this->getDisplay();
+  this->display->setFont(Metrics::LargeFont);
+  this->display->setTextSize(Metrics::LargeFontTextSize);
 
-  display->setFont(Metrics::LargeFont);
-  display->fillScreen(Config::ColorHomeBackground);
+  this->display->fillScreen(Config::ColorHomeBackground);
 
   this->drawPreset1();
   this->drawMenu();
@@ -30,33 +31,35 @@ void HomeScreen::onButton(Button button)
 {
   if (this->idle)
   {
-    this->getScreenManager()->displayOn();
+    this->screenManager->displayOn();
     this->idle = false;
     this->showTime = CurrentTime;
 
     // Preset buttons activate immediately
-    if (button == Button::Menu)
+    if (button == Button::Middle)
       return;
   }
 
+  uint16_t targetHeight;
+
   switch (button)
   {
-    case Button::Up:
-      Control.movePrepare(Settings.Height.Preset[0]);
-      this->getScreenManager()->show<MoveScreen>();
-      Control.moveStart();
+    case Button::Top:
+      targetHeight = Settings.Height.Preset[0];
       break;
 
-    case Button::Down:
-      Control.movePrepare(Settings.Height.Preset[1]);
-      this->getScreenManager()->show<MoveScreen>();
-      Control.moveStart();
+    case Button::Bottom:
+      targetHeight = Settings.Height.Preset[1];
       break;
 
-    case Button::Menu:
-      this->getScreenManager()->show<MenuScreen>();
-      break;
+    case Button::Middle:
+      this->screenManager->show<MenuScreen>();
+      return;
   }
+
+
+  Control.moveStart(targetHeight);
+  this->screenManager->show<MoveScreen>();
 }
 
 
@@ -64,7 +67,7 @@ void HomeScreen::onTick()
 {
   if (!this->idle && CurrentTime - this->showTime >= Config::DisplayIdleTime)
   {
-    this->getScreenManager()->displayOff();
+    this->screenManager->displayOff();
     this->idle = true;
   }
 }
@@ -84,13 +87,12 @@ void HomeScreen::drawPreset2()
 
 void HomeScreen::drawNonPresetHeight()
 {
-  auto display = this->getDisplay();
   auto y = Metrics::LargeTextLineHeight;
 
   if (Control.getCurrentHeight() != Settings.Height.Preset[0] &&
       Control.getCurrentHeight() != Settings.Height.Preset[1])
   {
-    display->setTextColor(Config::ColorNonPresetText);
+    this->display->setTextColor(Config::ColorNonPresetText);
     this->drawHeight(y, Control.getCurrentHeight());
   }
 }
@@ -98,7 +100,6 @@ void HomeScreen::drawNonPresetHeight()
 
 void HomeScreen::drawPreset(int16_t y, uint16_t value)
 {
-  auto display = this->getDisplay();
   uint16_t textColor;
   uint16_t backgroundColor;
   uint16_t arrowColor;
@@ -117,12 +118,12 @@ void HomeScreen::drawPreset(int16_t y, uint16_t value)
     arrowColor = Config::ColorPresetArrow;
   }
 
-  display->fillRect(0, y, Config::DisplayWidth, Metrics::LargeTextLineHeight, backgroundColor);
+  this->display->fillRect(0, y, Config::DisplayWidth, Metrics::LargeTextLineHeight, backgroundColor);
 
   if (arrowColor)
     this->drawArrowLeft(Metrics::ArrowMargin, y + Metrics::LargeTextLineHArrowYOffset, arrowColor);
 
-  display->setTextColor(textColor);
+  this->display->setTextColor(textColor);
   this->drawHeight(y, value);
 }
 
@@ -138,10 +139,8 @@ void HomeScreen::drawHeight(int16_t y, uint16_t value)
 
 void HomeScreen::drawMenu()
 {
-  auto display = this->getDisplay();
-
   this->drawArrowLeft(Metrics::ArrowMargin, Metrics::MiddleLargeTextLineY, Config::ColorHomeMenuArrow);
 
-  display->setTextColor(Config::ColorHomeMenuText);
+  this->display->setTextColor(Config::ColorHomeMenuText);
   this->printCentered("Menu", Metrics::MiddleLargeTextLineY + Metrics::LargeTextLineYOffset);
 }
diff --git a/src/lib/screen/home.h b/src/lib/screen/home.h
index 253a504..e06ceab 100644
--- a/src/lib/screen/home.h
+++ b/src/lib/screen/home.h
@@ -1,6 +1,7 @@
 #ifndef __screen_home
 #define __screen_home
 
+#include "include/screenids.h"
 #include "../screen.h"
 #include "../Control.h"
 
@@ -12,12 +13,14 @@
 class HomeScreen : public BaseScreen
 {
   public:
-    HomeScreen(ScreenManager* screenManager) : BaseScreen(screenManager) { }
+    HomeScreen(ScreenManager* screenManager, Adafruit_GFX* display) : BaseScreen(screenManager, display) { }
 
     void onShow();
     void onButton(Button button);
     void onTick();
 
+    ScreenId screenId() { return ScreenId::Home; };
+
   private:
     uint32_t showTime = 0;
     bool idle = false;
diff --git a/src/lib/screen/manual.cpp b/src/lib/screen/manual.cpp
index 18bd6b3..abf5366 100644
--- a/src/lib/screen/manual.cpp
+++ b/src/lib/screen/manual.cpp
@@ -3,8 +3,6 @@
 
 void ManualScreen::onShow()
 {
-  //auto display = this->getDisplay();
-
   // TODO: implement ManualScreen
 }
 
diff --git a/src/lib/screen/manual.h b/src/lib/screen/manual.h
index 8efa894..627beed 100644
--- a/src/lib/screen/manual.h
+++ b/src/lib/screen/manual.h
@@ -1,6 +1,7 @@
 #ifndef __screen_manual
 #define __screen_manual
 
+#include "include/screenids.h"
 #include "../screen.h"
 #include "../Control.h"
 
@@ -12,12 +13,14 @@
 class ManualScreen : public BaseScreen
 {
   public:
-    ManualScreen(ScreenManager* screenManager) : BaseScreen(screenManager) { }
+    ManualScreen(ScreenManager* screenManager, Adafruit_GFX* display) : BaseScreen(screenManager, display) { }
 
     void onShow();
     void onButton(Button button);
     void onTick();
 
+    ScreenId screenId() { return ScreenId::Manual; };
+
   private:
 };
 
diff --git a/src/lib/screen/menu.cpp b/src/lib/screen/menu.cpp
index f54e86f..ebed8fc 100644
--- a/src/lib/screen/menu.cpp
+++ b/src/lib/screen/menu.cpp
@@ -3,8 +3,6 @@
 
 void MenuScreen::onShow()
 {
-  //auto display = this->getDisplay();
-
   // TODO: implement MenuScreen
 }
 
diff --git a/src/lib/screen/menu.h b/src/lib/screen/menu.h
index 454c4ec..96950ec 100644
--- a/src/lib/screen/menu.h
+++ b/src/lib/screen/menu.h
@@ -1,6 +1,7 @@
 #ifndef __screen_menu
 #define __screen_menu
 
+#include "include/screenids.h"
 #include "../screen.h"
 #include "../Control.h"
 
@@ -13,12 +14,14 @@
 class MenuScreen : public BaseScreen
 {
   public:
-    MenuScreen(ScreenManager* screenManager) : BaseScreen(screenManager) { }
+    MenuScreen(ScreenManager* screenManager, Adafruit_GFX* display) : BaseScreen(screenManager, display) { }
 
     void onShow();
     void onButton(Button button);
     void onTick();
 
+    ScreenId screenId() { return ScreenId::Menu; };
+
   private:
 };
 
diff --git a/src/lib/screen/move-overcurrent.cpp b/src/lib/screen/move-overcurrent.cpp
index 26d3f59..d71b4f7 100644
--- a/src/lib/screen/move-overcurrent.cpp
+++ b/src/lib/screen/move-overcurrent.cpp
@@ -1,19 +1,17 @@
 #include "./move-overcurrent.h"
 
 
-void MoveOverCurrentScreen::onShow()
+void MoveOvercurrentScreen::onShow()
 {
-  //auto display = this->getDisplay();
-
-  // TODO: implement MoveOverCurrentScreen
+  // TODO: implement MoveOvercurrentScreen
 }
 
 
-void MoveOverCurrentScreen::onButton(Button button)
+void MoveOvercurrentScreen::onButton(Button button)
 {
 }
 
 
-void MoveOverCurrentScreen::onTick()
+void MoveOvercurrentScreen::onTick()
 {
 }
\ No newline at end of file
diff --git a/src/lib/screen/move-overcurrent.h b/src/lib/screen/move-overcurrent.h
index b9f4242..0264160 100644
--- a/src/lib/screen/move-overcurrent.h
+++ b/src/lib/screen/move-overcurrent.h
@@ -1,6 +1,7 @@
 #ifndef __screen_move_overcurrent
 #define __screen_move_overcurrent
 
+#include "include/screenids.h"
 #include "../screen.h"
 #include "../Control.h"
 
@@ -9,15 +10,17 @@
  * Move overcurrent screen
  *   Displays a warning that the motor driver has reached the set maximum current.
  */
-class MoveOverCurrentScreen : public BaseScreen
+class MoveOvercurrentScreen : public BaseScreen
 {
   public:
-    MoveOverCurrentScreen(ScreenManager* screenManager) : BaseScreen(screenManager) { }
+    MoveOvercurrentScreen(ScreenManager* screenManager, Adafruit_GFX* display) : BaseScreen(screenManager, display) { }
 
     void onShow();
     void onButton(Button button);
     void onTick();
 
+    ScreenId screenId() { return ScreenId::MoveOvercurrent; };
+
   private:
 };
 
diff --git a/src/lib/screen/move-sensorerror.cpp b/src/lib/screen/move-sensorerror.cpp
index 94d4d39..7eaa9af 100644
--- a/src/lib/screen/move-sensorerror.cpp
+++ b/src/lib/screen/move-sensorerror.cpp
@@ -1,4 +1,5 @@
 #include "./move-sensorerror.h"
+#include "./home.h"
 #include "include/config.h"
 #include "include/metrics.h"
 #include "lib/control.h"
@@ -7,15 +8,21 @@
 
 void MoveSensorErrorScreen::onShow()
 {
-  auto display = this->getDisplay();
   auto y = Metrics::LargeTextLineHeight + Metrics::LargeTextLineYOffset;
 
-  display->setFont(Metrics::LargeFont);
-  display->setTextColor(Config::ColorMoveErrorText);
+  this->display->fillScreen(Config::ColorErrorBackground);
+
+
+  this->display->setFont(Metrics::LargeFont);
+  this->display->setTextSize(Metrics::LargeFontTextSize);
+
+  this->display->setTextColor(Config::ColorErrorText);
   this->printCentered("ERROR", y);
   y += Metrics::LargeTextLineHeight;
 
-  display->setFont(Metrics::SmallFont);
+  this->display->setFont(Metrics::SmallFont);
+  this->display->setTextSize(Metrics::SmallFontTextSize);
+
   this->printCentered("height sensor failed", y);
   y += Metrics::SmallTextLineHeight;
 
@@ -24,8 +31,14 @@ void MoveSensorErrorScreen::onShow()
 
   this->currentHeightY = y;
 
+
+  this->display->setFont(Metrics::LargeFont);
+  this->display->setTextSize(Metrics::LargeFontTextSize);
+
   this->lastRefresh = CurrentTime;
-  this->drawCurrentHeight();
+  this->drawLastMeasurement();
+
+  Control.stabilizeStart();
 }
 
 
@@ -36,19 +49,28 @@ void MoveSensorErrorScreen::onButton(Button button)
 
 void MoveSensorErrorScreen::onTick()
 {
+  if (Control.stabilized())
+  {
+    this->screenManager->show<HomeScreen>();
+    return;
+  }
+
+  if (CurrentTime - this->lastRefresh >= Config::DisplayRefreshRate)
+  {
+    this->drawLastMeasurement();
+    this->lastRefresh = CurrentTime;
+  }
 }
 
 
-void MoveSensorErrorScreen::drawCurrentHeight()
+void MoveSensorErrorScreen::drawLastMeasurement()
 {
-  auto display = this->getDisplay();
-
   char currentHeightText[6];
-  Control.getDisplayHeight(&currentHeightText[0], Control.getCurrentHeight());
+  Control.getDisplayHeight(&currentHeightText[0], Control.getLastMeasurement());
 
   if (this->lastTextWidth > 0)
-    display->fillRect((Config::DisplayWidth - this->lastTextWidth) / 2, this->currentHeightY, this->lastTextWidth, Metrics::LargeTextLineHeight, Config::ColorMoveBackground);
+    this->display->fillRect((Config::DisplayWidth - this->lastTextWidth) / 2, this->currentHeightY, this->lastTextWidth, Metrics::LargeTextLineHeight, Config::ColorErrorBackground);
 
-  display->setTextColor(Config::ColorMoveTarget);
+  this->display->setTextColor(Config::ColorMoveTarget);
   this->lastTextWidth = this->printCentered(&currentHeightText[0], this->currentHeightY + Metrics::LargeFontBaseline);
 }
\ No newline at end of file
diff --git a/src/lib/screen/move-sensorerror.h b/src/lib/screen/move-sensorerror.h
index 4ef6311..8531bfa 100644
--- a/src/lib/screen/move-sensorerror.h
+++ b/src/lib/screen/move-sensorerror.h
@@ -1,6 +1,7 @@
 #ifndef __screen_move_sensorerror
 #define __screen_move_sensorerror
 
+#include "include/screenids.h"
 #include "../screen.h"
 #include "../Control.h"
 
@@ -13,18 +14,20 @@
 class MoveSensorErrorScreen : public BaseScreen
 {
   public:
-    MoveSensorErrorScreen(ScreenManager* screenManager) : BaseScreen(screenManager) { }
+    MoveSensorErrorScreen(ScreenManager* screenManager, Adafruit_GFX* display) : BaseScreen(screenManager, display) { }
 
     void onShow();
     void onButton(Button button);
     void onTick();
 
+    ScreenId screenId() { return ScreenId::MoveSensorError; };
+
   private:
     uint32_t lastRefresh;
     uint8_t currentHeightY;
     uint16_t lastTextWidth = 0;
 
-    void drawCurrentHeight();
+    void drawLastMeasurement();
 };
 
 #endif
diff --git a/src/lib/screen/move.cpp b/src/lib/screen/move.cpp
index 3f62c5f..9fb8440 100644
--- a/src/lib/screen/move.cpp
+++ b/src/lib/screen/move.cpp
@@ -9,25 +9,28 @@
 
 void MoveScreen::onShow()
 {
-  auto display = this->getDisplay();
   auto startY = Metrics::LargeTextLineHeight;
   auto arrowY = startY + Metrics::LargeTextLineHeight + Metrics::LargeTextLineVArrowYOffset;
   auto arrowX = (Config::DisplayWidth - Metrics::VArrowWidth) / 2;
   auto stopY = Config::DisplayHeight - Metrics::LargeTextLineHeight;
 
-  display->fillScreen(Config::ColorMoveBackground);
+  this->display->fillScreen(Config::ColorMoveBackground);
 
   // Stop
-  display->setFont(Metrics::SmallFont);
-  display->setTextColor(Config::ColorMoveStop);
+  this->display->setFont(Metrics::SmallFont);
+  this->display->setTextSize(Metrics::SmallFontTextSize);
+
+  this->display->setTextColor(Config::ColorMoveStop);
   this->printCentered("Press any button to", stopY - Metrics::SmallTextLineHeight);
 
-  display->setFont(Metrics::LargeFont);
+  this->display->setFont(Metrics::LargeFont);
+  this->display->setTextSize(Metrics::LargeFontTextSize);
+
   this->printCentered("STOP", stopY + Metrics::LargeTextLineYOffset);
 
   char targetHeightText[6];
   Control.getDisplayHeight(&targetHeightText[0], Control.getMoveTarget());
-  display->setTextColor(Config::ColorMoveCurrent);
+  this->display->setTextColor(Config::ColorMoveCurrent);
 
   // Target and arrow
   if (Control.getMoveDirection() == MoveDirection::Up)
@@ -53,32 +56,37 @@ void MoveScreen::onShow()
 void MoveScreen::onButton(Button button)
 {
   Control.moveStop();
-  this->getScreenManager()->show<HomeScreen>();
+  this->screenManager->show<HomeScreen>();
 }
 
 
 void MoveScreen::onTick()
 {
+  bool isStabilizing = Control.getIsStabilizing();
+
   // Don't update every tick, monitoring the current height is more
   // important and the flicker would be unpleasant as well.
-  if (CurrentTime - this->lastRefresh >= Config::DisplayMoveRefreshRate)
+  if (this->lastIsStabilizing != isStabilizing || CurrentTime - this->lastRefresh >= Config::DisplayRefreshRate)
   {
     this->drawCurrentHeight();
     this->lastRefresh = CurrentTime;
+    this->lastIsStabilizing = isStabilizing;
   }
 }
 
 
 void MoveScreen::drawCurrentHeight()
 {
-  auto display = this->getDisplay();
-
   char currentHeightText[6];
-  Control.getDisplayHeight(&currentHeightText[0], Control.getCurrentHeight());
+
+  if (Control.getIsStabilizing())
+    Control.getDisplayHeight(&currentHeightText[0], 0);
+  else
+    Control.getDisplayHeight(&currentHeightText[0], Control.getCurrentHeight());
 
   if (this->lastTextWidth > 0)
-    display->fillRect((Config::DisplayWidth - this->lastTextWidth) / 2, this->currentHeightY, this->lastTextWidth, Metrics::LargeTextLineHeight, Config::ColorMoveBackground);
+    this->display->fillRect((Config::DisplayWidth - this->lastTextWidth) / 2, this->currentHeightY, this->lastTextWidth, Metrics::LargeTextLineHeight, Config::ColorMoveBackground);
 
-  display->setTextColor(Config::ColorMoveTarget);
-  this->lastTextWidth = this->printCentered(&currentHeightText[0], this->currentHeightY + Metrics::LargeFontBaseline);
+  this->display->setTextColor(Config::ColorMoveTarget);
+  this->lastTextWidth = this->printCentered(&currentHeightText[0], this->currentHeightY + Metrics::LargeTextLineYOffset);
 }
\ No newline at end of file
diff --git a/src/lib/screen/move.h b/src/lib/screen/move.h
index 5acc83c..ba4f15b 100644
--- a/src/lib/screen/move.h
+++ b/src/lib/screen/move.h
@@ -1,6 +1,7 @@
 #ifndef __screen_move
 #define __screen_move
 
+#include "include/screenids.h"
 #include "../screen.h"
 #include "../Control.h"
 
@@ -12,16 +13,19 @@
 class MoveScreen : public BaseScreen
 {
   public:
-    MoveScreen(ScreenManager* screenManager) : BaseScreen(screenManager) { }
+    MoveScreen(ScreenManager* screenManager, Adafruit_GFX* display) : BaseScreen(screenManager, display) { }
 
     void onShow();
     void onButton(Button button);
     void onTick();
 
+    ScreenId screenId() { return ScreenId::Move; };
+
   private:
     uint32_t lastRefresh;
     uint8_t currentHeightY;
     uint16_t lastTextWidth = 0;
+    bool lastIsStabilizing = true;
 
     void drawCurrentHeight();
 };
diff --git a/src/lib/settings.h b/src/lib/settings.h
index 0715721..0318642 100644
--- a/src/lib/settings.h
+++ b/src/lib/settings.h
@@ -6,7 +6,9 @@
 
 struct SettingsHeights
 {
-  uint8_t Offset;
+  uint16_t Offset;
+  uint16_t Minimum;
+  uint16_t Maximum;
   uint16_t Preset[2];
 };
 
diff --git a/src/lib/vl53l0x.cpp b/src/lib/vl53l0x.cpp
index 3aa438a..25f89cc 100644
--- a/src/lib/vl53l0x.cpp
+++ b/src/lib/vl53l0x.cpp
@@ -1,5 +1,6 @@
 // Slightly modified version of https://github.com/pololu/vl53l0x-arduino
-// which returns an error code if initialization fails.
+// which returns an error code if initialization fails, and allows for
+// asynchronous readings.
 
 // Most of the functionality of this library is based on the VL53L0X API
 // provided by ST (STSW-IMG005), and some of the explanatory comments are quoted
@@ -866,6 +867,89 @@ uint16_t VL53L0X::readRangeSingleMillimeters(void)
   return readRangeContinuousMillimeters();
 }
 
+
+// Asynchronous reading - call asyncStartReadSingle to begin a measurement,
+// then call asyncReadSingle in the loop to determine if a measurement is
+// available. This will return VL53L0XRangeNotReady if not ready, or VL53L0XRangeTimeout if a timeout occured.
+void VL53L0X::asyncStartReadRangeSingleMillimeters()
+{
+  if (asyncReading())
+    return;
+
+  async_state = VL53L0XAsyncStateWaitingForStart;
+  writeReg(0x80, 0x01);
+  writeReg(0xFF, 0x01);
+  writeReg(0x00, 0x00);
+  writeReg(0x91, stop_variable);
+  writeReg(0x00, 0x01);
+  writeReg(0xFF, 0x00);
+  writeReg(0x80, 0x00);
+
+  writeReg(SYSRANGE_START, 0x01);
+
+  // "Wait until start bit has been cleared"
+  startTimeout();
+}
+
+
+uint16_t VL53L0X::asyncReadRangeSingleMillimeters()
+{
+  if (async_state == VL53L0XAsyncStateWaitingForStart)
+  {
+    if (readReg(SYSRANGE_START) & 0x01)
+    {
+      if (checkTimeoutExpired())
+      {
+        did_timeout = true;
+        async_state = VL53L0XAsyncStateNotStarted;
+        return VL53L0XRangeTimeout;
+      }
+
+      return VL53L0XRangeNotReady;
+    }
+    else
+    {
+      startTimeout();
+      async_state = VL53L0XAsyncStateWaitingForStatus;
+    }
+  }
+
+  if (async_state == VL53L0XAsyncStateWaitingForStatus)
+  {
+    if ((readReg(RESULT_INTERRUPT_STATUS) & 0x07) == 0)
+    {
+      if (checkTimeoutExpired())
+      {
+        did_timeout = true;
+        async_state = VL53L0XAsyncStateNotStarted;
+        return VL53L0XRangeTimeout;
+      }
+
+      return VL53L0XRangeNotReady;
+    }
+    else
+    {
+      // assumptions: Linearity Corrective Gain is 1000 (default);
+      // fractional ranging is not enabled
+      uint16_t range = readReg16Bit(RESULT_RANGE_STATUS + 10);
+      writeReg(SYSTEM_INTERRUPT_CLEAR, 0x01);
+
+      async_state = VL53L0XAsyncStateNotStarted;
+      return range;
+    }
+  }
+
+  return VL53L0XRangeNotStarted;
+}
+
+
+// Indicates whether an async read has started, but not yet finished by
+// calling asyncReadRangleSingleMillimeters.
+bool VL53L0X::asyncReading()
+{
+  return (async_state != VL53L0XAsyncStateNotStarted);
+}
+
 // Did a timeout occur in one of the read functions since the last call to
 // timeoutOccurred()?
 bool VL53L0X::timeoutOccurred()
diff --git a/src/lib/vl53l0x.h b/src/lib/vl53l0x.h
index 57483de..c752775 100644
--- a/src/lib/vl53l0x.h
+++ b/src/lib/vl53l0x.h
@@ -9,14 +9,23 @@
 
 enum class VL53L0XInitResult
 {
-  Success = 0,
-  InvalidIdentification = 1,
-  GetSpadInfoFailed = 2,
-  VHVCalibrationFailed = 3,
-  PhaseCalibrationFailed = 4,
+  Success,
+  InvalidIdentification,
+  GetSpadInfoFailed,
+  VHVCalibrationFailed,
+  PhaseCalibrationFailed,
 };
 
 
+static const uint16_t VL53L0XRangeNotStarted = 65533;
+static const uint16_t VL53L0XRangeNotReady = 65534;
+static const uint16_t VL53L0XRangeTimeout = 65535;
+
+static const uint8_t VL53L0XAsyncStateNotStarted = 0;
+static const uint8_t VL53L0XAsyncStateWaitingForStart = 1;
+static const uint8_t VL53L0XAsyncStateWaitingForStatus = 2;
+
+
 class VL53L0X
 {
   public:
@@ -141,6 +150,10 @@ class VL53L0X
     uint16_t readRangeContinuousMillimeters(void);
     uint16_t readRangeSingleMillimeters(void);
 
+    void asyncStartReadRangeSingleMillimeters();
+    uint16_t asyncReadRangeSingleMillimeters();
+    bool asyncReading();
+
     inline void setTimeout(uint16_t timeout) { io_timeout = timeout; }
     inline uint16_t getTimeout(void) { return io_timeout; }
     bool timeoutOccurred(void);
@@ -171,6 +184,8 @@ class VL53L0X
     uint8_t stop_variable; // read by init and used when starting measurement; is StopVariable field of VL53L0X_DevData_t structure in API
     uint32_t measurement_timing_budget_us;
 
+    uint8_t async_state = VL53L0XAsyncStateNotStarted;
+
     bool getSpadInfo(uint8_t * count, bool * type_is_aperture);
 
     void getSequenceStepEnables(SequenceStepEnables * enables);
diff --git a/src/main.cpp b/src/main.cpp
index ed69d48..505e9d8 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -4,6 +4,7 @@
 #include <Bounce2.h>
 
 #include "./include/config.h"
+#include "./include/screenids.h"
 #include "./lib/debug.h"
 #include "./lib/settings.h"
 #include "./lib/screen.h"
@@ -21,8 +22,8 @@
 enum class InitSequenceStep
 {
   EEPROM = 0,
-  HeightSensorInit = 1,
-  HeightSensorTest = 2,
+  HeightSensorInit,
+  HeightSensorTest,
 
   Last = HeightSensorTest
 };
@@ -30,7 +31,7 @@ enum class InitSequenceStep
 
 // Forward declarations
 inline void setupHeightSensor();
-inline uint16_t testHeightSensor();
+inline void testHeightSensor();
 
 void initSequenceStart();
 void initSequenceSuccess(InitSequenceStep step);
@@ -45,6 +46,7 @@ auto screenManager = ScreenManager(&display);
 Bounce buttons[3];
 
 
+
 /*
 
   Setup
@@ -53,11 +55,11 @@ Bounce buttons[3];
 void setup()
 {
   DebugInit();
-  dln("Debug log active");
+  dln("[ MAIN     ] Debug log started");
 
-  buttons[0].attach(Config::ButtonPinUp, INPUT_PULLUP);
-  buttons[1].attach(Config::ButtonPinMenu, INPUT_PULLUP);
-  buttons[2].attach(Config::ButtonPinDown, INPUT_PULLUP);
+  buttons[0].attach(Config::ButtonPinTop, INPUT_PULLUP);
+  buttons[1].attach(Config::ButtonPinMiddle, INPUT_PULLUP);
+  buttons[2].attach(Config::ButtonPinBottom, INPUT_PULLUP);
 
   display.init(Config::DisplayWidth, Config::DisplayHeight, SPI_MODE3);
   display.setRotation(Config::DisplayRotation);
@@ -73,8 +75,10 @@ void setup()
 
 
   // Initialize VL53L0X sensor
+  CurrentTime = millis();
+
   setupHeightSensor();
-  auto currentHeight = testHeightSensor();
+  testHeightSensor();
 
   initSequenceEnd();
 
@@ -84,7 +88,6 @@ void setup()
 
   if (initialized)
   {
-//    Control.getCurrentHeight() = currentHeight;
     Control.snapToPreset();
 
     screenManager.show<HomeScreen>();
@@ -135,8 +138,26 @@ inline void setupHeightSensor()
 
 
 
-inline uint16_t testHeightSensor()
+inline void testHeightSensor()
 {
+  Control.stabilizeStart();
+
+  while (true)
+  {
+    if (Control.update() != ControlUpdateResult::SensorError)
+    {
+      initSequenceDisplayHeight(Control.getCurrentHeight());
+
+      if (Control.stabilized())
+        break;
+    }
+    else
+      initSequenceDisplayHeight(0);
+
+    CurrentTime = millis();
+  }
+
+  initSequenceSuccess(InitSequenceStep::HeightSensorTest);
 }
 
 
@@ -158,11 +179,11 @@ void loop()
       break;
 
     case ControlUpdateResult::SensorError:
-      screenManager.show<MoveSensorErrorScreen>();
+      screenManager.show<MoveSensorErrorScreen>(ScreenId::MoveSensorError);
       break;
 
-    case ControlUpdateResult::OverCurrent:
-      screenManager.show<MoveOverCurrentScreen>();
+    case ControlUpdateResult::Overcurrent:
+      screenManager.show<MoveOvercurrentScreen>(ScreenId::MoveOvercurrent);
       break;
 
     default:
@@ -174,21 +195,28 @@ void loop()
   buttons[1].update();
   buttons[2].update();
 
+  if (buttons[0].fell())
+  {
+    dln("[ MAIN     ] Button pressed: Top");
+    screenManager.button(Button::Top);
+  }
 
-  if (buttons[0].rose())
-    screenManager.button(Button::Up);
+  if (buttons[1].fell())
+  {
+    dln("[ MAIN     ] Button pressed: Middle");
+    screenManager.button(Button::Middle);
+  }
 
-  if (buttons[1].rose())
-    screenManager.button(Button::Menu);
-
-  if (buttons[2].rose())
-    screenManager.button(Button::Down);
+  if (buttons[2].fell())
+  {
+    dln("[ MAIN     ] Button pressed: Bottom");
+    screenManager.button(Button::Bottom);
+  }
 
   screenManager.tick();
 }
 
 
-
 /*
 
   Helper functions for the status display during the initialization sequence