DeskControl/src/main.cpp

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

#include "./include/display.h"
#include "./include/heightsensor.h"
#include "./include/colors.h"
#include "./lib/debug.h"
#include "./lib/persist.h"
#include "./lib/screen.h"
#include "./lib/vl53l0x.h"
#include "./lib/limits.h"
#include "./lib/state.h"
#include "./lib/screen/home.h"


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

void initSequenceStart();
void initSequenceSuccess(uint8_t which);
void initSequenceError(uint8_t which);
void initSequenceDisplayHeight(uint16_t measurement);
void initSequenceEnd();


#define INITSEQ_EEPROM 0
#define INITSEQ_HEIGHTSENSORINIT 1
#define INITSEQ_HEIGHTSENSORBUDGET 2
#define INITSEQ_HEIGHTSENSORTEST 3
#define INITSEQ_MAX INITSEQ_HEIGHTSENSORTEST



auto display = Adafruit_ST7789(DISPLAY_PORT_CS, DISPLAY_PORT_DC, DISPLAY_PORT_RST);
auto heightSensor = VL53L0X();

uint32_t currentTime = 0;

auto persist = Persist();
auto screenManager = ScreenManager(&display, &currentTime);


/*

  Setup

*/
void setup()
{
  DebugInit();

  pinMode(DISPLAY_PORT_BL, OUTPUT);
  digitalWrite(DISPLAY_PORT_BL, HIGH);

  display.init(DISPLAY_WIDTH, DISPLAY_HEIGHT, SPI_MODE3);
  display.setRotation(DISPLAY_ROTATION);

  initSequenceStart();

  // Load settings from EEPROM
  auto initialized = persist.init();
  initSequenceSuccess(INITSEQ_EEPROM);


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

  initSequenceEnd();


  if (initialized)
  {
    setCurrentHeight(currentHeight + persist.getHeightOffset());
    screenManager.show<HomeScreen>();
  }
  else
    // TODO show height configuration screen
    screenManager.show<HomeScreen>();
}



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

  VL53L0XResult result;
  if (!heightSensor.init(HEIGHTSENSOR_I2C_ADDRESS, &result))
  {
    initSequenceError(INITSEQ_HEIGHTSENSORINIT);

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

    while(1);
  }
  initSequenceSuccess(INITSEQ_HEIGHTSENSORINIT);


  VL53L0X_Error error;
  if (!heightSensor.setMeasurementTimingBudget(33000, &error))
  {
    initSequenceError(INITSEQ_HEIGHTSENSORBUDGET);

    display.print(result.error);

    while(1);
  }
  initSequenceSuccess(INITSEQ_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) && measurement <= HeightMeasurementMax)
    {
      initSequenceDisplayHeight(measurement);

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

      reference = 0;
      closeCount = 0;
    }

    delay(500);
  }

  initSequenceSuccess(INITSEQ_HEIGHTSENSORTEST);
  return reference / 10;
}



/*

  Loop

*/
void loop()
{
  // TODO
}




/*

  For display sleep:


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



/*

  Helper functions for the status display during the initialization sequence

*/
#define initSequenceTextSize 2

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


void initSequenceStart()
{
  display.fillScreen(COLOR_INITSEQ_BACKGROUND);
  display.setTextSize(initSequenceTextSize);

  display.setCursor(0, 0);
  display.setTextColor(COLOR_INITSEQ_TITLE);
  display.println("Initializing...");

  display.setTextColor(COLOR_INITSEQ_ITEMS, COLOR_INITSEQ_BACKGROUND);

  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(uint8_t which)
{

  display.drawChar(0, initSequenceTextY(which), 'v', COLOR_INITSEQ_SUCCESS, COLOR_INITSEQ_BACKGROUND, initSequenceTextSize);
}


void initSequenceError(uint8_t which)
{
  display.drawChar(0, initSequenceTextY(which), 'x', COLOR_INITSEQ_ERROR, COLOR_INITSEQ_BACKGROUND, initSequenceTextSize);
  display.setCursor(0, initSequenceTextY(INITSEQ_MAX + 2));
}


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

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


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