315 lines
6.2 KiB
C++
315 lines
6.2 KiB
C++
#include <SPI.h>
|
|
#include <Wire.h>
|
|
#include <Adafruit_GFX.h>
|
|
#include <Adafruit_ST7789.h>
|
|
#include <Bounce2.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/motorstate.h"
|
|
#include "./lib/screen/home.h"
|
|
|
|
|
|
enum class InitSequenceStep
|
|
{
|
|
EEPROM = 0,
|
|
HeightSensorInit = 1,
|
|
HeightSensorTest = 2,
|
|
|
|
Last = HeightSensorTest
|
|
};
|
|
|
|
|
|
// Forward declarations
|
|
inline void setupHeightSensor();
|
|
inline uint16_t testHeightSensor();
|
|
|
|
void initSequenceStart();
|
|
void initSequenceSuccess(InitSequenceStep step);
|
|
void initSequenceError(InitSequenceStep step);
|
|
void initSequenceDisplayHeight(uint16_t measurement);
|
|
void initSequenceEnd();
|
|
|
|
bool heightSensorGetRange(uint16_t* measurement);
|
|
|
|
|
|
auto display = Adafruit_ST7789(Config::DisplayPortCS, Config::DisplayPortDC, Config::DisplayPortRST);
|
|
auto heightSensor = VL53L0X();
|
|
|
|
auto screenManager = ScreenManager(&display);
|
|
|
|
Bounce buttons[3];
|
|
|
|
|
|
/*
|
|
|
|
Setup
|
|
|
|
*/
|
|
void setup()
|
|
{
|
|
DebugInit();
|
|
|
|
buttons[0].attach(Config::ButtonPortUp, INPUT_PULLUP);
|
|
buttons[1].attach(Config::ButtonPortMenu, INPUT_PULLUP);
|
|
buttons[2].attach(Config::ButtonPortDown, INPUT_PULLUP);
|
|
|
|
display.init(Config::DisplayWidth, Config::DisplayHeight, SPI_MODE3);
|
|
display.setRotation(Config::DisplayRotation);
|
|
|
|
screenManager.init();
|
|
|
|
initSequenceStart();
|
|
|
|
// Load settings from EEPROM
|
|
auto initialized = readSettings();
|
|
initSequenceSuccess(InitSequenceStep::EEPROM);
|
|
|
|
|
|
// Initialize VL53L0X sensor
|
|
setupHeightSensor();
|
|
auto currentHeight = testHeightSensor();
|
|
|
|
initSequenceEnd();
|
|
|
|
|
|
State.CurrentTime = millis();
|
|
|
|
if (initialized)
|
|
{
|
|
State.CurrentHeight = currentHeight;
|
|
screenManager.show<HomeScreen>();
|
|
}
|
|
else
|
|
// TODO show height configuration screen
|
|
screenManager.show<HomeScreen>();
|
|
}
|
|
|
|
|
|
|
|
inline void setupHeightSensor()
|
|
{
|
|
Wire.begin();
|
|
|
|
heightSensor.setTimeout(500);
|
|
auto error = heightSensor.init();
|
|
|
|
if (error != VL53L0XInitResult::Success)
|
|
{
|
|
initSequenceError(InitSequenceStep::HeightSensorInit);
|
|
|
|
switch (error)
|
|
{
|
|
case VL53L0XInitResult::InvalidIdentification:
|
|
display.print("Invalid identification");
|
|
break;
|
|
|
|
case VL53L0XInitResult::GetSpadInfoFailed:
|
|
display.print("GetSpadInfo failed");
|
|
break;
|
|
|
|
case VL53L0XInitResult::VHVCalibrationFailed:
|
|
display.print("VHV calibration failed");
|
|
break;
|
|
|
|
case VL53L0XInitResult::PhaseCalibrationFailed:
|
|
display.print("Phase calibration failed");
|
|
break;
|
|
|
|
default:
|
|
display.print("Unknown error");
|
|
break;
|
|
}
|
|
|
|
while(1);
|
|
}
|
|
|
|
heightSensor.setMeasurementTimingBudget(Config::HeightSensorBudget);
|
|
|
|
initSequenceSuccess(InitSequenceStep::HeightSensorInit);
|
|
}
|
|
|
|
|
|
|
|
inline uint16_t testHeightSensor()
|
|
{
|
|
uint16_t reference = 0;
|
|
uint8_t closeCount = 0;
|
|
uint16_t measurement;
|
|
|
|
while (closeCount < 3)
|
|
{
|
|
if (heightSensorGetRange(&measurement))
|
|
{
|
|
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;
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
Loop
|
|
|
|
*/
|
|
// Forward declarations
|
|
void updateHeight();
|
|
|
|
|
|
|
|
void loop()
|
|
{
|
|
State.CurrentTime = millis();
|
|
|
|
if (State.MoveDirection != Direction::None)
|
|
{
|
|
if (motorStateCheckOverCurrent())
|
|
{
|
|
// TODO go to overcurrent screen
|
|
}
|
|
else
|
|
updateHeight();
|
|
}
|
|
|
|
buttons[0].update();
|
|
buttons[1].update();
|
|
buttons[2].update();
|
|
|
|
|
|
if (buttons[0].rose())
|
|
screenManager.button(Button::Up);
|
|
|
|
if (buttons[1].rose())
|
|
screenManager.button(Button::Menu);
|
|
|
|
if (buttons[2].rose())
|
|
screenManager.button(Button::Down);
|
|
|
|
screenManager.tick();
|
|
}
|
|
|
|
|
|
|
|
uint32_t lastValidMeasurement;
|
|
|
|
void updateHeight()
|
|
{
|
|
uint16_t measurement;
|
|
|
|
if (heightSensorGetRange(&measurement))
|
|
{
|
|
State.CurrentHeight = measurement;
|
|
lastValidMeasurement = State.CurrentTime;
|
|
|
|
if (motorStateCheckTargetReached())
|
|
screenManager.show<HomeScreen>();
|
|
}
|
|
else if (State.CurrentTime - lastValidMeasurement >= Config::HeightMeasurementAbortTimeout)
|
|
{
|
|
motorStateStop();
|
|
|
|
// TODO go to height sensor error screen
|
|
}
|
|
}
|
|
|
|
|
|
bool heightSensorGetRange(uint16_t* measurement)
|
|
{
|
|
*measurement = heightSensor.readRangeSingleMillimeters();
|
|
return !heightSensor.timeoutOccurred() && *measurement <= Config::HeightMeasurementMax;
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
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(" 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);
|
|
} |