diff --git a/doc/pinout.md b/doc/pinout.md
index 56a5394..fa7df40 100644
--- a/doc/pinout.md
+++ b/doc/pinout.md
@@ -21,7 +21,7 @@ https://www.pololu.com/product/2995
 | PWM        | Motor PWM                  | 14          | 23 - PC0       |
 | DIR        | Direction                  | 15          | 24 - PC1       |
 | SLP        | Sleep (active low)         | 16          | 25 - PC2       |
-| CS         | Current sensing            | 17          | 26 - PC3       |
+| CS         | Current sensing            | 17 / A3     | 26 - PC3       |
 
 CS has a voltage of 50mV + 20mV per A.
 
diff --git a/src/fonts/trim.bat b/src/fonts/trim.bat
index 4487c57..851144f 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\.mSTOPMenu] FreeSansBold18pt7b.h
+AdafruitGFXFontTrim [ 0-9\.mSTOPMenursaybto] FreeSansBold18pt7b.h
 
 pause
\ No newline at end of file
diff --git a/src/include/config.h b/src/include/config.h
index d15d978..79e6b83 100644
--- a/src/include/config.h
+++ b/src/include/config.h
@@ -1,27 +1,7 @@
 #ifndef __config
 #define __config
 
-
-// The default color set provided by Adafruit
-#define BLACK    0x0000
-#define BLUE     0x001F
-#define RED      0xF800
-#define GREEN    0x07E0
-#define CYAN     0x07FF
-#define MAGENTA  0xF81F
-#define YELLOW   0xFFE0
-#define WHITE    0xFFFF
-
-
-    /*
-
-    TODO make constants for these values once they are used
-
-    #define STOP_RED 0xF9A6
-    #define MOVING_ORANGE 0xF443
-    #define MOVING_ORANGE_LIGHT 0xFD8C
-    */
-
+#include <stdint.h>
 
 
 class Config
@@ -32,9 +12,9 @@ class Config
       Buttons
 
     */
-    static const uint8_t ButtonPortUp = 3;
-    static const uint8_t ButtonPortMenu = 5;
-    static const uint8_t ButtonPortDown = 6;
+    static const uint8_t ButtonPinUp = 3;
+    static const uint8_t ButtonPinMenu = 5;
+    static const uint8_t ButtonPinDown = 6;
 
 
     /*
@@ -51,10 +31,10 @@ class Config
 
 
     // Control pins. Data is sent over the hardware SPI pins.
-    static const uint8_t DisplayPortCS = 10;
-    static const uint8_t DisplayPortRST = 9;
-    static const uint8_t DisplayPortDC = 8;
-    static const uint8_t DisplayPortBL = 7;
+    static const uint8_t DisplayPinCS = 10;
+    static const uint8_t DisplayPinRST = 9;
+    static const uint8_t DisplayPinDC = 8;
+    static const uint8_t DisplayPinBL = 7;
 
 
     static const uint16_t DisplayIdleTime = 10000;
@@ -85,43 +65,70 @@ class Config
     static const uint8_t HeightMeasurementDeltaOnTarget = 15;
 
     // How long we allow invalid measurements until the move operation is aborted
-    static const uint8_t HeightMeasurementAbortTimeout = 100;
+    static const uint16_t HeightMeasurementAbortTimeout = 500;
 
 
 
+    /*
+
+      Motor driver
+        Settings for a Polulu G2 driver board.
+
+    */
+    static const uint8_t MotorPinPWM = 14;
+    static const uint8_t MotorPinDirection = 15;
+    static const uint8_t MotorPinSleep = 16;
+    static const uint8_t MotorPinCurrentSensing = 17;
+
     /*
 
       Colors
 
     */
-
-
     // A useful RGB565 calculator: http://www.rinkydinkelectronics.com/calc_rgb565.php
+    #define ColorBlack 0x0000
+    #define ColorWhite 0xFFFF
+    #define ColorYellow 0xFFE0
+    #define ColorRed 0xF800
+    #define ColorGreen 0x07E0
 
-    static const uint16_t ColorInitSeqBackground = BLACK;
-    static const uint16_t ColorInitSeqTitle = YELLOW;
-    static const uint16_t ColorInitSeqItems = WHITE;
-    static const uint16_t ColorInitSeqSuccess = GREEN;
-    static const uint16_t ColorInitSeqError = RED;
+    #define ColorLightGray 0x9492
+    #define ColorDarkGray 0x528A
+    #define ColorDarkerGray 0x2965
 
-    static const uint16_t ColorHomeBackground = BLACK;
+    #define ColorSoftGreen 0x2BE7
 
-    static const uint16_t ColorHomeMenuTextColor = WHITE;
-    static const uint16_t ColorHomeMenuArrowColor = 0x528A;         // Dark gray
+    #define ColorStopRed 0xF9A6
 
-    static const uint16_t ColorNonPresetText = 0x9492;              // Light gray
-    static const uint16_t ColorNonPresetBackground = BLACK;
-    //static const uint16_t ColorNonPresetIndicator = 0x528A;         // Dark gray
 
-    static const uint16_t ColorPresetText = 0x9492;                 // Light gray
-    static const uint16_t ColorPresetBackground = 0x2965;           // Very dark gray
-    //static const uint16_t ColorPresetNumber = 0x528A;               // Dark gray
-    static const uint16_t ColorPresetArrowColor = 0x528A;           // Dark gray
+    static const uint16_t ColorInitSeqBackground = ColorBlack;
+    static const uint16_t ColorInitSeqTitle = ColorYellow;
+    static const uint16_t ColorInitSeqItems = ColorWhite;
+    static const uint16_t ColorInitSeqSuccess = ColorGreen;
+    static const uint16_t ColorInitSeqError = ColorRed;
 
-    static const uint16_t ColorPresetSelectedText = WHITE;
-    static const uint16_t ColorPresetSelectedBackground = 0x2BE7;   // Soft green
-    static const uint16_t ColorPresetSelectedNumber = 0x556B;       // Slightly lighter green
+    static const uint16_t ColorHomeBackground = ColorBlack;
 
+    static const uint16_t ColorHomeMenuText = ColorWhite;
+    static const uint16_t ColorHomeMenuArrow = ColorDarkGray;
+
+    static const uint16_t ColorNonPresetText = ColorLightGray;
+    static const uint16_t ColorNonPresetBackground = ColorBlack;
+
+    static const uint16_t ColorPresetText = ColorLightGray;
+    static const uint16_t ColorPresetBackground = ColorDarkerGray;
+    static const uint16_t ColorPresetArrow = ColorDarkGray;
+
+    static const uint16_t ColorPresetSelectedText = ColorWhite;
+    static const uint16_t ColorPresetSelectedBackground = ColorSoftGreen;
+
+
+    static const uint16_t ColorMoveBackground = ColorBlack;
+
+    static const uint16_t ColorMoveArrow = ColorDarkGray;
+    static const uint16_t ColorMoveTarget = ColorWhite;
+    static const uint16_t ColorMoveCurrent = ColorWhite;
+    static const uint16_t ColorMoveStop = ColorStopRed;
 };
 
 #endif
\ No newline at end of file
diff --git a/src/lib/debug.h b/src/lib/debug.h
index 1e42731..5d76651 100644
--- a/src/lib/debug.h
+++ b/src/lib/debug.h
@@ -1,16 +1,20 @@
 #ifndef __debug
 #define __debug
 
+#include <Arduino.h>
+
 #define SerialDebug
 
 
 
 #ifdef SerialDebug
   #define DebugInit() Serial.begin(9600)
-  #define DebugLog(...) Serial.println(__VA_ARGS__)
+  #define dl(value) Serial.print(value)
+  #define dln(value) Serial.println(value)
 #else
   #define DebugInit()
-  #define DebugLog(...)
+  #define dl(value)
+  #define dln(value)
 #endif
 
 
diff --git a/src/lib/motor.cpp b/src/lib/motor.cpp
index 77208c1..5df1f3d 100644
--- a/src/lib/motor.cpp
+++ b/src/lib/motor.cpp
@@ -1,21 +1,39 @@
 #include "./motor.h"
+#include "include/config.h"
+#include <Arduino.h>
+
+
+void motorInit()
+{
+  pinMode(Config::MotorPinSleep, OUTPUT);
+  digitalWrite(Config::MotorPinSleep, LOW);
+
+  pinMode(Config::MotorPinPWM, OUTPUT);
+  digitalWrite(Config::MotorPinPWM, LOW);
+
+  pinMode(Config::MotorPinDirection, OUTPUT);
+  pinMode(Config::MotorPinCurrentSensing, INPUT);
+}
 
 
 void motorStart(MotorDirection direction)
 {
-  // TODO
+  digitalWrite(Config::MotorPinDirection, direction == MotorDirection::Up ? HIGH : LOW);
+  digitalWrite(Config::MotorPinSleep, HIGH);
+  digitalWrite(Config::MotorPinPWM, HIGH);
 }
 
 
 void motorStop()
 {
-  // TODO
+  digitalWrite(Config::MotorPinPWM, LOW);
+  digitalWrite(Config::MotorPinSleep, LOW);
 }
 
 
 bool motorIsOverCurrent()
 {
-  // TODO
+  // TODO: implement motorIsOverCurrent
   return false;
 }
 
diff --git a/src/lib/motor.h b/src/lib/motor.h
index a2de8d6..48b6162 100644
--- a/src/lib/motor.h
+++ b/src/lib/motor.h
@@ -10,6 +10,7 @@ enum class MotorDirection
 };
 
 
+extern void motorInit();
 extern void motorStart(MotorDirection direction);
 extern void motorStop();
 extern bool motorIsOverCurrent();
diff --git a/src/lib/motorstate.cpp b/src/lib/motorstate.cpp
index be35e02..c6d4860 100644
--- a/src/lib/motorstate.cpp
+++ b/src/lib/motorstate.cpp
@@ -2,17 +2,23 @@
 #include "./motor.h"
 #include "./state.h"
 #include "./settings.h"
+#include "./debug.h"
 #include "include/config.h"
 
 
 void motorStateMoveTo(uint16_t height)
 {
+  dl("motorStateMoveTo: "); dln(height);
 
+  State.MoveTarget = height;
+  State.MoveDirection = height > State.CurrentHeight ? Direction::Up : Direction::Down;
 }
 
 
 bool motorStateCheckTargetReached()
 {
+  dl("motorStateCheckTargetReached: direction = "); dl((uint8_t)State.MoveDirection); dl(", currentHeight = "); dln(State.CurrentHeight);
+
   switch (State.MoveDirection)
   {
     case Direction::Up:
@@ -49,6 +55,8 @@ bool motorStateCheckOverCurrent()
 {
   if (motorIsOverCurrent())
   {
+    dln("motorStateCheckOverCurrent: overcurrent detected!");
+
     motorStateStop();
     return true;
   }
@@ -59,6 +67,8 @@ bool motorStateCheckOverCurrent()
 
 void motorStateStop()
 {
+  dln("motorStateStop");
+
   motorStop();
   State.MoveDirection = Direction::None;
 }
@@ -77,4 +87,5 @@ void getDisplayHeight(char* buffer, uint16_t value)
   buffer[2] = '0' + ((displayValue / 10) % 10);
   buffer[3] = '0' + (displayValue % 10);
   buffer[4] = 'm';
+  buffer[5] = 0;
 }
\ No newline at end of file
diff --git a/src/lib/motorstate.h b/src/lib/motorstate.h
index 5994226..f423958 100644
--- a/src/lib/motorstate.h
+++ b/src/lib/motorstate.h
@@ -11,7 +11,7 @@ extern void motorStateStop();
 
 
 // Formats a height value as "0.00m" (always exactly 5 characters long).
-// Buffer must be at least 5 bytes long. No null character is added.
+// Buffer must be at least 6 bytes long, a null character is added.
 // The value is the raw height sensor value, the offset is added by this function.
 extern void getDisplayHeight(char* buffer, uint16_t value);
 
diff --git a/src/lib/screen.cpp b/src/lib/screen.cpp
index d5896f9..2a2f759 100644
--- a/src/lib/screen.cpp
+++ b/src/lib/screen.cpp
@@ -8,16 +8,34 @@ Adafruit_GFX* BaseScreen::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);
+
+  textX = (Config::DisplayWidth - textW) / 2;
+  display->setCursor(textX, y);
+  display->print(text);
+
+  return textW;
+}
+
+
 void ScreenManager::init()
 {
-  pinMode(Config::DisplayPortBL, OUTPUT);
-  digitalWrite(Config::DisplayPortBL, HIGH);
+  pinMode(Config::DisplayPinBL, OUTPUT);
+  digitalWrite(Config::DisplayPinBL, HIGH);
 }
 
 
 void ScreenManager::displayOff()
 {
-  digitalWrite(Config::DisplayPortBL, LOW);
+  digitalWrite(Config::DisplayPinBL, LOW);
   this->display->sendCommand(ST77XX_SLPIN);
 }
 
@@ -25,5 +43,5 @@ void ScreenManager::displayOff()
 void ScreenManager::displayOn()
 {
   this->display->sendCommand(ST77XX_SLPOUT);
-  digitalWrite(Config::DisplayPortBL, HIGH);
+  digitalWrite(Config::DisplayPinBL, HIGH);
 }
diff --git a/src/lib/screen.h b/src/lib/screen.h
index 001c698..00e74e9 100644
--- a/src/lib/screen.h
+++ b/src/lib/screen.h
@@ -34,6 +34,8 @@ class BaseScreen
     ScreenManager* getScreenManager() { return this->screenManager; }
     Adafruit_GFX* getDisplay();
 
+    uint16_t printCentered(const char* text, int16_t y);
+
   private:
     ScreenManager* screenManager;
 };
diff --git a/src/lib/screen/calibrate.cpp b/src/lib/screen/calibrate.cpp
new file mode 100644
index 0000000..a765882
--- /dev/null
+++ b/src/lib/screen/calibrate.cpp
@@ -0,0 +1,19 @@
+#include "./calibrate.h"
+
+
+void CalibrateScreen::onShow()
+{
+  auto display = this->getDisplay();
+
+  // TODO: implement CalibrateScreen
+}
+
+
+void CalibrateScreen::onButton(Button button)
+{
+}
+
+
+void CalibrateScreen::onTick()
+{
+}
\ No newline at end of file
diff --git a/src/lib/screen/calibrate.h b/src/lib/screen/calibrate.h
new file mode 100644
index 0000000..b3193bc
--- /dev/null
+++ b/src/lib/screen/calibrate.h
@@ -0,0 +1,25 @@
+#ifndef __screen_calibrate
+#define __screen_calibrate
+
+#include "../screen.h"
+#include "../state.h"
+
+
+/*
+ * Calibrate screen
+ *   Configures the absolute height (calculates the offset) and
+ *   the allowed minimum and maximum absolute height.
+ */
+class CalibrateScreen : public BaseScreen
+{
+  public:
+    CalibrateScreen(ScreenManager* screenManager) : BaseScreen(screenManager) { }
+
+    void onShow();
+    void onButton(Button button);
+    void onTick();
+
+  private:
+};
+
+#endif
diff --git a/src/lib/screen/home.cpp b/src/lib/screen/home.cpp
index db00c5b..ce9256d 100644
--- a/src/lib/screen/home.cpp
+++ b/src/lib/screen/home.cpp
@@ -4,13 +4,12 @@
 #include "include/config.h"
 #include "lib/settings.h"
 #include "lib/motorstate.h"
+#include "./menu.h"
 
 
 #define HOME_FONT_BASELINE 25
 #define HOME_FONT_HEIGHT 42
 
-#define HOME_MOVING_REFRESHRATE 250
-
 #define PRESET_MARGIN 7
 
 // HOME_FONT_BASELINE is not a mistake, as there are no characters used which
@@ -53,7 +52,7 @@ void HomeScreen::onButton(Button button)
     this->idle = false;
     this->showTime = State.CurrentTime;
 
-    // TODO should preset buttons activate immediately?
+    // TODO: should preset buttons activate immediately?
     return;
   }
 
@@ -70,7 +69,7 @@ void HomeScreen::onButton(Button button)
       break;
 
     case Button::Menu:
-      // TODO show menu
+      this->getScreenManager()->show<MenuScreen>();
       break;
   }
 }
@@ -78,14 +77,11 @@ void HomeScreen::onButton(Button button)
 
 void HomeScreen::onTick()
 {
-  // TODO reactivate
-  /*
   if (!this->idle && State.CurrentTime - this->showTime >= Config::DisplayIdleTime)
   {
     this->getScreenManager()->displayOff();
     this->idle = true;
   }
-  */
 }
 
 
@@ -107,8 +103,6 @@ void HomeScreen::drawNonPresetHeight()
   auto display = this->getDisplay();
   auto y = PRESET_LINEHEIGHT;
 
-  display->fillRect(0, y, Config::DisplayWidth, PRESET_LINEHEIGHT, Config::ColorNonPresetBackground);
-
   if (State.CurrentHeight != Settings.Height.Preset[0] &&
       State.CurrentHeight != Settings.Height.Preset[1])
   {
@@ -118,17 +112,17 @@ void HomeScreen::drawNonPresetHeight()
 }
 
 
-void HomeScreen::drawArrow(uint8_t y, uint16_t color)
+void HomeScreen::drawArrow(int16_t y, uint16_t color)
 {
   this->getDisplay()->fillTriangle(
-    HOME_ARROW_MARGIN, y + HOME_ARROW_YOFFSET + (HOME_ARROW_HEIGHT / 2),                // Middle left
     HOME_ARROW_MARGIN + HOME_ARROW_WIDTH, y + HOME_ARROW_YOFFSET,                       // Top right
+    HOME_ARROW_MARGIN, y + HOME_ARROW_YOFFSET + (HOME_ARROW_HEIGHT / 2),                // Middle left
     HOME_ARROW_MARGIN + HOME_ARROW_WIDTH, y + HOME_ARROW_YOFFSET + HOME_ARROW_HEIGHT,   // Bottom right
     color);
 }
 
 
-void HomeScreen::drawPreset(uint8_t y, uint16_t value)
+void HomeScreen::drawPreset(int16_t y, uint16_t value)
 {
   auto display = this->getDisplay();
   uint16_t textColor;
@@ -146,7 +140,7 @@ void HomeScreen::drawPreset(uint8_t y, uint16_t value)
   {
     textColor = Config::ColorPresetText;
     backgroundColor = Config::ColorPresetBackground;
-    arrowColor = Config::ColorPresetArrowColor;
+    arrowColor = Config::ColorPresetArrow;
   }
 
   display->fillRect(0, y, Config::DisplayWidth, PRESET_LINEHEIGHT, backgroundColor);
@@ -159,24 +153,14 @@ void HomeScreen::drawPreset(uint8_t y, uint16_t value)
 }
 
 
-void HomeScreen::drawHeight(uint8_t y, uint16_t value)
+void HomeScreen::drawHeight(int16_t y, uint16_t value)
 {
   auto display = this->getDisplay();
 
   char textValue[6];
   getDisplayHeight(&textValue[0], value);
-  textValue[5] = 0;
 
-  // Calculate the center position
-  int16_t textX;
-  int16_t textY;
-  uint16_t textW;
-  uint16_t textH;
-  display->getTextBounds(&textValue[0], 0, 0, &textX, &textY, &textW, &textH);
-
-  textX = (Config::DisplayWidth - textW) / 2;
-  display->setCursor(textX, y + HOME_FONT_BASELINE + PRESET_MARGIN);
-  display->print(textValue);
+  this->printCentered(&textValue[0], y + HOME_FONT_BASELINE + PRESET_MARGIN);
 }
 
 
@@ -184,16 +168,8 @@ void HomeScreen::drawMenu()
 {
   auto display = this->getDisplay();
 
-  this->drawArrow(HOME_MENU_Y, Config::ColorHomeMenuArrowColor);
+  this->drawArrow(HOME_MENU_Y, Config::ColorHomeMenuArrow);
 
-  int16_t textX;
-  int16_t textY;
-  uint16_t textW;
-  uint16_t textH;
-  display->getTextBounds("Menu", 0, 0, &textX, &textY, &textW, &textH);
-
-  textX = (Config::DisplayWidth - textW) / 2;
-  display->setCursor(textX, HOME_MENU_Y + HOME_FONT_BASELINE + PRESET_MARGIN);
-  display->setTextColor(Config::ColorHomeMenuTextColor);
-  display->print("Menu");
+  display->setTextColor(Config::ColorHomeMenuText);
+  this->printCentered("Menu", HOME_MENU_Y + HOME_FONT_BASELINE + PRESET_MARGIN);
 }
diff --git a/src/lib/screen/home.h b/src/lib/screen/home.h
index 616abd4..1989b29 100644
--- a/src/lib/screen/home.h
+++ b/src/lib/screen/home.h
@@ -26,9 +26,9 @@ class HomeScreen : public BaseScreen
     void drawPreset2();
     void drawNonPresetHeight();
 
-    void drawArrow(uint8_t y, uint16_t color);
-    void drawPreset(uint8_t y, uint16_t value);
-    void drawHeight(uint8_t y, uint16_t value);
+    void drawArrow(int16_t y, uint16_t color);
+    void drawPreset(int16_t y, uint16_t value);
+    void drawHeight(int16_t y, uint16_t value);
     void drawMenu();
 };
 
diff --git a/src/lib/screen/manual.cpp b/src/lib/screen/manual.cpp
new file mode 100644
index 0000000..21e38d9
--- /dev/null
+++ b/src/lib/screen/manual.cpp
@@ -0,0 +1,19 @@
+#include "./manual.h"
+
+
+void ManualScreen::onShow()
+{
+  auto display = this->getDisplay();
+
+  // TODO: implement ManualScreen
+}
+
+
+void ManualScreen::onButton(Button button)
+{
+}
+
+
+void ManualScreen::onTick()
+{
+}
\ No newline at end of file
diff --git a/src/lib/screen/manual.h b/src/lib/screen/manual.h
new file mode 100644
index 0000000..5356fd6
--- /dev/null
+++ b/src/lib/screen/manual.h
@@ -0,0 +1,24 @@
+#ifndef __screen_manual
+#define __screen_manual
+
+#include "../screen.h"
+#include "../state.h"
+
+
+/*
+ * Manual screen
+ *   Allows manual entry of the preferred height.
+ */
+class ManualScreen : public BaseScreen
+{
+  public:
+    ManualScreen(ScreenManager* screenManager) : BaseScreen(screenManager) { }
+
+    void onShow();
+    void onButton(Button button);
+    void onTick();
+
+  private:
+};
+
+#endif
diff --git a/src/lib/screen/menu.cpp b/src/lib/screen/menu.cpp
new file mode 100644
index 0000000..c5196ff
--- /dev/null
+++ b/src/lib/screen/menu.cpp
@@ -0,0 +1,19 @@
+#include "./menu.h"
+
+
+void MenuScreen::onShow()
+{
+  auto display = this->getDisplay();
+
+  // TODO: implement MenuScreen
+}
+
+
+void MenuScreen::onButton(Button button)
+{
+}
+
+
+void MenuScreen::onTick()
+{
+}
\ No newline at end of file
diff --git a/src/lib/screen/menu.h b/src/lib/screen/menu.h
new file mode 100644
index 0000000..3e682d0
--- /dev/null
+++ b/src/lib/screen/menu.h
@@ -0,0 +1,25 @@
+#ifndef __screen_menu
+#define __screen_menu
+
+#include "../screen.h"
+#include "../state.h"
+
+
+/*
+ * Menu screen
+ *   Shows the menu which allows access to the calibration and
+ *   manual adjustment screens.
+ */
+class MenuScreen : public BaseScreen
+{
+  public:
+    MenuScreen(ScreenManager* screenManager) : BaseScreen(screenManager) { }
+
+    void onShow();
+    void onButton(Button button);
+    void onTick();
+
+  private:
+};
+
+#endif
diff --git a/src/lib/screen/move-overcurrent.cpp b/src/lib/screen/move-overcurrent.cpp
new file mode 100644
index 0000000..62efee0
--- /dev/null
+++ b/src/lib/screen/move-overcurrent.cpp
@@ -0,0 +1,19 @@
+#include "./move-overcurrent.h"
+
+
+void MoveOverCurrentScreen::onShow()
+{
+  auto display = this->getDisplay();
+
+  // TODO: implement MoveOverCurrentScreen
+}
+
+
+void MoveOverCurrentScreen::onButton(Button button)
+{
+}
+
+
+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
new file mode 100644
index 0000000..a039e28
--- /dev/null
+++ b/src/lib/screen/move-overcurrent.h
@@ -0,0 +1,24 @@
+#ifndef __screen_move_overcurrent
+#define __screen_move_overcurrent
+
+#include "../screen.h"
+#include "../state.h"
+
+
+/*
+ * Move overcurrent screen
+ *   Displays a warning that the motor driver has reached the set maximum current.
+ */
+class MoveOverCurrentScreen : public BaseScreen
+{
+  public:
+    MoveOverCurrentScreen(ScreenManager* screenManager) : BaseScreen(screenManager) { }
+
+    void onShow();
+    void onButton(Button button);
+    void onTick();
+
+  private:
+};
+
+#endif
diff --git a/src/lib/screen/move-sensorerror.cpp b/src/lib/screen/move-sensorerror.cpp
new file mode 100644
index 0000000..a65351b
--- /dev/null
+++ b/src/lib/screen/move-sensorerror.cpp
@@ -0,0 +1,19 @@
+#include "./move-sensorerror.h"
+
+
+void MoveSensorErrorScreen::onShow()
+{
+  auto display = this->getDisplay();
+
+  // TODO: implement MoveSensorErrorScreen
+}
+
+
+void MoveSensorErrorScreen::onButton(Button button)
+{
+}
+
+
+void MoveSensorErrorScreen::onTick()
+{
+}
\ No newline at end of file
diff --git a/src/lib/screen/move-sensorerror.h b/src/lib/screen/move-sensorerror.h
new file mode 100644
index 0000000..4dfa71b
--- /dev/null
+++ b/src/lib/screen/move-sensorerror.h
@@ -0,0 +1,25 @@
+#ifndef __screen_move_sensorerror
+#define __screen_move_sensorerror
+
+#include "../screen.h"
+#include "../state.h"
+
+
+/*
+ * Move sensor error screen
+ *   Shows a warning that the height sensor is not providing valid values.
+ *   Can only be dismissed after the sensor is stable again.
+ */
+class MoveSensorErrorScreen : public BaseScreen
+{
+  public:
+    MoveSensorErrorScreen(ScreenManager* screenManager) : BaseScreen(screenManager) { }
+
+    void onShow();
+    void onButton(Button button);
+    void onTick();
+
+  private:
+};
+
+#endif
diff --git a/src/lib/screen/move.cpp b/src/lib/screen/move.cpp
index 2c4f748..cde9714 100644
--- a/src/lib/screen/move.cpp
+++ b/src/lib/screen/move.cpp
@@ -9,17 +9,70 @@
 #define MOVE_FONT_BASELINE 25
 #define MOVE_FONT_HEIGHT 42
 
-#define MOVE_REFRESHRATE 250
+#define MOVE_REFRESHRATE 1000
+
+#define MOVE_LINE_MARGIN 7
+#define MOVE_LINE_HEIGHT (MOVE_FONT_BASELINE + (2 * MOVE_LINE_MARGIN))
+
+#define MOVE_STOP_SMALL_TEXT_SIZE
+#define MOVE_STOP_SMALL_LINE_HEIGHT 8
+
+#define MOVE_ARROW_WIDTH 20
+#define MOVE_ARROW_HEIGHT 10
+#define MOVE_ARROW_YOFFSET ((MOVE_LINE_HEIGHT - MOVE_ARROW_HEIGHT) / 2)
 
 
 void MoveScreen::onShow()
 {
   auto display = this->getDisplay();
+  //auto startY = (Config::DisplayHeight - (MOVE_LINE_HEIGHT * 3)) / 2;
+  auto startY = MOVE_LINE_HEIGHT;
+  auto arrowY = startY + MOVE_LINE_HEIGHT + MOVE_ARROW_YOFFSET;
+  auto arrowX = (Config::DisplayWidth - MOVE_ARROW_WIDTH) / 2;
+  auto stopY = Config::DisplayHeight - MOVE_LINE_HEIGHT;
+
+  display->fillScreen(Config::ColorMoveBackground);
+
+  // Stop
+  display->setFont(nullptr);
+  display->setTextColor(Config::ColorMoveStop);
+  this->printCentered("Press any button to", stopY - MOVE_STOP_SMALL_LINE_HEIGHT);
 
   display->setFont(&FreeSansBold18pt7bTrimmed);
-  display->fillScreen(Config::ColorHomeBackground);
+  this->printCentered("STOP", stopY + MOVE_FONT_BASELINE + MOVE_LINE_MARGIN);
 
-  // TODO draw current and target height
+  char targetHeightText[6];
+  getDisplayHeight(&targetHeightText[0], State.MoveTarget);
+  display->setTextColor(Config::ColorMoveCurrent);
+
+  // Target and arrow
+  if (State.MoveDirection == Direction::Up)
+  {
+    this->currentHeightY = startY + (MOVE_LINE_HEIGHT * 2);
+
+    this->printCentered(&targetHeightText[0], startY + MOVE_FONT_BASELINE + MOVE_LINE_MARGIN);
+
+    display->fillTriangle(
+      arrowX + (MOVE_ARROW_WIDTH / 2), arrowY,                // Top middle
+      arrowX, arrowY + MOVE_ARROW_HEIGHT,                     // Bottom left
+      arrowX + MOVE_ARROW_WIDTH, arrowY + MOVE_ARROW_HEIGHT,  // Bottom right
+      Config::ColorMoveArrow);
+  }
+  else
+  {
+    this->currentHeightY = startY;
+
+    this->printCentered(&targetHeightText[0], startY + (MOVE_LINE_HEIGHT * 2) + MOVE_FONT_BASELINE + MOVE_LINE_MARGIN);
+
+    display->fillTriangle(
+      arrowX, arrowY,                                                 // Top left
+      arrowX + MOVE_ARROW_WIDTH, arrowY,                              // Top right
+      arrowX + (MOVE_ARROW_WIDTH / 2), arrowY + MOVE_ARROW_HEIGHT,    // Bottom middle
+      Config::ColorMoveArrow);
+  }
+
+  this->lastRefresh = State.CurrentTime;
+  this->drawCurrentHeight();
 }
 
 
@@ -32,18 +85,32 @@ void MoveScreen::onButton(Button button)
 
 void MoveScreen::onTick()
 {
-  if (State.MoveDirection != Direction::None)
+  if (State.MoveDirection == Direction::None)
   {
     this->getScreenManager()->show<HomeScreen>();
     return;
   }
 
-  // Don't update every tick, it takes a significant amount of time
-  // and monitoring the current height is more important.
-  // TODO should we also stop updating when we're approaching the target height or is that overkill?
+  // Don't update every tick, monitoring the current height is more
+  // important and the flicker would be unpleasant as well.
   if (State.CurrentTime - this->lastRefresh >= MOVE_REFRESHRATE)
   {
-
+    this->drawCurrentHeight();
     this->lastRefresh = State.CurrentTime;
   }
+}
+
+
+void MoveScreen::drawCurrentHeight()
+{
+  auto display = this->getDisplay();
+
+  char currentHeightText[6];
+  getDisplayHeight(&currentHeightText[0], State.CurrentHeight);
+
+  if (this->lastTextWidth > 0)
+    display->fillRect((Config::DisplayWidth - this->lastTextWidth) / 2, this->currentHeightY, this->lastTextWidth, MOVE_LINE_HEIGHT, Config::ColorMoveBackground);
+
+  display->setTextColor(Config::ColorMoveTarget);
+  this->lastTextWidth = this->printCentered(&currentHeightText[0], this->currentHeightY + MOVE_FONT_BASELINE);
 }
\ No newline at end of file
diff --git a/src/lib/screen/move.h b/src/lib/screen/move.h
index 3cf5bc3..8f8f8c5 100644
--- a/src/lib/screen/move.h
+++ b/src/lib/screen/move.h
@@ -20,6 +20,10 @@ class MoveScreen : public BaseScreen
 
   private:
     uint32_t lastRefresh;
+    uint8_t currentHeightY;
+    uint16_t lastTextWidth = 0;
+
+    void drawCurrentHeight();
 };
 
 #endif
diff --git a/src/main.cpp b/src/main.cpp
index 09cc669..1607d59 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -13,6 +13,9 @@
 #include "./lib/motor.h"
 #include "./lib/motorstate.h"
 #include "./lib/screen/home.h"
+#include "./lib/screen/calibrate.h"
+#include "./lib/screen/move-overcurrent.h"
+#include "./lib/screen/move-sensorerror.h"
 
 
 enum class InitSequenceStep
@@ -38,7 +41,7 @@ void initSequenceEnd();
 bool heightSensorGetRange(uint16_t* measurement);
 
 
-auto display = Adafruit_ST7789(Config::DisplayPortCS, Config::DisplayPortDC, Config::DisplayPortRST);
+auto display = Adafruit_ST7789(Config::DisplayPinCS, Config::DisplayPinDC, Config::DisplayPinRST);
 auto heightSensor = VL53L0X();
 
 auto screenManager = ScreenManager(&display);
@@ -54,10 +57,11 @@ Bounce buttons[3];
 void setup()
 {
   DebugInit();
+  dln("Debug log active");
 
-  buttons[0].attach(Config::ButtonPortUp, INPUT_PULLUP);
-  buttons[1].attach(Config::ButtonPortMenu, INPUT_PULLUP);
-  buttons[2].attach(Config::ButtonPortDown, INPUT_PULLUP);
+  buttons[0].attach(Config::ButtonPinUp, INPUT_PULLUP);
+  buttons[1].attach(Config::ButtonPinMenu, INPUT_PULLUP);
+  buttons[2].attach(Config::ButtonPinDown, INPUT_PULLUP);
 
   display.init(Config::DisplayWidth, Config::DisplayHeight, SPI_MODE3);
   display.setRotation(Config::DisplayRotation);
@@ -65,6 +69,7 @@ void setup()
   screenManager.init();
 
   initSequenceStart();
+  motorInit();
 
   // Load settings from EEPROM
   auto initialized = readSettings();
@@ -80,16 +85,13 @@ void setup()
 
   State.CurrentTime = millis();
 
-  // TODO check if close to either preset, then use the preset height
+  // TODO: check if close to either preset, then use the preset height
   State.CurrentHeight = currentHeight;
 
   if (initialized)
-  {
     screenManager.show<HomeScreen>();
-  }
   else
-    // TODO show height configuration screen
-    screenManager.show<HomeScreen>();
+    screenManager.show<CalibrateScreen>();
 }
 
 
@@ -144,7 +146,7 @@ inline uint16_t testHeightSensor()
   uint8_t closeCount = 0;
   uint16_t measurement;
 
-  // TODO while (closeCount < 3)
+  // TODO: while (closeCount < 3)
   while (closeCount < 1)
   {
     if (heightSensorGetRange(&measurement))
@@ -193,9 +195,7 @@ void loop()
   if (State.MoveDirection != Direction::None)
   {
     if (motorStateCheckOverCurrent())
-    {
-      // TODO go to overcurrent screen
-    }
+      screenManager.show<MoveOverCurrentScreen>();
     else
       updateHeight();
   }
@@ -235,9 +235,10 @@ void updateHeight()
   }
   else if (State.CurrentTime - lastValidMeasurement >= Config::HeightMeasurementAbortTimeout)
   {
+    dln("Out of range timeout!");
     motorStateStop();
 
-    // TODO go to height sensor error screen
+    screenManager.show<MoveSensorErrorScreen>();
   }
 }
 
@@ -245,7 +246,9 @@ void updateHeight()
 bool heightSensorGetRange(uint16_t* measurement)
 {
   *measurement = heightSensor.readRangeSingleMillimeters();
-  return !heightSensor.timeoutOccurred() && *measurement <= Config::HeightMeasurementMax;
+  dl("Range: "); dln(*measurement);
+
+  return *measurement <= Config::HeightMeasurementMax;
 }