295 lines
5.8 KiB
C++
295 lines
5.8 KiB
C++
#include <SPI.h>
|
|
#include <Adafruit_GFX.h>
|
|
#include <Adafruit_ST7789.h>
|
|
#include <Bounce2.h>
|
|
|
|
#include "./include/config.h"
|
|
#include "./include/screenids.h"
|
|
#include "./lib/debug.h"
|
|
#include "./lib/settings.h"
|
|
#include "./lib/screen.h"
|
|
#include "./lib/vl53l0x.h"
|
|
#include "./lib/Control.h"
|
|
#include "./lib/motor.h"
|
|
#include "./lib/control.h"
|
|
#include "./lib/state.h"
|
|
#include "./lib/screen/home.h"
|
|
#include "./lib/screen/calibrate.h"
|
|
#include "./lib/screen/manual.h"
|
|
#include "./lib/screen/move-overcurrent.h"
|
|
#include "./lib/screen/move-sensorerror.h"
|
|
|
|
|
|
enum class InitSequenceStep
|
|
{
|
|
EEPROM = 0,
|
|
HeightSensorInit,
|
|
HeightSensorTest,
|
|
|
|
Last = HeightSensorTest
|
|
};
|
|
|
|
|
|
// Forward declarations
|
|
inline void setupHeightSensor();
|
|
inline void testHeightSensor();
|
|
|
|
void initSequenceStart();
|
|
void initSequenceSuccess(InitSequenceStep step);
|
|
void initSequenceError(InitSequenceStep step);
|
|
void initSequenceDisplayHeight(uint16_t measurement);
|
|
void initSequenceEnd();
|
|
|
|
|
|
auto display = Adafruit_ST7789(Config::DisplayPinCS, Config::DisplayPinDC, Config::DisplayPinRST);
|
|
auto screenManager = ScreenManager(&display);
|
|
|
|
Bounce buttons[3];
|
|
|
|
|
|
|
|
/*
|
|
|
|
Setup
|
|
|
|
*/
|
|
void setup()
|
|
{
|
|
DebugInit();
|
|
dln("[ MAIN ] Debug log started");
|
|
|
|
buttons[0].attach(Config::ButtonPinTop, INPUT_PULLUP);
|
|
buttons[1].attach(Config::ButtonPinMiddle, INPUT_PULLUP);
|
|
buttons[2].attach(Config::ButtonPinBottom, INPUT_PULLUP);
|
|
|
|
display.init(Config::DisplayWidth, Config::DisplayHeight, SPI_MODE3);
|
|
display.setRotation(Config::DisplayRotation);
|
|
|
|
screenManager.init();
|
|
|
|
initSequenceStart();
|
|
motorInit();
|
|
|
|
// Load settings from EEPROM
|
|
auto initialized = readSettings();
|
|
initSequenceSuccess(InitSequenceStep::EEPROM);
|
|
|
|
|
|
// Initialize VL53L0X sensor
|
|
CurrentTime = millis();
|
|
|
|
setupHeightSensor();
|
|
testHeightSensor();
|
|
|
|
initSequenceEnd();
|
|
|
|
|
|
CurrentTime = millis();
|
|
|
|
|
|
if (initialized)
|
|
{
|
|
Control.snapToPreset();
|
|
|
|
screenManager.show<HomeScreen>();
|
|
}
|
|
else
|
|
{
|
|
screenManager.show<CalibrateScreen>();
|
|
}
|
|
}
|
|
|
|
|
|
|
|
inline void setupHeightSensor()
|
|
{
|
|
auto error = Control.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);
|
|
}
|
|
|
|
initSequenceSuccess(InitSequenceStep::HeightSensorInit);
|
|
}
|
|
|
|
|
|
|
|
inline void testHeightSensor()
|
|
{
|
|
Control.stabilizeStart();
|
|
|
|
while (true)
|
|
{
|
|
if (Control.update() != ControlUpdateResult::SensorError)
|
|
{
|
|
initSequenceDisplayHeight(Control.getCurrentMeasurement());
|
|
|
|
if (Control.stabilized())
|
|
break;
|
|
}
|
|
else
|
|
initSequenceDisplayHeight(0);
|
|
|
|
CurrentTime = millis();
|
|
}
|
|
|
|
initSequenceSuccess(InitSequenceStep::HeightSensorTest);
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
Loop
|
|
|
|
*/
|
|
void loop()
|
|
{
|
|
CurrentTime = millis();
|
|
|
|
|
|
switch (Control.update())
|
|
{
|
|
case ControlUpdateResult::TargetReached:
|
|
switch (MoveReturnScreen)
|
|
{
|
|
case ScreenId::Manual:
|
|
screenManager.show<ManualScreen>();
|
|
break;
|
|
|
|
default:
|
|
screenManager.show<HomeScreen>();
|
|
break;
|
|
}
|
|
break;
|
|
|
|
case ControlUpdateResult::SensorError:
|
|
screenManager.show<MoveSensorErrorScreen>(ScreenId::MoveSensorError);
|
|
break;
|
|
|
|
case ControlUpdateResult::Overcurrent:
|
|
screenManager.show<MoveOvercurrentScreen>(ScreenId::MoveOvercurrent);
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
|
|
buttons[0].update();
|
|
buttons[1].update();
|
|
buttons[2].update();
|
|
|
|
if (buttons[0].fell())
|
|
{
|
|
//dln("[ MAIN ] Button pressed: Top");
|
|
screenManager.button(Button::Top);
|
|
}
|
|
|
|
if (buttons[1].fell())
|
|
{
|
|
//dln("[ MAIN ] Button pressed: Middle");
|
|
screenManager.button(Button::Middle);
|
|
}
|
|
|
|
if (buttons[2].fell())
|
|
{
|
|
//dln("[ MAIN ] Button pressed: Bottom");
|
|
screenManager.button(Button::Bottom);
|
|
}
|
|
|
|
screenManager.tick();
|
|
}
|
|
|
|
|
|
/*
|
|
|
|
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);
|
|
} |