Cleanup of the code

Got rid of the last bits of shouting defines

src/include/config.h

@@ -38,6 +38,7 @@ class Config
 
 
     static const uint16_t DisplayIdleTime = 10000;
+    static const uint16_t DisplayMoveRefreshRate = 1000;
 
 
     /*
@@ -57,6 +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
 
     // How far in advance to stop the motor
     static const uint8_t HeightMeasurementDeltaStop = 0;
@@ -96,9 +98,10 @@ class Config
     #define ColorDarkGray 0x528A
     #define ColorDarkerGray 0x2965
 
-    #define ColorSoftGreen 0x2BE7
-
     #define ColorStopRed 0xF9A6
+    #define ColorSoftGreen 0x2BE7
+    #define ColorSoftBlue 0x3376
+    #define ColorDarkBlue 0x0907
 
 
     static const uint16_t ColorInitSeqBackground = ColorBlack;
@@ -129,6 +132,16 @@ class Config
     static const uint16_t ColorMoveTarget = ColorWhite;
     static const uint16_t ColorMoveCurrent = ColorWhite;
     static const uint16_t ColorMoveStop = ColorStopRed;
+
+
+    static const uint16_t ColorMenuHeaderText = ColorWhite;
+    static const uint16_t ColorMenuHeaderBackground = ColorSoftBlue;
+
+    static const uint16_t ColorMenuSelectedText = ColorWhite;
+    static const uint16_t ColorMenuSelectedBackground = ColorDarkBlue;
+
+    static const uint16_t ColorCalibrateIndicators = ColorWhite;
+    static const uint16_t ColorCalibrateValue = ColorWhite;
 };
 
 #endif

src/include/metrics.h

@@ -0,0 +1,79 @@
+#ifndef __metrics
+#define __metrics
+
+#include <stdint.h>
+#include "fonts/FreeSansBold18pt7b.trimmed.h"
+
+class Metrics
+{
+  public:
+    /*
+
+      Small font
+        Uses the built-in Adafruit GFX 5x8 font.
+
+    */
+    static constexpr const GFXfont* SmallFont = nullptr;
+
+    static const uint16_t SmallFontBaseline = 0;
+    static const uint16_t SmallFontHeight = 8;
+    static const uint16_t SmallFontMaxHeight = Metrics::SmallFontHeight;
+
+
+    /*
+
+      Large font
+        Uses a trimmed version of the FreeSansBold 18pt font.
+
+    */
+    static constexpr const GFXfont* LargeFont = &FreeSansBold18pt7bTrimmed;
+
+    static const uint16_t LargeFontBaseline = 25;
+    static const uint16_t LargeFontHeight = Metrics::LargeFontBaseline;
+    static const uint16_t LargeFontMaxHeight = 42;
+
+
+
+    /*
+
+      Text line
+        Defines the height of a standard line of text including margins.
+        Suitable for applying a background color.
+
+    */
+    static const uint16_t LargeTextLineMargin = 7;
+    static const uint16_t LargeTextLineYOffset = Metrics::LargeFontBaseline + Metrics::LargeTextLineMargin;
+    static const uint16_t LargeTextLineHeight = (Metrics::LargeFontHeight + (2 * Metrics::LargeTextLineMargin));
+
+    static const uint16_t SmallTextLineMargin = 7;
+    static const uint16_t SmallTextLineHeight = (Metrics::SmallFontHeight + (2 * Metrics::SmallTextLineMargin));
+
+
+    /*
+
+      Arrows
+
+    */
+    static const uint16_t HArrowWidth = 10;
+    static const uint16_t HArrowHeight = 20;
+
+    static const uint16_t VArrowWidth = Metrics::HArrowHeight;
+    static const uint16_t VArrowHeight = Metrics::HArrowWidth;
+
+    static const uint16_t ArrowMargin = 8;
+
+    static const uint16_t LargeTextLineHArrowYOffset = (Metrics::LargeTextLineHeight - Metrics::HArrowHeight) / 2;
+    static const uint16_t LargeTextLineVArrowYOffset = (Metrics::LargeTextLineHeight - Metrics::VArrowHeight) / 2;
+
+
+
+    /*
+
+      Screen layout
+        Shared amongst screens for a consistent layout
+
+    */
+    static const uint16_t MiddleLargeTextLineY = 100;
+};
+
+#endif

src/lib/control.cpp

@@ -1,4 +1,4 @@
-#include "./motorstate.h"
+#include "./control.h"
 #include "./motor.h"
 #include "./state.h"
 #include "./settings.h"
@@ -6,18 +6,18 @@
 #include "include/config.h"
 
 
-void motorStateMoveTo(uint16_t height)
+void controlMoveTo(uint16_t height)
 {
-  dl("motorStateMoveTo: "); dln(height);
+  dl("controlMoveTo: "); dln(height);
 
   State.MoveTarget = height;
   State.MoveDirection = height > State.CurrentHeight ? Direction::Up : Direction::Down;
 }
 
 
-bool motorStateCheckTargetReached()
+bool controlCheckTargetReached()
 {
-  dl("motorStateCheckTargetReached: direction = "); dl((uint8_t)State.MoveDirection); dl(", currentHeight = "); dln(State.CurrentHeight);
+  dl("controlCheckTargetReached: direction = "); dl((uint8_t)State.MoveDirection); dl(", currentHeight = "); dln(State.CurrentHeight);
 
   switch (State.MoveDirection)
   {
@@ -27,7 +27,7 @@ bool motorStateCheckTargetReached()
         if (State.CurrentHeight - State.MoveTarget <= Config::HeightMeasurementDeltaOnTarget)
           State.CurrentHeight = State.MoveTarget;
 
-        motorStateStop();
+        controlStop();
         return true;
       }
       break;
@@ -38,7 +38,7 @@ bool motorStateCheckTargetReached()
         if (State.MoveTarget - State.CurrentHeight <= Config::HeightMeasurementDeltaOnTarget)
           State.CurrentHeight = State.MoveTarget;
 
-        motorStateStop();
+        controlStop();
         return true;
       }
       break;
@@ -51,13 +51,13 @@ bool motorStateCheckTargetReached()
 }
 
 
-bool motorStateCheckOverCurrent()
+bool controlCheckOverCurrent()
 {
   if (motorIsOverCurrent())
   {
-    dln("motorStateCheckOverCurrent: overcurrent detected!");
+    dln("controlCheckOverCurrent: overcurrent detected!");
 
-    motorStateStop();
+    controlStop();
     return true;
   }
 
@@ -65,15 +65,28 @@ bool motorStateCheckOverCurrent()
 }
 
 
-void motorStateStop()
+void controlStop()
 {
-  dln("motorStateStop");
+  dln("controlStop");
 
   motorStop();
   State.MoveDirection = Direction::None;
 }
 
 
+void controlSnapToPreset()
+{
+  for (uint8_t i = 0; i < 2; i++)
+  {
+    if (abs(State.CurrentHeight - Settings.Height.Preset[i]) <= Config::HeightMeasurementDeltaOnTarget)
+    {
+      State.CurrentHeight = Settings.Height.Preset[i];
+      break;
+    }
+  }
+}
+
+
 void getDisplayHeight(char* buffer, uint16_t value)
 {
   uint8_t displayValue = (value + Settings.Height.Offset) / 10;

src/lib/control.h

@@ -1,13 +1,15 @@
-#ifndef __motorstate
+#ifndef __control
-#define __motorstate
+#define __control
 
 #include <stdint.h>
 
 // High-level functions to control the motor and update the global state
-extern void motorStateMoveTo(uint16_t height);
+extern void controlMoveTo(uint16_t height);
-extern bool motorStateCheckTargetReached();
+extern bool controlCheckTargetReached();
-extern bool motorStateCheckOverCurrent();
+extern bool controlCheckOverCurrent();
-extern void motorStateStop();
+extern void controlStop();
+
+extern void controlSnapToPreset();
 
 
 // Formats a height value as "0.00m" (always exactly 5 characters long).

src/lib/screen.cpp

@@ -1,5 +1,6 @@
 #include "./screen.h"
 #include "include/config.h"
+#include "include/metrics.h"
 
 
 Adafruit_GFX* BaseScreen::getDisplay()
@@ -26,6 +27,36 @@ uint16_t BaseScreen::printCentered(const char* text, int16_t y)
 }
 
 
+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
+    color);
+}
+
+
+void BaseScreen::drawArrowUp(int16_t x, int16_t y, uint16_t color)
+{
+  this->getDisplay()->fillTriangle(
+    x + (Metrics::VArrowWidth / 2), y,                    // Top middle
+    x, y + Metrics::VArrowHeight,                         // Bottom left
+    x + Metrics::VArrowWidth, y + Metrics::VArrowHeight,  // Bottom right
+    color);
+}
+
+
+void BaseScreen::drawArrowDown(int16_t x, int16_t y, uint16_t color)
+{
+  this->getDisplay()->fillTriangle(
+    x, y,                                                       // Top left
+    x + Metrics::VArrowWidth, y,                                // Top right
+    x + (Metrics::VArrowWidth / 2), y + Metrics::VArrowHeight,  // Bottom middle
+    color);
+}
+
+
 void ScreenManager::init()
 {
   pinMode(Config::DisplayPinBL, OUTPUT);

src/lib/screen.h

@@ -36,6 +36,10 @@ class BaseScreen
 
     uint16_t printCentered(const char* text, int16_t y);
 
+    void drawArrowLeft(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;
 };

src/lib/screen/calibrate.cpp

@@ -3,7 +3,7 @@
 
 void CalibrateScreen::onShow()
 {
-  auto display = this->getDisplay();
+  //auto display = this->getDisplay();
 
   // TODO: implement CalibrateScreen
 }

src/lib/screen/home.cpp

@@ -1,31 +1,12 @@
 #include "./home.h"
 #include "./move.h"
-#include "fonts/FreeSansBold18pt7b.trimmed.h"
 #include "include/config.h"
+#include "include/metrics.h"
 #include "lib/settings.h"
-#include "lib/motorstate.h"
+#include "lib/control.h"
 #include "./menu.h"
 
 
-#define HOME_FONT_BASELINE 25
-#define HOME_FONT_HEIGHT 42
-
-#define PRESET_MARGIN 7
-
-// HOME_FONT_BASELINE is not a mistake, as there are no characters used which
-// go below the baseline.
-#define PRESET_LINEHEIGHT (HOME_FONT_BASELINE + (2 * PRESET_MARGIN))
-
-#define HOME_ARROW_WIDTH 10
-#define HOME_ARROW_HEIGHT 20
-#define HOME_ARROW_MARGIN 8
-#define HOME_ARROW_YOFFSET ((PRESET_LINEHEIGHT - HOME_ARROW_HEIGHT) / 2)
-
-#define HOME_INDICATOR_OFFSET (HOME_ARROW_WIDTH + (2 * HOME_ARROW_MARGIN))
-
-#define HOME_MENU_Y 100
-
-
 
 void HomeScreen::onShow()
 {
@@ -33,7 +14,7 @@ void HomeScreen::onShow()
 
   auto display = this->getDisplay();
 
-  display->setFont(&FreeSansBold18pt7bTrimmed);
+  display->setFont(Metrics::LargeFont);
   display->fillScreen(Config::ColorHomeBackground);
 
   this->drawPreset1();
@@ -52,19 +33,20 @@ void HomeScreen::onButton(Button button)
     this->idle = false;
     this->showTime = State.CurrentTime;
 
-    // TODO: should preset buttons activate immediately?
+    // Preset buttons activate immediately
-    return;
+    if (button == Button::Menu)
+      return;
   }
 
   switch (button)
   {
     case Button::Up:
-      motorStateMoveTo(Settings.Height.Preset[0]);
+      controlMoveTo(Settings.Height.Preset[0]);
       this->getScreenManager()->show<MoveScreen>();
       break;
 
     case Button::Down:
-      motorStateMoveTo(Settings.Height.Preset[1]);
+      controlMoveTo(Settings.Height.Preset[1]);
       this->getScreenManager()->show<MoveScreen>();
       break;
 
@@ -87,21 +69,20 @@ void HomeScreen::onTick()
 
 void HomeScreen::drawPreset1()
 {
-  //this->drawPreset(0, Settings.Height.Preset[0]);
+  this->drawPreset(0, Settings.Height.Preset[0]);
-  this->drawPreset(0, State.CurrentHeight);
 }
 
 
 void HomeScreen::drawPreset2()
 {
-  this->drawPreset(Config::DisplayHeight - PRESET_LINEHEIGHT, Settings.Height.Preset[1]);
+  this->drawPreset(Config::DisplayHeight - Metrics::LargeTextLineHeight, Settings.Height.Preset[1]);
 }
 
 
 void HomeScreen::drawNonPresetHeight()
 {
   auto display = this->getDisplay();
-  auto y = PRESET_LINEHEIGHT;
+  auto y = Metrics::LargeTextLineHeight;
 
   if (State.CurrentHeight != Settings.Height.Preset[0] &&
       State.CurrentHeight != Settings.Height.Preset[1])
@@ -112,16 +93,6 @@ void HomeScreen::drawNonPresetHeight()
 }
 
 
-void HomeScreen::drawArrow(int16_t y, uint16_t color)
-{
-  this->getDisplay()->fillTriangle(
-    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(int16_t y, uint16_t value)
 {
   auto display = this->getDisplay();
@@ -143,10 +114,10 @@ void HomeScreen::drawPreset(int16_t y, uint16_t value)
     arrowColor = Config::ColorPresetArrow;
   }
 
-  display->fillRect(0, y, Config::DisplayWidth, PRESET_LINEHEIGHT, backgroundColor);
+  display->fillRect(0, y, Config::DisplayWidth, Metrics::LargeTextLineHeight, backgroundColor);
 
   if (arrowColor)
-    this->drawArrow(y, arrowColor);
+    this->drawArrowLeft(Metrics::ArrowMargin, y + Metrics::LargeTextLineHArrowYOffset, arrowColor);
 
   display->setTextColor(textColor);
   this->drawHeight(y, value);
@@ -155,12 +126,10 @@ void HomeScreen::drawPreset(int16_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);
 
-  this->printCentered(&textValue[0], y + HOME_FONT_BASELINE + PRESET_MARGIN);
+  this->printCentered(&textValue[0], y + Metrics::LargeTextLineYOffset);
 }
 
 
@@ -168,8 +137,8 @@ void HomeScreen::drawMenu()
 {
   auto display = this->getDisplay();
 
-  this->drawArrow(HOME_MENU_Y, Config::ColorHomeMenuArrow);
+  this->drawArrowLeft(Metrics::ArrowMargin, Metrics::MiddleLargeTextLineY, Config::ColorHomeMenuArrow);
 
   display->setTextColor(Config::ColorHomeMenuText);
-  this->printCentered("Menu", HOME_MENU_Y + HOME_FONT_BASELINE + PRESET_MARGIN);
+  this->printCentered("Menu", Metrics::MiddleLargeTextLineY + Metrics::LargeTextLineYOffset);
 }

src/lib/screen/home.h

@@ -26,7 +26,6 @@ class HomeScreen : public BaseScreen
     void drawPreset2();
     void drawNonPresetHeight();
 
-    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();

src/lib/screen/manual.cpp

@@ -3,7 +3,7 @@
 
 void ManualScreen::onShow()
 {
-  auto display = this->getDisplay();
+  //auto display = this->getDisplay();
 
   // TODO: implement ManualScreen
 }

src/lib/screen/menu.cpp

@@ -3,7 +3,7 @@
 
 void MenuScreen::onShow()
 {
-  auto display = this->getDisplay();
+  //auto display = this->getDisplay();
 
   // TODO: implement MenuScreen
 }

src/lib/screen/move-overcurrent.cpp

@@ -3,7 +3,7 @@
 
 void MoveOverCurrentScreen::onShow()
 {
-  auto display = this->getDisplay();
+  //auto display = this->getDisplay();
 
   // TODO: implement MoveOverCurrentScreen
 }

src/lib/screen/move-sensorerror.cpp

@@ -3,7 +3,7 @@
 
 void MoveSensorErrorScreen::onShow()
 {
-  auto display = this->getDisplay();
+  //auto display = this->getDisplay();
 
   // TODO: implement MoveSensorErrorScreen
 }

src/lib/screen/move.cpp

@@ -1,45 +1,29 @@
 #include "./move.h"
 #include "./home.h"
-#include "fonts/FreeSansBold18pt7b.trimmed.h"
 #include "include/config.h"
+#include "include/metrics.h"
 #include "lib/settings.h"
-#include "lib/motorstate.h"
+#include "lib/control.h"
 
 
-#define MOVE_FONT_BASELINE 25
-#define MOVE_FONT_HEIGHT 42
-
-#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 = Metrics::LargeTextLineHeight;
-  auto startY = MOVE_LINE_HEIGHT;
+  auto arrowY = startY + Metrics::LargeTextLineHeight + Metrics::LargeTextLineVArrowYOffset;
-  auto arrowY = startY + MOVE_LINE_HEIGHT + MOVE_ARROW_YOFFSET;
+  auto arrowX = (Config::DisplayWidth - Metrics::VArrowWidth) / 2;
-  auto arrowX = (Config::DisplayWidth - MOVE_ARROW_WIDTH) / 2;
+  auto stopY = Config::DisplayHeight - Metrics::LargeTextLineHeight;
-  auto stopY = Config::DisplayHeight - MOVE_LINE_HEIGHT;
 
   display->fillScreen(Config::ColorMoveBackground);
 
   // Stop
-  display->setFont(nullptr);
+  display->setFont(Metrics::SmallFont);
   display->setTextColor(Config::ColorMoveStop);
-  this->printCentered("Press any button to", stopY - MOVE_STOP_SMALL_LINE_HEIGHT);
+  this->printCentered("Press any button to", stopY - Metrics::SmallTextLineHeight);
 
-  display->setFont(&FreeSansBold18pt7bTrimmed);
+  display->setFont(Metrics::LargeFont);
-  this->printCentered("STOP", stopY + MOVE_FONT_BASELINE + MOVE_LINE_MARGIN);
+  this->printCentered("STOP", stopY + Metrics::LargeTextLineYOffset);
 
   char targetHeightText[6];
   getDisplayHeight(&targetHeightText[0], State.MoveTarget);
@@ -48,27 +32,17 @@ void MoveScreen::onShow()
   // Target and arrow
   if (State.MoveDirection == Direction::Up)
   {
-    this->currentHeightY = startY + (MOVE_LINE_HEIGHT * 2);
+    this->currentHeightY = startY + (Metrics::LargeTextLineHeight * 2);
 
-    this->printCentered(&targetHeightText[0], startY + MOVE_FONT_BASELINE + MOVE_LINE_MARGIN);
+    this->printCentered(&targetHeightText[0], startY + Metrics::LargeTextLineYOffset);
-
+    this->drawArrowUp(arrowX, arrowY, Config::ColorMoveArrow);
-    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);
+    this->printCentered(&targetHeightText[0], startY + (Metrics::LargeTextLineHeight * 2) + Metrics::LargeTextLineYOffset);
-
+    this->drawArrowDown(arrowX, arrowY, Config::ColorMoveArrow);
-    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;
@@ -78,7 +52,7 @@ void MoveScreen::onShow()
 
 void MoveScreen::onButton(Button button)
 {
-  motorStateStop();
+  controlStop();
   this->getScreenManager()->show<HomeScreen>();
 }
 
@@ -93,7 +67,7 @@ void MoveScreen::onTick()
 
   // 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)
+  if (State.CurrentTime - this->lastRefresh >= Config::DisplayMoveRefreshRate)
   {
     this->drawCurrentHeight();
     this->lastRefresh = State.CurrentTime;
@@ -109,8 +83,8 @@ void MoveScreen::drawCurrentHeight()
   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->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 + MOVE_FONT_BASELINE);
+  this->lastTextWidth = this->printCentered(&currentHeightText[0], this->currentHeightY + Metrics::LargeFontBaseline);
 }

src/main.cpp

@@ -11,7 +11,7 @@
 #include "./lib/vl53l0x.h"
 #include "./lib/state.h"
 #include "./lib/motor.h"
-#include "./lib/motorstate.h"
+#include "./lib/control.h"
 #include "./lib/screen/home.h"
 #include "./lib/screen/calibrate.h"
 #include "./lib/screen/move-overcurrent.h"
@@ -85,13 +85,18 @@ void setup()
 
   State.CurrentTime = millis();
 
-  // TODO: check if close to either preset, then use the preset height
-  State.CurrentHeight = currentHeight;
 
   if (initialized)
+  {
+    State.CurrentHeight = currentHeight;
+    controlSnapToPreset();
+
     screenManager.show<HomeScreen>();
+  }
   else
+  {
     screenManager.show<CalibrateScreen>();
+  }
 }
 
 
@@ -146,8 +151,7 @@ inline uint16_t testHeightSensor()
   uint8_t closeCount = 0;
   uint16_t measurement;
 
-  // TODO: while (closeCount < 3)
+  while (true)
-  while (closeCount < 1)
   {
     if (heightSensorGetRange(&measurement))
     {
@@ -169,7 +173,10 @@ inline uint16_t testHeightSensor()
       closeCount = 0;
     }
 
-    delay(500);
+    if (closeCount < Config::HeightMeasurementDeltaStableCount)
+      delay(500);
+    else
+      break;
   }
 
   initSequenceSuccess(InitSequenceStep::HeightSensorTest);
@@ -194,7 +201,7 @@ void loop()
 
   if (State.MoveDirection != Direction::None)
   {
-    if (motorStateCheckOverCurrent())
+    if (controlCheckOverCurrent())
       screenManager.show<MoveOverCurrentScreen>();
     else
       updateHeight();
@@ -230,13 +237,13 @@ void updateHeight()
     State.CurrentHeight = measurement;
     lastValidMeasurement = State.CurrentTime;
 
-    if (motorStateCheckTargetReached())
+    if (controlCheckTargetReached())
       screenManager.show<HomeScreen>();
   }
   else if (State.CurrentTime - lastValidMeasurement >= Config::HeightMeasurementAbortTimeout)
   {
     dln("Out of range timeout!");
-    motorStateStop();
+    controlStop();
 
     screenManager.show<MoveSensorErrorScreen>();
   }