104 lines
2.1 KiB
C++
104 lines
2.1 KiB
C++
#include "./control.h"
|
|
#include "./motor.h"
|
|
#include "./state.h"
|
|
#include "./settings.h"
|
|
#include "./debug.h"
|
|
#include "include/config.h"
|
|
|
|
|
|
void controlMoveTo(uint16_t height)
|
|
{
|
|
dl("controlMoveTo: "); dln(height);
|
|
|
|
State.MoveTarget = height;
|
|
State.MoveDirection = height > State.CurrentHeight ? Direction::Up : Direction::Down;
|
|
}
|
|
|
|
|
|
bool controlCheckTargetReached()
|
|
{
|
|
dl("controlCheckTargetReached: direction = "); dl((uint8_t)State.MoveDirection); dl(", currentHeight = "); dln(State.CurrentHeight);
|
|
|
|
switch (State.MoveDirection)
|
|
{
|
|
case Direction::Up:
|
|
if (State.CurrentHeight >= State.MoveTarget - Config::HeightMeasurementDeltaStop)
|
|
{
|
|
if (State.CurrentHeight - State.MoveTarget <= Config::HeightMeasurementDeltaOnTarget)
|
|
State.CurrentHeight = State.MoveTarget;
|
|
|
|
controlStop();
|
|
return true;
|
|
}
|
|
break;
|
|
|
|
case Direction::Down:
|
|
if (State.CurrentHeight <= State.MoveTarget + Config::HeightMeasurementDeltaStop)
|
|
{
|
|
if (State.MoveTarget - State.CurrentHeight <= Config::HeightMeasurementDeltaOnTarget)
|
|
State.CurrentHeight = State.MoveTarget;
|
|
|
|
controlStop();
|
|
return true;
|
|
}
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
|
|
bool controlCheckOverCurrent()
|
|
{
|
|
if (motorIsOverCurrent())
|
|
{
|
|
dln("controlCheckOverCurrent: overcurrent detected!");
|
|
|
|
controlStop();
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
|
|
void controlStop()
|
|
{
|
|
dln("controlStop");
|
|
|
|
motorStop();
|
|
State.MoveDirection = Direction::None;
|
|
}
|
|
|
|
|
|
void controlSnapToPreset()
|
|
{
|
|
for (uint8_t i = 0; i < 2; i++)
|
|
{
|
|
if (abs(State.CurrentHeight - Settings.Height.Preset[i]) <= Config::HeightMeasurementDeltaOnTarget)
|
|
{
|
|
State.CurrentHeight = Settings.Height.Preset[i];
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
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;
|
|
} |