91 lines
1.9 KiB
C++
91 lines
1.9 KiB
C++
#include "./motorstate.h"
|
|
#include "./motor.h"
|
|
#include "./state.h"
|
|
#include "./settings.h"
|
|
#include "./debug.h"
|
|
#include "include/config.h"
|
|
|
|
|
|
void motorStateMoveTo(uint16_t height)
|
|
{
|
|
dl("motorStateMoveTo: "); dln(height);
|
|
|
|
State.MoveTarget = height;
|
|
State.MoveDirection = height > State.CurrentHeight ? Direction::Up : Direction::Down;
|
|
}
|
|
|
|
|
|
bool motorStateCheckTargetReached()
|
|
{
|
|
dl("motorStateCheckTargetReached: 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;
|
|
|
|
motorStateStop();
|
|
return true;
|
|
}
|
|
break;
|
|
|
|
case Direction::Down:
|
|
if (State.CurrentHeight <= State.MoveTarget + Config::HeightMeasurementDeltaStop)
|
|
{
|
|
if (State.MoveTarget - State.CurrentHeight <= Config::HeightMeasurementDeltaOnTarget)
|
|
State.CurrentHeight = State.MoveTarget;
|
|
|
|
motorStateStop();
|
|
return true;
|
|
}
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
|
|
bool motorStateCheckOverCurrent()
|
|
{
|
|
if (motorIsOverCurrent())
|
|
{
|
|
dln("motorStateCheckOverCurrent: overcurrent detected!");
|
|
|
|
motorStateStop();
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
|
|
void motorStateStop()
|
|
{
|
|
dln("motorStateStop");
|
|
|
|
motorStop();
|
|
State.MoveDirection = Direction::None;
|
|
}
|
|
|
|
|
|
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;
|
|
} |