Much refactoring, wow.

Mostly the state/control situation
This commit is contained in:
Mark van Renswoude 2020-01-31 16:50:19 +01:00
parent 8b5e23cecb
commit 1ca611e504
18 changed files with 348 additions and 200 deletions

View File

@ -2,6 +2,6 @@
REM It is a nuisance to keep the filter up-to-date with whatever is printed
REM in the actual source, but the space savings are worth it.
AdafruitGFXFontTrim [ 0-9\.mSTOPMenursaybto] FreeSansBold18pt7b.h
AdafruitGFXFontTrim [ 0-9\.mSTOPMenuER] FreeSansBold18pt7b.h
pause

View File

@ -58,7 +58,7 @@ class Config
// How much the measurements can change to still be considered "stable"
static const uint8_t HeightMeasurementDeltaStable = 10;
static const uint8_t HeightMeasurementDeltaStableCount = 1; // TODO: restore StableCount to 3
static const uint8_t HeightMeasurementDeltaStableCount = 3;
// How far in advance to stop the motor
static const uint8_t HeightMeasurementDeltaStop = 0;
@ -104,12 +104,15 @@ class Config
#define ColorDarkBlue 0x0907
// Init sequence
static const uint16_t ColorInitSeqBackground = ColorBlack;
static const uint16_t ColorInitSeqTitle = ColorYellow;
static const uint16_t ColorInitSeqItems = ColorWhite;
static const uint16_t ColorInitSeqSuccess = ColorGreen;
static const uint16_t ColorInitSeqError = ColorRed;
// Home
static const uint16_t ColorHomeBackground = ColorBlack;
static const uint16_t ColorHomeMenuText = ColorWhite;
@ -126,6 +129,7 @@ class Config
static const uint16_t ColorPresetSelectedBackground = ColorSoftGreen;
// Move
static const uint16_t ColorMoveBackground = ColorBlack;
static const uint16_t ColorMoveArrow = ColorDarkGray;
@ -134,6 +138,11 @@ class Config
static const uint16_t ColorMoveStop = ColorStopRed;
// Move error / overcurrent
static const uint16_t ColorMoveErrorText = ColorStopRed;
// Menu
static const uint16_t ColorMenuHeaderText = ColorWhite;
static const uint16_t ColorMenuHeaderBackground = ColorSoftBlue;

View File

@ -1,31 +1,118 @@
#include "./control.h"
#include <Wire.h>
#include "./motor.h"
#include "./state.h"
#include "./Control.h"
#include "./settings.h"
#include "./debug.h"
#include "include/config.h"
void controlMoveTo(uint16_t height)
{
dl("controlMoveTo: "); dln(height);
ControlManager Control = ControlManager();
State.MoveTarget = height;
State.MoveDirection = height > State.CurrentHeight ? Direction::Up : Direction::Down;
VL53L0XInitResult ControlManager::init()
{
Wire.begin();
this->heightSensor.setTimeout(500);
auto result = this->heightSensor.init();
if (result != VL53L0XInitResult::Success)
return result;
this->heightSensor.setMeasurementTimingBudget(Config::HeightSensorBudget);
return result;
}
ControlUpdateResult ControlManager::update()
{
}
void ControlManager::stabilizeStart()
{
this->stabilizing = true;
}
bool ControlManager::stabilized()
{
// TODO: stabilized
}
void ControlManager::movePrepare(uint16_t height)
{
this->moveTarget = height;
this->moveDirection = height > this->currentHeight ? MoveDirection::Up : MoveDirection::Down;
}
bool ControlManager::moveStart()
{
// TODO: moveStart - wait for stable / timeout
this->moveDirection = MoveDirection::None;
this->stabilizeStart();
//this->sensorError = true;
return false;
motorStart(this->moveDirection == MoveDirection::Up ? MotorDirection::Up : MotorDirection::Down);
return true;
}
void ControlManager::moveStop()
{
motorStop();
this->moveDirection = MoveDirection::None;
}
void ControlManager::snapToPreset()
{
for (uint8_t i = 0; i < 2; i++)
{
if (abs(this->currentHeight - Settings.Height.Preset[i]) <= Config::HeightMeasurementDeltaOnTarget)
{
this->currentHeight = Settings.Height.Preset[i];
break;
}
}
}
void ControlManager::getDisplayHeight(char* buffer, uint16_t value)
{
uint8_t displayValue = (value + Settings.Height.Offset) / 10;
if (displayValue > 99)
buffer[0] = '0' + ((displayValue / 100) % 10);
else
buffer[0] = '0';
buffer[1] = '.';
buffer[2] = '0' + ((displayValue / 10) % 10);
buffer[3] = '0' + (displayValue % 10);
buffer[4] = 'm';
buffer[5] = 0;
}
/*
bool controlCheckTargetReached()
{
dl("controlCheckTargetReached: direction = "); dl((uint8_t)State.MoveDirection); dl(", currentHeight = "); dln(State.CurrentHeight);
dl("controlCheckTargetReached: direction = "); dl((uint8_t)Control.getMoveDirection()); dl(", currentHeight = "); dln(Control.getCurrentHeight());
switch (State.MoveDirection)
switch (Control.getMoveDirection())
{
case Direction::Up:
if (State.CurrentHeight >= State.MoveTarget - Config::HeightMeasurementDeltaStop)
if (Control.getCurrentHeight() >= Control.getMoveTarget() - Config::HeightMeasurementDeltaStop)
{
if (State.CurrentHeight - State.MoveTarget <= Config::HeightMeasurementDeltaOnTarget)
State.CurrentHeight = State.MoveTarget;
if (Control.getCurrentHeight() - Control.getMoveTarget() <= Config::HeightMeasurementDeltaOnTarget)
Control.getCurrentHeight() = Control.getMoveTarget();
controlStop();
return true;
@ -33,10 +120,10 @@ bool controlCheckTargetReached()
break;
case Direction::Down:
if (State.CurrentHeight <= State.MoveTarget + Config::HeightMeasurementDeltaStop)
if (Control.getCurrentHeight() <= Control.getMoveTarget() + Config::HeightMeasurementDeltaStop)
{
if (State.MoveTarget - State.CurrentHeight <= Config::HeightMeasurementDeltaOnTarget)
State.CurrentHeight = State.MoveTarget;
if (Control.getMoveTarget() - Control.getCurrentHeight() <= Config::HeightMeasurementDeltaOnTarget)
Control.getCurrentHeight() = Control.getMoveTarget();
controlStop();
return true;
@ -65,40 +152,95 @@ bool controlCheckOverCurrent()
}
void controlStop()
{
dln("controlStop");
motorStop();
State.MoveDirection = Direction::None;
if (Control.getMoveDirection() != Direction::None)
{
if (controlCheckOverCurrent())
screenManager.show<MoveOverCurrentScreen>();
else
updateHeight();
}
else if (Control.SensorError)
updateHeight();
void updateHeight()
{
uint16_t measurement;
if (heightSensorGetRange(&measurement))
{
Control.getCurrentHeight() = measurement;
if (Control.getMoveDirection() != Direction::None)
{
lastValidMeasurement = CurrentTime;
if (controlCheckTargetReached())
screenManager.show<HomeScreen>();
}
}
else if (Control.getMoveDirection() != Direction::None && CurrentTime - lastValidMeasurement >= Config::HeightMeasurementAbortTimeout)
{
dln("Out of range timeout!");
Control.moveStop();
screenManager.show<MoveSensorErrorScreen>();
}
}
void controlSnapToPreset()
bool heightSensorGetRange(uint16_t* measurement)
{
for (uint8_t i = 0; i < 2; i++)
*measurement = heightSensor.readRangeSingleMillimeters();
dl("Range: "); dln(*measurement);
return *measurement <= Config::HeightMeasurementMax;
}
uint16_t reference = 0;
uint8_t closeCount = 0;
uint16_t measurement;
while (true)
{
if (abs(State.CurrentHeight - Settings.Height.Preset[i]) <= Config::HeightMeasurementDeltaOnTarget)
if (heightSensorGetRange(&measurement))
{
State.CurrentHeight = Settings.Height.Preset[i];
initSequenceDisplayHeight(measurement);
if (abs(measurement - reference) <= Config::HeightMeasurementDeltaStable)
closeCount++;
else
{
reference = measurement;
closeCount = 0;
}
}
else
{
initSequenceDisplayHeight(0);
reference = 0;
closeCount = 0;
}
if (closeCount < Config::HeightMeasurementDeltaStableCount)
delay(500);
else
break;
}
}
}
initSequenceSuccess(InitSequenceStep::HeightSensorTest);
return reference;
void getDisplayHeight(char* buffer, uint16_t value)
{
uint8_t displayValue = (value + Settings.Height.Offset) / 10;
if (displayValue > 99)
buffer[0] = '0' + ((displayValue / 100) % 10);
else
buffer[0] = '0';
buffer[1] = '.';
buffer[2] = '0' + ((displayValue / 10) % 10);
buffer[3] = '0' + (displayValue % 10);
buffer[4] = 'm';
buffer[5] = 0;
}
*/

View File

@ -2,20 +2,68 @@
#define __control
#include <stdint.h>
// High-level functions to control the motor and update the global state
extern void controlMoveTo(uint16_t height);
extern bool controlCheckTargetReached();
extern bool controlCheckOverCurrent();
extern void controlStop();
extern void controlSnapToPreset();
#include "./vl53l0x.h"
// Formats a height value as "0.00m" (always exactly 5 characters long).
// Buffer must be at least 6 bytes long, a null character is added.
// The value is the raw height sensor value, the offset is added by this function.
extern void getDisplayHeight(char* buffer, uint16_t value);
enum class ControlUpdateResult
{
Idle = 0,
Moving = 1,
TargetReached = 2,
SensorError = 3,
OverCurrent = 4
};
enum class MoveDirection
{
None = 0,
Up = 1,
Down = 2
};
class ControlManager
{
public:
VL53L0XInitResult init();
ControlUpdateResult update();
void stabilizeStart();
bool stabilized();
void movePrepare(uint16_t height);
bool moveStart();
void moveStop();
void snapToPreset();
// Formats a height value as "0.00m" (always exactly 5 characters long).
// Buffer must be at least 6 bytes long, a null character is added.
// The value is the raw height sensor value, the offset is added by this function.
void getDisplayHeight(char* buffer, uint16_t value);
uint16_t getCurrentHeight() { return this->currentHeight; }
MoveDirection getMoveDirection() { return this->moveDirection; }
uint16_t getMoveTarget() { return this->moveTarget; }
private:
VL53L0X heightSensor = VL53L0X();
uint16_t currentHeight;
MoveDirection moveDirection;
uint16_t moveTarget;
bool stabilizing = false;
uint32_t lastValidMeasurement = 0;
};
extern ControlManager Control;
#endif

View File

@ -2,7 +2,7 @@
#define __screen_calibrate
#include "../screen.h"
#include "../state.h"
#include "../control.h"
/*

View File

@ -4,13 +4,14 @@
#include "include/metrics.h"
#include "lib/settings.h"
#include "lib/control.h"
#include "lib/state.h"
#include "./menu.h"
void HomeScreen::onShow()
{
this->showTime = State.CurrentTime;
this->showTime = CurrentTime;
auto display = this->getDisplay();
@ -31,7 +32,7 @@ void HomeScreen::onButton(Button button)
{
this->getScreenManager()->displayOn();
this->idle = false;
this->showTime = State.CurrentTime;
this->showTime = CurrentTime;
// Preset buttons activate immediately
if (button == Button::Menu)
@ -41,13 +42,15 @@ void HomeScreen::onButton(Button button)
switch (button)
{
case Button::Up:
controlMoveTo(Settings.Height.Preset[0]);
Control.movePrepare(Settings.Height.Preset[0]);
this->getScreenManager()->show<MoveScreen>();
Control.moveStart();
break;
case Button::Down:
controlMoveTo(Settings.Height.Preset[1]);
Control.movePrepare(Settings.Height.Preset[1]);
this->getScreenManager()->show<MoveScreen>();
Control.moveStart();
break;
case Button::Menu:
@ -59,7 +62,7 @@ void HomeScreen::onButton(Button button)
void HomeScreen::onTick()
{
if (!this->idle && State.CurrentTime - this->showTime >= Config::DisplayIdleTime)
if (!this->idle && CurrentTime - this->showTime >= Config::DisplayIdleTime)
{
this->getScreenManager()->displayOff();
this->idle = true;
@ -84,11 +87,11 @@ void HomeScreen::drawNonPresetHeight()
auto display = this->getDisplay();
auto y = Metrics::LargeTextLineHeight;
if (State.CurrentHeight != Settings.Height.Preset[0] &&
State.CurrentHeight != Settings.Height.Preset[1])
if (Control.getCurrentHeight() != Settings.Height.Preset[0] &&
Control.getCurrentHeight() != Settings.Height.Preset[1])
{
display->setTextColor(Config::ColorNonPresetText);
this->drawHeight(y, State.CurrentHeight);
this->drawHeight(y, Control.getCurrentHeight());
}
}
@ -101,7 +104,7 @@ void HomeScreen::drawPreset(int16_t y, uint16_t value)
uint16_t arrowColor;
// An exact comparison is enough here, the movement code takes care of that if it's "close enough"
if (value == State.CurrentHeight)
if (value == Control.getCurrentHeight())
{
textColor = Config::ColorPresetSelectedText;
backgroundColor = Config::ColorPresetSelectedBackground;
@ -127,7 +130,7 @@ void HomeScreen::drawPreset(int16_t y, uint16_t value)
void HomeScreen::drawHeight(int16_t y, uint16_t value)
{
char textValue[6];
getDisplayHeight(&textValue[0], value);
Control.getDisplayHeight(&textValue[0], value);
this->printCentered(&textValue[0], y + Metrics::LargeTextLineYOffset);
}

View File

@ -2,7 +2,7 @@
#define __screen_home
#include "../screen.h"
#include "../state.h"
#include "../Control.h"
/*

View File

@ -2,7 +2,7 @@
#define __screen_manual
#include "../screen.h"
#include "../state.h"
#include "../Control.h"
/*

View File

@ -2,7 +2,7 @@
#define __screen_menu
#include "../screen.h"
#include "../state.h"
#include "../Control.h"
/*

View File

@ -2,7 +2,7 @@
#define __screen_move_overcurrent
#include "../screen.h"
#include "../state.h"
#include "../Control.h"
/*

View File

@ -1,11 +1,31 @@
#include "./move-sensorerror.h"
#include "include/config.h"
#include "include/metrics.h"
#include "lib/control.h"
#include "lib/state.h"
void MoveSensorErrorScreen::onShow()
{
//auto display = this->getDisplay();
auto display = this->getDisplay();
auto y = Metrics::LargeTextLineHeight + Metrics::LargeTextLineYOffset;
// TODO: implement MoveSensorErrorScreen
display->setFont(Metrics::LargeFont);
display->setTextColor(Config::ColorMoveErrorText);
this->printCentered("ERROR", y);
y += Metrics::LargeTextLineHeight;
display->setFont(Metrics::SmallFont);
this->printCentered("height sensor failed", y);
y += Metrics::SmallTextLineHeight;
this->printCentered("waiting for stable value", y);
y += Metrics::SmallTextLineHeight + Metrics::LargeTextLineHeight;
this->currentHeightY = y;
this->lastRefresh = CurrentTime;
this->drawCurrentHeight();
}
@ -17,3 +37,18 @@ void MoveSensorErrorScreen::onButton(Button button)
void MoveSensorErrorScreen::onTick()
{
}
void MoveSensorErrorScreen::drawCurrentHeight()
{
auto display = this->getDisplay();
char currentHeightText[6];
Control.getDisplayHeight(&currentHeightText[0], Control.getCurrentHeight());
if (this->lastTextWidth > 0)
display->fillRect((Config::DisplayWidth - this->lastTextWidth) / 2, this->currentHeightY, this->lastTextWidth, Metrics::LargeTextLineHeight, Config::ColorMoveBackground);
display->setTextColor(Config::ColorMoveTarget);
this->lastTextWidth = this->printCentered(&currentHeightText[0], this->currentHeightY + Metrics::LargeFontBaseline);
}

View File

@ -2,7 +2,7 @@
#define __screen_move_sensorerror
#include "../screen.h"
#include "../state.h"
#include "../Control.h"
/*
@ -20,6 +20,11 @@ class MoveSensorErrorScreen : public BaseScreen
void onTick();
private:
uint32_t lastRefresh;
uint8_t currentHeightY;
uint16_t lastTextWidth = 0;
void drawCurrentHeight();
};
#endif

View File

@ -4,7 +4,7 @@
#include "include/metrics.h"
#include "lib/settings.h"
#include "lib/control.h"
#include "lib/state.h"
void MoveScreen::onShow()
@ -26,11 +26,11 @@ void MoveScreen::onShow()
this->printCentered("STOP", stopY + Metrics::LargeTextLineYOffset);
char targetHeightText[6];
getDisplayHeight(&targetHeightText[0], State.MoveTarget);
Control.getDisplayHeight(&targetHeightText[0], Control.getMoveTarget());
display->setTextColor(Config::ColorMoveCurrent);
// Target and arrow
if (State.MoveDirection == Direction::Up)
if (Control.getMoveDirection() == MoveDirection::Up)
{
this->currentHeightY = startY + (Metrics::LargeTextLineHeight * 2);
@ -45,32 +45,26 @@ void MoveScreen::onShow()
this->drawArrowDown(arrowX, arrowY, Config::ColorMoveArrow);
}
this->lastRefresh = State.CurrentTime;
this->lastRefresh = CurrentTime;
this->drawCurrentHeight();
}
void MoveScreen::onButton(Button button)
{
controlStop();
Control.moveStop();
this->getScreenManager()->show<HomeScreen>();
}
void MoveScreen::onTick()
{
if (State.MoveDirection == Direction::None)
{
this->getScreenManager()->show<HomeScreen>();
return;
}
// Don't update every tick, monitoring the current height is more
// important and the flicker would be unpleasant as well.
if (State.CurrentTime - this->lastRefresh >= Config::DisplayMoveRefreshRate)
if (CurrentTime - this->lastRefresh >= Config::DisplayMoveRefreshRate)
{
this->drawCurrentHeight();
this->lastRefresh = State.CurrentTime;
this->lastRefresh = CurrentTime;
}
}
@ -80,7 +74,7 @@ void MoveScreen::drawCurrentHeight()
auto display = this->getDisplay();
char currentHeightText[6];
getDisplayHeight(&currentHeightText[0], State.CurrentHeight);
Control.getDisplayHeight(&currentHeightText[0], Control.getCurrentHeight());
if (this->lastTextWidth > 0)
display->fillRect((Config::DisplayWidth - this->lastTextWidth) / 2, this->currentHeightY, this->lastTextWidth, Metrics::LargeTextLineHeight, Config::ColorMoveBackground);

View File

@ -2,7 +2,7 @@
#define __screen_move
#include "../screen.h"
#include "../state.h"
#include "../Control.h"
/*

View File

@ -1,3 +1,3 @@
#include "state.h"
#include "Control.h"
StateContainer State;
uint32_t CurrentTime;

View File

@ -3,23 +3,6 @@
#include "stdint.h"
enum class Direction
{
None = 0,
Up = 1,
Down = 2
};
struct StateContainer
{
uint32_t CurrentTime;
uint16_t CurrentHeight;
Direction MoveDirection;
uint16_t MoveTarget;
};
extern StateContainer State;
extern uint32_t CurrentTime;
#endif

View File

@ -1,5 +1,4 @@
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ST7789.h>
#include <Bounce2.h>
@ -9,9 +8,10 @@
#include "./lib/settings.h"
#include "./lib/screen.h"
#include "./lib/vl53l0x.h"
#include "./lib/state.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/move-overcurrent.h"
@ -38,12 +38,8 @@ void initSequenceError(InitSequenceStep step);
void initSequenceDisplayHeight(uint16_t measurement);
void initSequenceEnd();
bool heightSensorGetRange(uint16_t* measurement);
auto display = Adafruit_ST7789(Config::DisplayPinCS, Config::DisplayPinDC, Config::DisplayPinRST);
auto heightSensor = VL53L0X();
auto screenManager = ScreenManager(&display);
Bounce buttons[3];
@ -83,13 +79,13 @@ void setup()
initSequenceEnd();
State.CurrentTime = millis();
CurrentTime = millis();
if (initialized)
{
State.CurrentHeight = currentHeight;
controlSnapToPreset();
// Control.getCurrentHeight() = currentHeight;
Control.snapToPreset();
screenManager.show<HomeScreen>();
}
@ -103,11 +99,7 @@ void setup()
inline void setupHeightSensor()
{
Wire.begin();
heightSensor.setTimeout(500);
auto error = heightSensor.init();
auto error = Control.init();
if (error != VL53L0XInitResult::Success)
{
initSequenceError(InitSequenceStep::HeightSensorInit);
@ -138,8 +130,6 @@ inline void setupHeightSensor()
while(1);
}
heightSensor.setMeasurementTimingBudget(Config::HeightSensorBudget);
initSequenceSuccess(InitSequenceStep::HeightSensorInit);
}
@ -147,40 +137,6 @@ inline void setupHeightSensor()
inline uint16_t testHeightSensor()
{
uint16_t reference = 0;
uint8_t closeCount = 0;
uint16_t measurement;
while (true)
{
if (heightSensorGetRange(&measurement))
{
initSequenceDisplayHeight(measurement);
if (abs(measurement - reference) <= Config::HeightMeasurementDeltaStable)
closeCount++;
else
{
reference = measurement;
closeCount = 0;
}
}
else
{
initSequenceDisplayHeight(0);
reference = 0;
closeCount = 0;
}
if (closeCount < Config::HeightMeasurementDeltaStableCount)
delay(500);
else
break;
}
initSequenceSuccess(InitSequenceStep::HeightSensorTest);
return reference;
}
@ -190,23 +146,30 @@ inline uint16_t testHeightSensor()
Loop
*/
// Forward declarations
void updateHeight();
void loop()
{
State.CurrentTime = millis();
CurrentTime = millis();
if (State.MoveDirection != Direction::None)
switch (Control.update())
{
if (controlCheckOverCurrent())
case ControlUpdateResult::TargetReached:
screenManager.show<HomeScreen>();
break;
case ControlUpdateResult::SensorError:
screenManager.show<MoveSensorErrorScreen>();
break;
case ControlUpdateResult::OverCurrent:
screenManager.show<MoveOverCurrentScreen>();
else
updateHeight();
break;
default:
break;
}
buttons[0].update();
buttons[1].update();
buttons[2].update();
@ -226,40 +189,6 @@ void loop()
uint32_t lastValidMeasurement;
void updateHeight()
{
uint16_t measurement;
if (heightSensorGetRange(&measurement))
{
State.CurrentHeight = measurement;
lastValidMeasurement = State.CurrentTime;
if (controlCheckTargetReached())
screenManager.show<HomeScreen>();
}
else if (State.CurrentTime - lastValidMeasurement >= Config::HeightMeasurementAbortTimeout)
{
dln("Out of range timeout!");
controlStop();
screenManager.show<MoveSensorErrorScreen>();
}
}
bool heightSensorGetRange(uint16_t* measurement)
{
*measurement = heightSensor.readRangeSingleMillimeters();
dl("Range: "); dln(*measurement);
return *measurement <= Config::HeightMeasurementMax;
}
/*
Helper functions for the status display during the initialization sequence