gustice
04a8904cbe
moving SPIT-App parts to main app LiveShowDemoProject added Debugging motor movements
51 lines
1.4 KiB
C++
51 lines
1.4 KiB
C++
#pragma once
|
|
|
|
#include "StepperDrv8833.hpp"
|
|
#include "stepperMotor.hpp"
|
|
#include <cmath>
|
|
|
|
class WatchMotor {
|
|
static constexpr uint8_t Positions = 6;
|
|
|
|
public:
|
|
WatchMotor(StepperMotor &stepper, OutputPort &sleep)
|
|
: _stepper(stepper),
|
|
_motorSleep(sleep) {
|
|
_motorSleep.write(false);
|
|
}
|
|
|
|
void moveTo(uint8_t sign, uint8_t profile = 0) {
|
|
sign = sign % 6;
|
|
|
|
float delta = 2.0F * std::numbers::pi;
|
|
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;
|
|
}
|
|
|
|
// stepper.getScaledPos() * 6
|
|
if (_currentIndex < sign) {
|
|
delta = delta / 6 * (sign - _currentIndex);
|
|
} else if (_currentIndex > sign) {
|
|
delta = delta / 6 * (sign + 6 - _currentIndex);
|
|
}
|
|
ESP_LOGD("MV", "Moving to %d by %.2f + %.2f", sign, delta, corr);
|
|
|
|
_stepper.moveBy(delta + corr, profile);
|
|
_currentIndex = sign;
|
|
}
|
|
|
|
float getScaled() { return _stepper.getScaledPos() * 6; }
|
|
uint8_t getIndex() { return _currentIndex; }
|
|
|
|
private:
|
|
StepperMotor &_stepper;
|
|
OutputPort &_motorSleep;
|
|
uint8_t _currentIndex{};
|
|
};
|