diff --git a/doc/DeskControl UI mockup - LCD version.psd b/doc/DeskControl UI mockup - LCD version.psd
index 4ac78a2..7c7baed 100644
Binary files a/doc/DeskControl UI mockup - LCD version.psd and b/doc/DeskControl UI mockup - LCD version.psd differ
diff --git a/src/fonts/trim.bat b/src/fonts/trim.bat
index 851144f..c1e7361 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\.mSTOPMenursaybto] FreeSansBold18pt7b.h
+AdafruitGFXFontTrim [ 0-9\.mSTOPMenuER] FreeSansBold18pt7b.h
 
 pause
\ No newline at end of file
diff --git a/src/include/config.h b/src/include/config.h
index b556fbf..740eef7 100644
--- a/src/include/config.h
+++ b/src/include/config.h
@@ -58,7 +58,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 = 1; // TODO: restore StableCount to 3
+    static const uint8_t HeightMeasurementDeltaStableCount = 3;
 
     // How far in advance to stop the motor
     static const uint8_t HeightMeasurementDeltaStop = 0;
@@ -104,12 +104,15 @@ class Config
     #define ColorDarkBlue 0x0907
 
 
+    // Init sequence
     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;
 
+
+    // Home
     static const uint16_t ColorHomeBackground = ColorBlack;
 
     static const uint16_t ColorHomeMenuText = ColorWhite;
@@ -126,6 +129,7 @@ class Config
     static const uint16_t ColorPresetSelectedBackground = ColorSoftGreen;
 
 
+    // Move
     static const uint16_t ColorMoveBackground = ColorBlack;
 
     static const uint16_t ColorMoveArrow = ColorDarkGray;
@@ -134,6 +138,11 @@ class Config
     static const uint16_t ColorMoveStop = ColorStopRed;
 
 
+    // Move error / overcurrent
+    static const uint16_t ColorMoveErrorText = ColorStopRed;
+
+
+    // Menu
     static const uint16_t ColorMenuHeaderText = ColorWhite;
     static const uint16_t ColorMenuHeaderBackground = ColorSoftBlue;
 
diff --git a/src/lib/control.cpp b/src/lib/control.cpp
index 759b4f3..516e01d 100644
--- a/src/lib/control.cpp
+++ b/src/lib/control.cpp
@@ -1,31 +1,118 @@
 #include "./control.h"
+#include <Wire.h>
 #include "./motor.h"
-#include "./state.h"
+#include "./Control.h"
 #include "./settings.h"
 #include "./debug.h"
 #include "include/config.h"
 
 
-void controlMoveTo(uint16_t height)
-{
-  dl("controlMoveTo: "); dln(height);
+ControlManager Control = ControlManager();
 
-  State.MoveTarget = height;
-  State.MoveDirection = height > State.CurrentHeight ? Direction::Up : Direction::Down;
+
+VL53L0XInitResult ControlManager::init()
+{
+  Wire.begin();
+
+  this->heightSensor.setTimeout(500);
+  auto result = this->heightSensor.init();
+  if (result != VL53L0XInitResult::Success)
+    return result;
+
+  this->heightSensor.setMeasurementTimingBudget(Config::HeightSensorBudget);
+  return result;
 }
 
 
+ControlUpdateResult ControlManager::update()
+{
+
+}
+
+
+void ControlManager::stabilizeStart()
+{
+  this->stabilizing = true;
+}
+
+
+bool ControlManager::stabilized()
+{
+  // TODO: stabilized
+}
+
+
+void ControlManager::movePrepare(uint16_t 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;
+  this->stabilizeStart();
+  //this->sensorError = true;
+  return false;
+
+  motorStart(this->moveDirection == MoveDirection::Up ? MotorDirection::Up : MotorDirection::Down);
+  return true;
+}
+
+
+void ControlManager::moveStop()
+{
+  motorStop();
+  this->moveDirection = MoveDirection::None;
+}
+
+
+
+void ControlManager::snapToPreset()
+{
+  for (uint8_t i = 0; i < 2; i++)
+  {
+    if (abs(this->currentHeight - Settings.Height.Preset[i]) <= Config::HeightMeasurementDeltaOnTarget)
+    {
+      this->currentHeight = Settings.Height.Preset[i];
+      break;
+    }
+  }
+}
+
+
+void ControlManager::getDisplayHeight(char* buffer, uint16_t value)
+{
+  uint8_t displayValue = (value + Settings.Height.Offset) / 10;
+
+  if (displayValue > 99)
+    buffer[0] = '0' + ((displayValue / 100) % 10);
+  else
+    buffer[0] = '0';
+
+  buffer[1] = '.';
+  buffer[2] = '0' + ((displayValue / 10) % 10);
+  buffer[3] = '0' + (displayValue % 10);
+  buffer[4] = 'm';
+  buffer[5] = 0;
+}
+
+
+/*
 bool controlCheckTargetReached()
 {
-  dl("controlCheckTargetReached: direction = "); dl((uint8_t)State.MoveDirection); dl(", currentHeight = "); dln(State.CurrentHeight);
+  dl("controlCheckTargetReached: direction = "); dl((uint8_t)Control.getMoveDirection()); dl(", currentHeight = "); dln(Control.getCurrentHeight());
 
-  switch (State.MoveDirection)
+  switch (Control.getMoveDirection())
   {
     case Direction::Up:
-      if (State.CurrentHeight >= State.MoveTarget - Config::HeightMeasurementDeltaStop)
+      if (Control.getCurrentHeight() >= Control.getMoveTarget() - Config::HeightMeasurementDeltaStop)
       {
-        if (State.CurrentHeight - State.MoveTarget <= Config::HeightMeasurementDeltaOnTarget)
-          State.CurrentHeight = State.MoveTarget;
+        if (Control.getCurrentHeight() - Control.getMoveTarget() <= Config::HeightMeasurementDeltaOnTarget)
+          Control.getCurrentHeight() = Control.getMoveTarget();
 
         controlStop();
         return true;
@@ -33,10 +120,10 @@ bool controlCheckTargetReached()
       break;
 
     case Direction::Down:
-      if (State.CurrentHeight <= State.MoveTarget + Config::HeightMeasurementDeltaStop)
+      if (Control.getCurrentHeight() <= Control.getMoveTarget() + Config::HeightMeasurementDeltaStop)
       {
-        if (State.MoveTarget - State.CurrentHeight <= Config::HeightMeasurementDeltaOnTarget)
-          State.CurrentHeight = State.MoveTarget;
+        if (Control.getMoveTarget() - Control.getCurrentHeight() <= Config::HeightMeasurementDeltaOnTarget)
+          Control.getCurrentHeight() = Control.getMoveTarget();
 
         controlStop();
         return true;
@@ -65,40 +152,95 @@ bool controlCheckOverCurrent()
 }
 
 
-void controlStop()
-{
-  dln("controlStop");
-
-  motorStop();
-  State.MoveDirection = Direction::None;
-}
 
 
-void controlSnapToPreset()
-{
-  for (uint8_t i = 0; i < 2; i++)
+  if (Control.getMoveDirection() != Direction::None)
   {
-    if (abs(State.CurrentHeight - Settings.Height.Preset[i]) <= Config::HeightMeasurementDeltaOnTarget)
+    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)
     {
-      State.CurrentHeight = Settings.Height.Preset[i];
-      break;
+      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>();
+  }
 }
 
 
-void getDisplayHeight(char* buffer, uint16_t value)
+bool heightSensorGetRange(uint16_t* measurement)
 {
-  uint8_t displayValue = (value + Settings.Height.Offset) / 10;
+  *measurement = heightSensor.readRangeSingleMillimeters();
+  dl("Range: "); dln(*measurement);
 
-  if (displayValue > 99)
-    buffer[0] = '0' + ((displayValue / 100) % 10);
-  else
-    buffer[0] = '0';
+  return *measurement <= Config::HeightMeasurementMax;
+}
 
-  buffer[1] = '.';
-  buffer[2] = '0' + ((displayValue / 10) % 10);
-  buffer[3] = '0' + (displayValue % 10);
-  buffer[4] = 'm';
-  buffer[5] = 0;
-}
\ No newline at end of file
+
+
+
+
+
+
+
+  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 bf60a46..df27946 100644
--- a/src/lib/control.h
+++ b/src/lib/control.h
@@ -2,20 +2,68 @@
 #define __control
 
 #include <stdint.h>
-
-// High-level functions to control the motor and update the global state
-extern void controlMoveTo(uint16_t height);
-extern bool controlCheckTargetReached();
-extern bool controlCheckOverCurrent();
-extern void controlStop();
-
-extern void controlSnapToPreset();
+#include "./vl53l0x.h"
 
 
-// Formats a height value as "0.00m" (always exactly 5 characters long).
-// 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);
+enum class ControlUpdateResult
+{
+  Idle = 0,
+  Moving = 1,
+  TargetReached = 2,
+  SensorError = 3,
+  OverCurrent = 4
+};
+
+
+enum class MoveDirection
+{
+  None = 0,
+  Up = 1,
+  Down = 2
+};
+
+
+
+class ControlManager
+{
+  public:
+    VL53L0XInitResult init();
+    ControlUpdateResult update();
+
+
+    void stabilizeStart();
+    bool stabilized();
+
+    void movePrepare(uint16_t height);
+    bool moveStart();
+    void moveStop();
+
+    void snapToPreset();
+
+
+    // Formats a height value as "0.00m" (always exactly 5 characters long).
+    // 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.
+    void getDisplayHeight(char* buffer, uint16_t value);
+
+
+    uint16_t getCurrentHeight() { return this->currentHeight; }
+    MoveDirection getMoveDirection() { return this->moveDirection; }
+    uint16_t getMoveTarget() { return this->moveTarget; }
+
+  private:
+    VL53L0X heightSensor = VL53L0X();
+    uint16_t currentHeight;
+
+    MoveDirection moveDirection;
+    uint16_t moveTarget;
+
+    bool stabilizing = false;
+    uint32_t lastValidMeasurement = 0;
+};
+
+
+extern ControlManager Control;
 
 
 #endif
\ No newline at end of file
diff --git a/src/lib/screen/calibrate.h b/src/lib/screen/calibrate.h
index b3193bc..6291a42 100644
--- a/src/lib/screen/calibrate.h
+++ b/src/lib/screen/calibrate.h
@@ -2,7 +2,7 @@
 #define __screen_calibrate
 
 #include "../screen.h"
-#include "../state.h"
+#include "../control.h"
 
 
 /*
diff --git a/src/lib/screen/home.cpp b/src/lib/screen/home.cpp
index 6b63bbf..2d87247 100644
--- a/src/lib/screen/home.cpp
+++ b/src/lib/screen/home.cpp
@@ -4,13 +4,14 @@
 #include "include/metrics.h"
 #include "lib/settings.h"
 #include "lib/control.h"
+#include "lib/state.h"
 #include "./menu.h"
 
 
 
 void HomeScreen::onShow()
 {
-  this->showTime = State.CurrentTime;
+  this->showTime = CurrentTime;
 
   auto display = this->getDisplay();
 
@@ -31,7 +32,7 @@ void HomeScreen::onButton(Button button)
   {
     this->getScreenManager()->displayOn();
     this->idle = false;
-    this->showTime = State.CurrentTime;
+    this->showTime = CurrentTime;
 
     // Preset buttons activate immediately
     if (button == Button::Menu)
@@ -41,13 +42,15 @@ void HomeScreen::onButton(Button button)
   switch (button)
   {
     case Button::Up:
-      controlMoveTo(Settings.Height.Preset[0]);
+      Control.movePrepare(Settings.Height.Preset[0]);
       this->getScreenManager()->show<MoveScreen>();
+      Control.moveStart();
       break;
 
     case Button::Down:
-      controlMoveTo(Settings.Height.Preset[1]);
+      Control.movePrepare(Settings.Height.Preset[1]);
       this->getScreenManager()->show<MoveScreen>();
+      Control.moveStart();
       break;
 
     case Button::Menu:
@@ -59,7 +62,7 @@ void HomeScreen::onButton(Button button)
 
 void HomeScreen::onTick()
 {
-  if (!this->idle && State.CurrentTime - this->showTime >= Config::DisplayIdleTime)
+  if (!this->idle && CurrentTime - this->showTime >= Config::DisplayIdleTime)
   {
     this->getScreenManager()->displayOff();
     this->idle = true;
@@ -84,11 +87,11 @@ void HomeScreen::drawNonPresetHeight()
   auto display = this->getDisplay();
   auto y = Metrics::LargeTextLineHeight;
 
-  if (State.CurrentHeight != Settings.Height.Preset[0] &&
-      State.CurrentHeight != Settings.Height.Preset[1])
+  if (Control.getCurrentHeight() != Settings.Height.Preset[0] &&
+      Control.getCurrentHeight() != Settings.Height.Preset[1])
   {
     display->setTextColor(Config::ColorNonPresetText);
-    this->drawHeight(y, State.CurrentHeight);
+    this->drawHeight(y, Control.getCurrentHeight());
   }
 }
 
@@ -101,7 +104,7 @@ void HomeScreen::drawPreset(int16_t y, uint16_t value)
   uint16_t arrowColor;
 
   // An exact comparison is enough here, the movement code takes care of that if it's "close enough"
-  if (value == State.CurrentHeight)
+  if (value == Control.getCurrentHeight())
   {
     textColor = Config::ColorPresetSelectedText;
     backgroundColor = Config::ColorPresetSelectedBackground;
@@ -127,7 +130,7 @@ void HomeScreen::drawPreset(int16_t y, uint16_t value)
 void HomeScreen::drawHeight(int16_t y, uint16_t value)
 {
   char textValue[6];
-  getDisplayHeight(&textValue[0], value);
+  Control.getDisplayHeight(&textValue[0], value);
 
   this->printCentered(&textValue[0], y + Metrics::LargeTextLineYOffset);
 }
diff --git a/src/lib/screen/home.h b/src/lib/screen/home.h
index 55ee460..253a504 100644
--- a/src/lib/screen/home.h
+++ b/src/lib/screen/home.h
@@ -2,7 +2,7 @@
 #define __screen_home
 
 #include "../screen.h"
-#include "../state.h"
+#include "../Control.h"
 
 
 /*
diff --git a/src/lib/screen/manual.h b/src/lib/screen/manual.h
index 5356fd6..8efa894 100644
--- a/src/lib/screen/manual.h
+++ b/src/lib/screen/manual.h
@@ -2,7 +2,7 @@
 #define __screen_manual
 
 #include "../screen.h"
-#include "../state.h"
+#include "../Control.h"
 
 
 /*
diff --git a/src/lib/screen/menu.h b/src/lib/screen/menu.h
index 3e682d0..454c4ec 100644
--- a/src/lib/screen/menu.h
+++ b/src/lib/screen/menu.h
@@ -2,7 +2,7 @@
 #define __screen_menu
 
 #include "../screen.h"
-#include "../state.h"
+#include "../Control.h"
 
 
 /*
diff --git a/src/lib/screen/move-overcurrent.h b/src/lib/screen/move-overcurrent.h
index a039e28..b9f4242 100644
--- a/src/lib/screen/move-overcurrent.h
+++ b/src/lib/screen/move-overcurrent.h
@@ -2,7 +2,7 @@
 #define __screen_move_overcurrent
 
 #include "../screen.h"
-#include "../state.h"
+#include "../Control.h"
 
 
 /*
diff --git a/src/lib/screen/move-sensorerror.cpp b/src/lib/screen/move-sensorerror.cpp
index dae888b..94d4d39 100644
--- a/src/lib/screen/move-sensorerror.cpp
+++ b/src/lib/screen/move-sensorerror.cpp
@@ -1,11 +1,31 @@
 #include "./move-sensorerror.h"
+#include "include/config.h"
+#include "include/metrics.h"
+#include "lib/control.h"
+#include "lib/state.h"
 
 
 void MoveSensorErrorScreen::onShow()
 {
-  //auto display = this->getDisplay();
+  auto display = this->getDisplay();
+  auto y = Metrics::LargeTextLineHeight + Metrics::LargeTextLineYOffset;
 
-  // TODO: implement MoveSensorErrorScreen
+  display->setFont(Metrics::LargeFont);
+  display->setTextColor(Config::ColorMoveErrorText);
+  this->printCentered("ERROR", y);
+  y += Metrics::LargeTextLineHeight;
+
+  display->setFont(Metrics::SmallFont);
+  this->printCentered("height sensor failed", y);
+  y += Metrics::SmallTextLineHeight;
+
+  this->printCentered("waiting for stable value", y);
+  y += Metrics::SmallTextLineHeight + Metrics::LargeTextLineHeight;
+
+  this->currentHeightY = y;
+
+  this->lastRefresh = CurrentTime;
+  this->drawCurrentHeight();
 }
 
 
@@ -16,4 +36,19 @@ void MoveSensorErrorScreen::onButton(Button button)
 
 void MoveSensorErrorScreen::onTick()
 {
+}
+
+
+void MoveSensorErrorScreen::drawCurrentHeight()
+{
+  auto display = this->getDisplay();
+
+  char currentHeightText[6];
+  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);
+
+  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 4dfa71b..4ef6311 100644
--- a/src/lib/screen/move-sensorerror.h
+++ b/src/lib/screen/move-sensorerror.h
@@ -2,7 +2,7 @@
 #define __screen_move_sensorerror
 
 #include "../screen.h"
-#include "../state.h"
+#include "../Control.h"
 
 
 /*
@@ -20,6 +20,11 @@ class MoveSensorErrorScreen : public BaseScreen
     void onTick();
 
   private:
+    uint32_t lastRefresh;
+    uint8_t currentHeightY;
+    uint16_t lastTextWidth = 0;
+
+    void drawCurrentHeight();
 };
 
 #endif
diff --git a/src/lib/screen/move.cpp b/src/lib/screen/move.cpp
index 668d574..3f62c5f 100644
--- a/src/lib/screen/move.cpp
+++ b/src/lib/screen/move.cpp
@@ -4,7 +4,7 @@
 #include "include/metrics.h"
 #include "lib/settings.h"
 #include "lib/control.h"
-
+#include "lib/state.h"
 
 
 void MoveScreen::onShow()
@@ -26,11 +26,11 @@ void MoveScreen::onShow()
   this->printCentered("STOP", stopY + Metrics::LargeTextLineYOffset);
 
   char targetHeightText[6];
-  getDisplayHeight(&targetHeightText[0], State.MoveTarget);
+  Control.getDisplayHeight(&targetHeightText[0], Control.getMoveTarget());
   display->setTextColor(Config::ColorMoveCurrent);
 
   // Target and arrow
-  if (State.MoveDirection == Direction::Up)
+  if (Control.getMoveDirection() == MoveDirection::Up)
   {
     this->currentHeightY = startY + (Metrics::LargeTextLineHeight * 2);
 
@@ -45,32 +45,26 @@ void MoveScreen::onShow()
     this->drawArrowDown(arrowX, arrowY, Config::ColorMoveArrow);
   }
 
-  this->lastRefresh = State.CurrentTime;
+  this->lastRefresh = CurrentTime;
   this->drawCurrentHeight();
 }
 
 
 void MoveScreen::onButton(Button button)
 {
-  controlStop();
+  Control.moveStop();
   this->getScreenManager()->show<HomeScreen>();
 }
 
 
 void MoveScreen::onTick()
 {
-  if (State.MoveDirection == Direction::None)
-  {
-    this->getScreenManager()->show<HomeScreen>();
-    return;
-  }
-
   // 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 >= Config::DisplayMoveRefreshRate)
+  if (CurrentTime - this->lastRefresh >= Config::DisplayMoveRefreshRate)
   {
     this->drawCurrentHeight();
-    this->lastRefresh = State.CurrentTime;
+    this->lastRefresh = CurrentTime;
   }
 }
 
@@ -80,7 +74,7 @@ void MoveScreen::drawCurrentHeight()
   auto display = this->getDisplay();
 
   char currentHeightText[6];
-  getDisplayHeight(&currentHeightText[0], State.CurrentHeight);
+  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);
diff --git a/src/lib/screen/move.h b/src/lib/screen/move.h
index 8f8f8c5..5acc83c 100644
--- a/src/lib/screen/move.h
+++ b/src/lib/screen/move.h
@@ -2,7 +2,7 @@
 #define __screen_move
 
 #include "../screen.h"
-#include "../state.h"
+#include "../Control.h"
 
 
 /*
diff --git a/src/lib/state.cpp b/src/lib/state.cpp
index 9f40292..92e1c34 100644
--- a/src/lib/state.cpp
+++ b/src/lib/state.cpp
@@ -1,3 +1,3 @@
-#include "state.h"
+#include "Control.h"
 
-StateContainer State;
\ No newline at end of file
+uint32_t CurrentTime;
\ No newline at end of file
diff --git a/src/lib/state.h b/src/lib/state.h
index 9261962..6d9aa35 100644
--- a/src/lib/state.h
+++ b/src/lib/state.h
@@ -3,23 +3,6 @@
 
 #include "stdint.h"
 
-enum class Direction
-{
-  None = 0,
-  Up = 1,
-  Down = 2
-};
-
-
-struct StateContainer
-{
-  uint32_t CurrentTime;
-  uint16_t CurrentHeight;
-
-  Direction MoveDirection;
-  uint16_t MoveTarget;
-};
-
-extern StateContainer State;
+extern uint32_t CurrentTime;
 
 #endif
diff --git a/src/main.cpp b/src/main.cpp
index e33d884..ed69d48 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,5 +1,4 @@
 #include <SPI.h>
-#include <Wire.h>
 #include <Adafruit_GFX.h>
 #include <Adafruit_ST7789.h>
 #include <Bounce2.h>
@@ -9,9 +8,10 @@
 #include "./lib/settings.h"
 #include "./lib/screen.h"
 #include "./lib/vl53l0x.h"
-#include "./lib/state.h"
+#include "./lib/Control.h"
 #include "./lib/motor.h"
 #include "./lib/control.h"
+#include "./lib/state.h"
 #include "./lib/screen/home.h"
 #include "./lib/screen/calibrate.h"
 #include "./lib/screen/move-overcurrent.h"
@@ -38,12 +38,8 @@ void initSequenceError(InitSequenceStep step);
 void initSequenceDisplayHeight(uint16_t measurement);
 void initSequenceEnd();
 
-bool heightSensorGetRange(uint16_t* measurement);
-
 
 auto display = Adafruit_ST7789(Config::DisplayPinCS, Config::DisplayPinDC, Config::DisplayPinRST);
-auto heightSensor = VL53L0X();
-
 auto screenManager = ScreenManager(&display);
 
 Bounce buttons[3];
@@ -83,13 +79,13 @@ void setup()
   initSequenceEnd();
 
 
-  State.CurrentTime = millis();
+  CurrentTime = millis();
 
 
   if (initialized)
   {
-    State.CurrentHeight = currentHeight;
-    controlSnapToPreset();
+//    Control.getCurrentHeight() = currentHeight;
+    Control.snapToPreset();
 
     screenManager.show<HomeScreen>();
   }
@@ -103,11 +99,7 @@ void setup()
 
 inline void setupHeightSensor()
 {
-  Wire.begin();
-
-  heightSensor.setTimeout(500);
-  auto error = heightSensor.init();
-
+  auto error = Control.init();
   if (error != VL53L0XInitResult::Success)
   {
     initSequenceError(InitSequenceStep::HeightSensorInit);
@@ -138,8 +130,6 @@ inline void setupHeightSensor()
     while(1);
   }
 
-  heightSensor.setMeasurementTimingBudget(Config::HeightSensorBudget);
-
   initSequenceSuccess(InitSequenceStep::HeightSensorInit);
 }
 
@@ -147,40 +137,6 @@ inline void setupHeightSensor()
 
 inline uint16_t testHeightSensor()
 {
-  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;
 }
 
 
@@ -190,23 +146,30 @@ inline uint16_t testHeightSensor()
   Loop
 
 */
-// Forward declarations
-void updateHeight();
-
-
-
 void loop()
 {
-  State.CurrentTime = millis();
+  CurrentTime = millis();
 
-  if (State.MoveDirection != Direction::None)
+
+  switch (Control.update())
   {
-    if (controlCheckOverCurrent())
+    case ControlUpdateResult::TargetReached:
+      screenManager.show<HomeScreen>();
+      break;
+
+    case ControlUpdateResult::SensorError:
+      screenManager.show<MoveSensorErrorScreen>();
+      break;
+
+    case ControlUpdateResult::OverCurrent:
       screenManager.show<MoveOverCurrentScreen>();
-    else
-      updateHeight();
+      break;
+
+    default:
+      break;
   }
 
+
   buttons[0].update();
   buttons[1].update();
   buttons[2].update();
@@ -226,40 +189,6 @@ void loop()
 
 
 
-uint32_t lastValidMeasurement;
-
-void updateHeight()
-{
-  uint16_t measurement;
-
-  if (heightSensorGetRange(&measurement))
-  {
-    State.CurrentHeight = measurement;
-    lastValidMeasurement = State.CurrentTime;
-
-    if (controlCheckTargetReached())
-      screenManager.show<HomeScreen>();
-  }
-  else if (State.CurrentTime - lastValidMeasurement >= Config::HeightMeasurementAbortTimeout)
-  {
-    dln("Out of range timeout!");
-    controlStop();
-
-    screenManager.show<MoveSensorErrorScreen>();
-  }
-}
-
-
-bool heightSensorGetRange(uint16_t* measurement)
-{
-  *measurement = heightSensor.readRangeSingleMillimeters();
-  dl("Range: "); dln(*measurement);
-
-  return *measurement <= Config::HeightMeasurementMax;
-}
-
-
-
 /*
 
   Helper functions for the status display during the initialization sequence