284 lines
6.9 KiB
C++
284 lines
6.9 KiB
C++
#include "./control.h"
|
|
#include <Wire.h>
|
|
#include "./debug.h"
|
|
#include "./motor.h"
|
|
#include "./state.h"
|
|
#include "./settings.h"
|
|
#include "include/config.h"
|
|
|
|
|
|
ControlManager Control = ControlManager();
|
|
|
|
|
|
VL53L0XInitResult ControlManager::init()
|
|
{
|
|
dln("[ CONTROL ] Initializing");
|
|
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()
|
|
{
|
|
// Always read the range if in async reading mode, even if we're not supposed to
|
|
// be moving anymore, so it clears the status.
|
|
bool asyncReading = heightSensor.asyncReading();
|
|
|
|
uint16_t measurement = asyncReading
|
|
? heightSensor.asyncReadRangeSingleMillimeters()
|
|
: VL53L0XRangeNotReady;
|
|
|
|
|
|
vl("[ CONTROL ] Update: asyncReading = "); vl(asyncReading); vl(", measurement = "); vl(measurement);
|
|
vl(", moveDirection = "); vl((uint8_t)this->moveDirection); vl(", stabilizationStart = "); vln(this->stabilizationStart);
|
|
|
|
|
|
bool moving = this->moveDirection != MoveDirection::None && this->stabilizationStart == 0;
|
|
if (!moving && this->stabilizationStart == 0)
|
|
{
|
|
vl("[ CONTROL ] Idle");
|
|
return ControlUpdateResult::Idle;
|
|
}
|
|
|
|
|
|
// Check for over-current
|
|
// TODO: don't check current every update
|
|
if (moving && motorIsOvercurrent())
|
|
{
|
|
dln("[ CONTROL ] Overcurrent detected!");
|
|
this->moveStop();
|
|
|
|
return ControlUpdateResult::Overcurrent;
|
|
}
|
|
|
|
// Read sensor
|
|
if (!asyncReading)
|
|
{
|
|
vln("[ CONTROL ] Starting async read");
|
|
heightSensor.asyncStartReadRangeSingleMillimeters();
|
|
}
|
|
|
|
|
|
if (measurement != VL53L0XRangeNotReady)
|
|
{
|
|
if (measurement > Config::HeightMeasurementMax)
|
|
// Treat invalid results as a timeout, so the checks further on are more readable
|
|
measurement = VL53L0XRangeTimeout;
|
|
|
|
|
|
dl("[ CONTROL ] Measurement: "); dln(measurement);
|
|
this->lastMeasurement = measurement != VL53L0XRangeTimeout ? measurement : 0;
|
|
}
|
|
|
|
|
|
if (measurement != VL53L0XRangeNotReady && measurement != VL53L0XRangeTimeout)
|
|
{
|
|
this->currentHeight = measurement;
|
|
this->lastValidMeasurement = CurrentTime;
|
|
|
|
// Check if target has been reached
|
|
if (moving && this->targetReached())
|
|
{
|
|
this->moveStop();
|
|
|
|
return ControlUpdateResult::TargetReached;
|
|
}
|
|
}
|
|
|
|
if (measurement == VL53L0XRangeTimeout && moving)
|
|
{
|
|
// While moving, allow incidental invalid results
|
|
if (CurrentTime - lastValidMeasurement >= Config::HeightMeasurementAbortTimeout)
|
|
{
|
|
dln("[ CONTROL ] Timeout while moving!");
|
|
this->moveStop();
|
|
|
|
return ControlUpdateResult::SensorError;
|
|
}
|
|
}
|
|
|
|
if (measurement == VL53L0XRangeTimeout && this->moveDirection == MoveDirection::None)
|
|
{
|
|
// In pure stabilization (not pre-move), immediately return the sensor error
|
|
return ControlUpdateResult::SensorError;
|
|
}
|
|
|
|
|
|
|
|
if (this->stabilizationStart && this->moveDirection != MoveDirection::None)
|
|
{
|
|
// Pre-move stabilization
|
|
if (this->stabilized())
|
|
{
|
|
dln("[ CONTROL ] Pre-move stabilization successful");
|
|
|
|
// Sensor looks good, let's go!
|
|
motorStart(this->moveDirection == MoveDirection::Up ? MotorDirection::Up : MotorDirection::Down);
|
|
return ControlUpdateResult::Moving;
|
|
}
|
|
else if (CurrentTime - this->stabilizationStart >= Config::HeightMeasurementDeltaStableMoveTimeout)
|
|
{
|
|
dln("[ CONTROL ] Timeout in pre-move stabilization!");
|
|
|
|
// Timeout expired, abort the move
|
|
this->stabilizationStart = 0;
|
|
this->moveDirection = MoveDirection::None;
|
|
|
|
return ControlUpdateResult::SensorError;
|
|
}
|
|
}
|
|
|
|
return moving ? ControlUpdateResult::Moving : ControlUpdateResult::Stabilizing;
|
|
}
|
|
|
|
|
|
void ControlManager::stabilizeStart()
|
|
{
|
|
dln("[ CONTROL ] Starting stabilization");
|
|
|
|
this->stabilizationStart = CurrentTime;
|
|
this->stabilizeTarget = 0;
|
|
}
|
|
|
|
|
|
bool ControlManager::stabilized()
|
|
{
|
|
if (this->stabilizeTarget == 0)
|
|
{
|
|
this->stabilizeTarget = this->currentHeight;
|
|
this->stabilizationLastCheck = this->lastValidMeasurement;
|
|
return false;
|
|
}
|
|
|
|
// Only count if we have a new measurement
|
|
if (this->stabilizationLastCheck == this->lastValidMeasurement)
|
|
return false;
|
|
|
|
this->stabilizationLastCheck = this->lastValidMeasurement;
|
|
|
|
|
|
int16_t delta = this->currentHeight - this->stabilizeTarget;
|
|
if (abs(delta) <= Config::HeightMeasurementDeltaStable)
|
|
{
|
|
this->stabilizeCount++;
|
|
|
|
if (this->stabilizeCount >= Config::HeightMeasurementDeltaStableCount)
|
|
{
|
|
dln("[ CONTROL ] Stable");
|
|
this->stabilizationStart = 0;
|
|
return true;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// If it's this much off, chances are the actual value is somewhere in the middle
|
|
this->stabilizeTarget += (delta / 2);
|
|
this->stabilizeCount = 0;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
|
|
void ControlManager::moveStart(uint16_t height)
|
|
{
|
|
dl("[ CONTROL ] Starting move to: "); dln(height);
|
|
|
|
this->moveTarget = height;
|
|
this->moveDirection = height > this->currentHeight ? MoveDirection::Up : MoveDirection::Down;
|
|
|
|
// Wait for a stable result
|
|
this->stabilizeStart();
|
|
}
|
|
|
|
|
|
void ControlManager::moveStop()
|
|
{
|
|
dln("[ CONTROL ] Stopping move");
|
|
|
|
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;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
bool ControlManager::targetReached()
|
|
{
|
|
// Checks depend on the direction, so it returns true immediately if we've overshot the target
|
|
switch (this->moveDirection)
|
|
{
|
|
case MoveDirection::Up:
|
|
if (this->currentHeight >= this->moveTarget - Config::HeightMeasurementDeltaStop)
|
|
{
|
|
// Snap to the target
|
|
if (this->currentHeight - this->moveTarget <= Config::HeightMeasurementDeltaOnTarget)
|
|
this->currentHeight = this->moveTarget;
|
|
|
|
dln("[ CONTROL ] Target reached");
|
|
return true;
|
|
}
|
|
break;
|
|
|
|
case MoveDirection::Down:
|
|
if (this->currentHeight <= this->moveTarget + Config::HeightMeasurementDeltaStop)
|
|
{
|
|
// Snap to the target
|
|
if (this->moveTarget - this->currentHeight <= Config::HeightMeasurementDeltaOnTarget)
|
|
this->currentHeight = this->moveTarget;
|
|
|
|
dln("[ CONTROL ] Target reached");
|
|
return true;
|
|
}
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
|
|
void ControlManager::getDisplayHeight(char* buffer, uint16_t value)
|
|
{
|
|
if (value == 0)
|
|
{
|
|
buffer[0] = '-';
|
|
buffer[1] = 0;
|
|
return;
|
|
}
|
|
|
|
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;
|
|
}
|