69 lines
2.0 KiB
C++
69 lines
2.0 KiB
C++
#pragma once
|
|
|
|
#include "stepperDrv.hpp"
|
|
#include "stepperMotor.hpp"
|
|
#include <cmath>
|
|
#include "syslog.hpp"
|
|
|
|
class WatchMotor {
|
|
static constexpr uint8_t Positions = 6;
|
|
|
|
public:
|
|
using AlmostFinishedCb = void (*)(uint8_t newIdx);
|
|
WatchMotor(IStepperMotor &stepper, AlmostFinishedCb onDone)
|
|
: _stepper(stepper),
|
|
_onFinCb(onDone) {
|
|
}
|
|
|
|
void moveTo(uint8_t position, uint8_t profile, int minJumps, float accel, float speed) {
|
|
auto sign = position % 6;
|
|
|
|
int minTurns = minJumps / 6;
|
|
|
|
float corr = 0.0F;
|
|
float currentPos = _stepper.getScaledPos() * 6;
|
|
if ((_currentIndex > 4) && (currentPos < 2)) {
|
|
corr = 6 - _currentIndex - currentPos;
|
|
} else if ((_currentIndex < 2) && (currentPos > 2)) {
|
|
corr = _currentIndex + 6 - currentPos;
|
|
} else {
|
|
corr = _currentIndex - currentPos;
|
|
}
|
|
|
|
int steps = 0;
|
|
if (_currentIndex < sign) {
|
|
steps = (sign - _currentIndex);
|
|
} else if (_currentIndex >= sign) {
|
|
steps = (sign + 6 - _currentIndex);
|
|
}
|
|
steps += minTurns * 6;
|
|
if (steps < minJumps) {
|
|
steps += 6;
|
|
}
|
|
|
|
float delta = 2.0F * std::numbers::pi / 6 * steps;
|
|
|
|
ESP_LOGD("MV", "Moving to %d by %.2f + %.2f", sign, delta, corr);
|
|
|
|
_stepper.moveBy(delta + corr, profile, accel, speed, [&](int diff) {
|
|
Syslog::logv(Syslog::Severity::Debug,"Almost done with head=%i",diff);
|
|
if (diff < 6) {
|
|
Syslog::logv(Syslog::Severity::Warning,"Low headroom in Profile pipeline=%i",diff);
|
|
}
|
|
|
|
if (_onFinCb) {
|
|
_onFinCb(position);
|
|
}
|
|
});
|
|
_currentIndex = sign;
|
|
}
|
|
|
|
float getScaled() { return _stepper.getScaledPos() * 6; }
|
|
uint8_t getIndex() { return _currentIndex; }
|
|
|
|
private:
|
|
IStepperMotor &_stepper;
|
|
uint8_t _currentIndex{};
|
|
AlmostFinishedCb _onFinCb{};
|
|
};
|