DeskControl/src/main.cpp

#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ST7789.h>

#include "./include/config.h"
#include "./lib/debug.h"
#include "./lib/settings.h"
#include "./lib/screen.h"
#include "./lib/vl53l0x.h"
#include "./lib/state.h"
#include "./lib/motor.h"
#include "./lib/screen/home.h"


enum class InitSequenceStep
{
  EEPROM = 0,
  HeightSensorInit = 1,
  HeightSensorBudget = 2,
  HeightSensorTest = 3,

  Last = HeightSensorTest
};


// Forward declarations
inline void setupHeightSensor();
inline uint8_t testHeightSensor();

void initSequenceStart();
void initSequenceSuccess(InitSequenceStep step);
void initSequenceError(InitSequenceStep step);
void initSequenceDisplayHeight(uint16_t measurement);
void initSequenceEnd();



auto display = Adafruit_ST7789(Config::DisplayPortCS, Config::DisplayPortDC, Config::DisplayPortRST);
auto heightSensor = VL53L0X();

auto screenManager = ScreenManager(&display);


/*

  Setup

*/
void setup()
{
  State.CurrentTime = millis();
  DebugInit();

  pinMode(Config::DisplayPortBL, OUTPUT);
  digitalWrite(Config::DisplayPortBL, HIGH);

  display.init(Config::DisplayWidth, Config::DisplayHeight, SPI_MODE3);
  display.setRotation(Config::DisplayRotation);

  initSequenceStart();

  // Load settings from EEPROM
  auto initialized = readSettings();
  initSequenceSuccess(InitSequenceStep::EEPROM);


  // Initialize VL53L0X sensor
  setupHeightSensor();
  auto currentHeight = testHeightSensor();

  initSequenceEnd();


  if (initialized)
  {
    State.CurrentHeight = currentHeight;
    screenManager.show<HomeScreen>();
  }
  else
    // TODO show height configuration screen
    screenManager.show<HomeScreen>();
}



inline void setupHeightSensor()
{
  Wire.begin();

  VL53L0XResult result;
  if (!heightSensor.init(Config::HeightSensorI2CAddress, &result))
  {
    initSequenceError(InitSequenceStep::HeightSensorInit);

    display.print(result.error);
    display.print(" @ ");
    display.print(result.position);

    while(1);
  }
  initSequenceSuccess(InitSequenceStep::HeightSensorInit);


  VL53L0X_Error error;
  if (!heightSensor.setMeasurementTimingBudget(33000, &error))
  {
    initSequenceError(InitSequenceStep::HeightSensorBudget);

    display.print(result.error);

    while(1);
  }
  initSequenceSuccess(InitSequenceStep::HeightSensorBudget);
}



inline uint8_t testHeightSensor()
{
  VL53L0X_Error error;
  uint16_t reference = 0;
  uint8_t closeCount = 0;

  while (closeCount < 3)
  {
    uint16_t measurement;

    if (heightSensor.getSingleRangingMeasurement(&measurement, &error, Config::HeightMeasurementMax))
    {
      initSequenceDisplayHeight(measurement);

      if (abs(measurement - reference) <= Config::HeightMeasurementDeltaStable)
        closeCount++;
      else
      {
        reference = measurement;
        closeCount = 0;
      }
    }
    else
    {
      initSequenceDisplayHeight(0);

      reference = 0;
      closeCount = 0;
    }

    delay(500);
  }

  initSequenceSuccess(InitSequenceStep::HeightSensorTest);
  return reference / 10;
}



/*

  Loop

*/
// Forward declarations
void updateHeight();



void loop()
{
  State.CurrentTime = millis();

  if (State.MoveDirection != Direction::None)
  {
    if (motorIsOverCurrent())
    {
      motorStop();
      State.MoveDirection = Direction::None;

      // TODO go to overcurrent screen
    }
    else
      updateHeight();
  }

  screenManager.tick();
}



uint32_t lastValidMeasurement;

void updateHeight()
{
  VL53L0X_Error error;
  uint16_t measurement;

  if (heightSensor.getSingleRangingMeasurement(&measurement, &error, Config::HeightMeasurementMax))
  {
    State.CurrentHeight = measurement;
    lastValidMeasurement = State.CurrentTime;


    // Check if we've reached the target
    switch (State.MoveDirection)
    {
      case Direction::Up:
        if (measurement >= State.MoveTarget - Config::HeightMeasurementDeltaStop)
        {
          if (measurement - State.MoveTarget <= Config::HeightMeasurementDeltaOnTarget)
            State.CurrentHeight = State.MoveTarget;

          motorStop();
          State.MoveDirection = Direction::None;
        }
        break;

      case Direction::Down:
        if (measurement <= State.MoveTarget + Config::HeightMeasurementDeltaStop)
        {
          if (State.MoveTarget - measurement <= Config::HeightMeasurementDeltaOnTarget)
            State.CurrentHeight = State.MoveTarget;

          motorStop();
          State.MoveDirection = Direction::None;
        }
        break;

      default:
        break;
    }
  }
  else if (State.CurrentTime - lastValidMeasurement >= Config::HeightMeasurementAbortTimeout)
  {
    motorStop();
    State.MoveDirection = Direction::None;

    // TODO go to height sensor error screen
  }
}


/*

  For display sleep:


    delay(1000);
    display.sendCommand(ST77XX_SLPIN);
    // toggle backlight pin


  For motor sleep toggle slp pin
    */



/*

  Helper functions for the status display during the initialization sequence

*/
#define initSequenceTextSize 2

// Default font is 5x7
#define initSequenceTextY(step) ((1 + (uint8_t)step) * ((7 + 3) * initSequenceTextSize))


void initSequenceStart()
{
  display.fillScreen(Config::ColorInitSeqBackground);
  display.setTextSize(initSequenceTextSize);

  display.setCursor(0, 0);
  display.setTextColor(Config::ColorInitSeqTitle);
  display.println("Initializing...");

  display.setTextColor(Config::ColorInitSeqItems, Config::ColorInitSeqBackground);

  display.setCursor(0, initSequenceTextY(0));
  display.print("  reading EEPROM");

  display.setCursor(0, initSequenceTextY(1));
  display.print("  height sensor");

  display.setCursor(0, initSequenceTextY(2));
  display.print("  timing budget");

  display.setCursor(0, initSequenceTextY(3));
  display.print("  sensor test");
}


void initSequenceSuccess(InitSequenceStep step)
{

  display.drawChar(0, initSequenceTextY(step), 'v', Config::ColorInitSeqSuccess, Config::ColorInitSeqBackground, initSequenceTextSize);
}


void initSequenceError(InitSequenceStep step)
{
  display.drawChar(0, initSequenceTextY(step), 'x', Config::ColorInitSeqError, Config::ColorInitSeqBackground, initSequenceTextSize);
  display.setCursor(0, initSequenceTextY(InitSequenceStep::Last + 2));
}


void initSequenceDisplayHeight(uint16_t measurement)
{
  display.setCursor(0, initSequenceTextY(InitSequenceStep::Last + 2));

  if (measurement > 0)
  {
    display.print(measurement);
    display.print("mm     ");
  }
  else
    display.print("-          ");
}


void initSequenceEnd()
{
  display.setTextSize(1);
  delay(1000);
}