Persist calibration settings

Clarified height vs measurement values

src/lib/control.cpp

@@ -80,7 +80,7 @@ ControlUpdateResult ControlManager::update()
 
   if (measurement != VL53L0XRangeNotReady && measurement != VL53L0XRangeTimeout)
   {
-    this->currentHeight = measurement;
+    this->currentMeasurement = measurement;
     this->lastValidMeasurement = CurrentTime;
 
     // Check if target has been reached
@@ -144,15 +144,15 @@ void ControlManager::stabilizeStart()
   dln("[ CONTROL  ] Starting stabilization");
 
   this->stabilizationStart = CurrentTime;
-  this->stabilizeTarget = 0;
+  this->stabilizeTargetMeasurement = 0;
 }
 
 
 bool ControlManager::stabilized()
 {
-  if (this->stabilizeTarget == 0)
+  if (this->stabilizeTargetMeasurement == 0)
   {
-    this->stabilizeTarget = this->currentHeight;
+    this->stabilizeTargetMeasurement = this->currentMeasurement;
     this->stabilizationLastCheck = this->lastValidMeasurement;
     return false;
   }
@@ -164,7 +164,7 @@ bool ControlManager::stabilized()
   this->stabilizationLastCheck = this->lastValidMeasurement;
 
 
-  int16_t delta = this->currentHeight - this->stabilizeTarget;
+  int16_t delta = this->currentMeasurement - this->stabilizeTargetMeasurement;
   if (abs(delta) <= Config::HeightMeasurementDeltaStable)
   {
     this->stabilizeCount++;
@@ -179,7 +179,7 @@ bool ControlManager::stabilized()
   else
   {
     // If it's this much off, chances are the actual value is somewhere in the middle
-    this->stabilizeTarget += (delta / 2);
+    this->stabilizeTargetMeasurement += (delta / 2);
     this->stabilizeCount = 0;
   }
 
@@ -191,8 +191,19 @@ void ControlManager::moveStart(uint16_t height)
 {
   dl("[ CONTROL  ] Starting move to: "); dln(height);
 
-  this->moveTarget = height;
+  if (height < Settings.Height.Minimum)
-  this->moveDirection = height > this->currentHeight ? MoveDirection::Up : MoveDirection::Down;
+  {
+    dl("[ CONTROL  ] Target is below minimum, changing to: "); dln(Settings.Height.Minimum);
+    height = Settings.Height.Minimum;
+  }
+  else if (height > Settings.Height.Maximum)
+  {
+    dl("[ CONTROL  ] Target is above maximum, changing to: "); dln(Settings.Height.Maximum);
+    height = Settings.Height.Maximum;
+  }
+
+  this->moveTargetHeight = height;
+  this->moveDirection = height > this->getCurrentHeight() ? MoveDirection::Up : MoveDirection::Down;
 
   // Wait for a stable result
   this->stabilizeStart();
@@ -213,9 +224,9 @@ void ControlManager::snapToPreset()
 {
   for (uint8_t i = 0; i < 2; i++)
   {
-    if (abs(this->currentHeight - Settings.Height.Preset[i]) <= Config::HeightMeasurementDeltaOnTarget)
+    if (abs(this->getCurrentHeight() - Settings.Height.Preset[i]) <= Config::HeightMeasurementDeltaOnTarget)
     {
-      this->currentHeight = Settings.Height.Preset[i];
+      this->currentMeasurement = Settings.Height.Preset[i] - Settings.Height.Offset;
       break;
     }
   }
@@ -224,15 +235,17 @@ void ControlManager::snapToPreset()
 
 bool ControlManager::targetReached()
 {
+  auto currentHeight = this->getCurrentHeight();
+
   // 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)
+      if (currentHeight >= this->moveTargetHeight - Config::HeightMeasurementDeltaStop)
       {
         // Snap to the target
-        if (this->currentHeight - this->moveTarget <= Config::HeightMeasurementDeltaOnTarget)
+        if (currentHeight - this->moveTargetHeight <= Config::HeightMeasurementDeltaOnTarget)
-          this->currentHeight = this->moveTarget;
+          this->currentMeasurement = this->moveTargetHeight - Settings.Height.Offset;
 
         dln("[ CONTROL  ] Target reached");
         return true;
@@ -240,11 +253,11 @@ bool ControlManager::targetReached()
       break;
 
     case MoveDirection::Down:
-      if (this->currentHeight <= this->moveTarget + Config::HeightMeasurementDeltaStop)
+      if (currentHeight <= this->moveTargetHeight + Config::HeightMeasurementDeltaStop)
       {
         // Snap to the target
-        if (this->moveTarget - this->currentHeight <= Config::HeightMeasurementDeltaOnTarget)
+        if (this->moveTargetHeight - currentHeight <= Config::HeightMeasurementDeltaOnTarget)
-          this->currentHeight = this->moveTarget;
+          this->currentMeasurement = this->moveTargetHeight - Settings.Height.Offset;
 
         dln("[ CONTROL  ] Target reached");
         return true;
@@ -268,7 +281,7 @@ void ControlManager::getDisplayHeight(char* buffer, uint16_t value)
     return;
   }
 
-  uint8_t displayValue = (value + Settings.Height.Offset) / 10;
+  uint8_t displayValue = value / 10;
 
   if (displayValue > 99)
     buffer[0] = '0' + ((displayValue / 100) % 10);

src/lib/control.h

@@ -3,6 +3,7 @@
 
 #include <stdint.h>
 #include "./vl53l0x.h"
+#include "./settings.h"
 
 
 enum class ControlUpdateResult
@@ -47,26 +48,28 @@ class ControlManager
     void getDisplayHeight(char* buffer, uint16_t value);
 
 
-    uint16_t getCurrentHeight() { return this->currentHeight; }
+    uint16_t getCurrentHeight() { return this->currentMeasurement + Settings.Height.Offset; }
+    uint16_t getCurrentMeasurement() { return this->currentMeasurement; }
+
     uint16_t getLastMeasurement() { return this->lastMeasurement; }
     MoveDirection getMoveDirection() { return this->moveDirection; }
-    uint16_t getMoveTarget() { return this->moveTarget; }
+    uint16_t getMoveTargetHeight() { return this->moveTargetHeight; }
     bool getIsStabilizing() { return this->stabilizationStart != 0; }
 
   private:
     bool targetReached();
 
     VL53L0X heightSensor = VL53L0X();
-    uint16_t currentHeight;
+    uint16_t currentMeasurement;
     uint16_t lastMeasurement = 0;
     uint32_t lastValidMeasurement = 0;
 
     MoveDirection moveDirection;
-    uint16_t moveTarget;
+    uint16_t moveTargetHeight;
 
     uint32_t stabilizationStart = 0;
     uint8_t stabilizeCount = 0;
-    uint16_t stabilizeTarget = 0;
+    uint16_t stabilizeTargetMeasurement = 0;
     uint32_t stabilizationLastCheck = 0;
 };
 

src/lib/screen/calibrate.cpp

@@ -12,9 +12,9 @@ void CalibrateScreen::onShow()
   this->display->fillScreen(Config::ColorHomeBackground);
 
   this->drawTitle();
-  this->height[CalibrateStepCurrent] = Control.getCurrentHeight() + Settings.Height.Offset;
+  this->height[CalibrateStepCurrent] = Control.getCurrentHeight();
-  this->height[CalibrateStepMax] = 1969; // TODO: read min/max from settings
+  this->height[CalibrateStepMax] = Settings.Height.Maximum;
-  this->height[CalibrateStepMin] = 0;
+  this->height[CalibrateStepMin] = Settings.Height.Minimum;
 
   this->drawArrowUp(Metrics::ArrowMargin, Metrics::LargeTextLineHeight + Metrics::LargeTextLineVArrowYOffset, Config::ColorCalibrateIndicators);
   this->drawArrowRight(Metrics::ArrowMargin, Metrics::MiddleLargeTextLineY + Metrics::LargeTextLineHArrowYOffset, Config::ColorCalibrateIndicators);
@@ -87,7 +87,11 @@ void CalibrateScreen::onButton(Button button)
       this->step = (CalibrateStep)(this->step + 1);
       if (this->step == CalibrateStepCount)
       {
-        // TODO: store new settings
+        Settings.Height.Offset = this->height[CalibrateStepCurrent] - Control.getCurrentMeasurement();
+        Settings.Height.Maximum = this->height[CalibrateStepMax];
+        Settings.Height.Minimum = this->height[CalibrateStepMin];
+        writeSettingsHeights();
+
         this->screenManager->show<HomeScreen>();
       }
       else
@@ -112,7 +116,7 @@ bool CalibrateScreen::isValidHeight()
   switch (this->step)
   {
     case CalibrateStepCurrent:
-      return this->height[CalibrateStepCurrent] >= Control.getCurrentHeight();
+      return this->height[CalibrateStepCurrent] >= Control.getCurrentMeasurement();
 
     case CalibrateStepMin:
       return this->height[CalibrateStepMin] <= this->height[CalibrateStepCurrent];

src/lib/screen/home.cpp

@@ -29,11 +29,12 @@ void HomeScreen::onShow()
 
 void HomeScreen::onButton(Button button)
 {
+  this->showTime = CurrentTime;
+
   if (this->idle)
   {
     this->screenManager->displayOn();
     this->idle = false;
-    this->showTime = CurrentTime;
 
     // Preset buttons activate immediately
     if (button == Button::Middle)
@@ -58,8 +59,11 @@ void HomeScreen::onButton(Button button)
   }
 
 
-  Control.moveStart(targetHeight);
+  if (targetHeight > 0)
-  this->screenManager->show<MoveScreen>();
+  {
+    Control.moveStart(targetHeight);
+    this->screenManager->show<MoveScreen>();
+  }
 }
 
 
@@ -139,7 +143,7 @@ void HomeScreen::drawHeight(int16_t y, uint16_t value)
 
 void HomeScreen::drawMenu()
 {
-  this->drawArrowLeft(Metrics::ArrowMargin, Metrics::MiddleLargeTextLineY, Config::ColorHomeMenuArrow);
+  this->drawArrowLeft(Metrics::ArrowMargin, Metrics::MiddleLargeTextLineY + Metrics::LargeTextLineHArrowYOffset, Config::ColorHomeMenuArrow);
 
   this->display->setTextColor(Config::ColorHomeMenuText);
   this->printCentered("Menu", Metrics::MiddleLargeTextLineY + Metrics::LargeTextLineYOffset);

src/lib/screen/menu.cpp

@@ -1,14 +1,23 @@
 #include "./menu.h"
+#include "./home.h"
+#include "include/metrics.h"
 
 
 void MenuScreen::onShow()
 {
-  // TODO: implement MenuScreen
+  this->display->setFont(Metrics::LargeFont);
+  this->display->setTextSize(Metrics::LargeFontTextSize);
+
+  this->display->fillScreen(Config::ColorHomeBackground);
+  this->drawLargeTextLineCentered("Menu", 0, Config::ColorMenuHeaderText, Config::ColorMenuHeaderBackground);
+
+// TODO: implement MenuScreen
 }
 
 
 void MenuScreen::onButton(Button button)
 {
+  this->screenManager->show<HomeScreen>();
 }
 
 

src/lib/screen/move.cpp

@@ -29,7 +29,7 @@ void MoveScreen::onShow()
   this->printCentered("STOP", stopY + Metrics::LargeTextLineYOffset);
 
   char targetHeightText[6];
-  Control.getDisplayHeight(&targetHeightText[0], Control.getMoveTarget());
+  Control.getDisplayHeight(&targetHeightText[0], Control.getMoveTargetHeight());
   this->display->setTextColor(Config::ColorMoveCurrent);
 
   // Target and arrow

src/lib/settings.cpp

@@ -16,6 +16,8 @@ struct SettingsHeader
 
 #define SettingsHeightsAddress sizeof(SettingsHeader)
 
+bool initialized = false;
+
 
 // Forward declarations
 void readHeights();
@@ -33,6 +35,7 @@ bool readSettings()
     if (header.version >= 1)
         readHeights();
 
+    initialized = true;
     return true;
   }
 
@@ -40,6 +43,24 @@ bool readSettings()
 }
 
 
+bool allowPartialWrite()
+{
+  if (initialized)
+    return true;
+
+  initialized = true;
+
+  SettingsHeader header;
+  header.identifier = SettingsIdentifier;
+  header.version = SettingsVersion1;
+
+  EEPROM.put(0, header);
+  writeSettingsHeights();
+
+  return false;
+}
+
+
 void readHeights()
 {
   EEPROM.get(SettingsHeightsAddress, Settings.Height);
@@ -48,5 +69,6 @@ void readHeights()
 
 void writeSettingsHeights()
 {
-  EEPROM.put(SettingsHeightsAddress, Settings.Height);
+  if (allowPartialWrite())
+    EEPROM.put(SettingsHeightsAddress, Settings.Height);
 }

src/main.cpp

@@ -146,7 +146,7 @@ inline void testHeightSensor()
   {
     if (Control.update() != ControlUpdateResult::SensorError)
     {
-      initSequenceDisplayHeight(Control.getCurrentHeight());
+      initSequenceDisplayHeight(Control.getCurrentMeasurement());
 
       if (Control.stabilized())
         break;