exodus/1NiM-WatingRoom-WatchController
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

motor control changed to infinit pipeline with continuous buffer

Browse Source
This commit is contained in:
Jakob Gegeniger
2024-12-22 17:01:06 +01:00
parent eb0eb436ed
commit dd9f59e418
8 changed files with 593 additions and 593 deletions
Download Patch File Download Diff File Expand all files Collapse all files
components/movement/src/stepperDrv8833.cpp
+16 -13
View File
@@ -14,9 +14,11 @@
void populateLookUps(int resolution,
std::vector<uint8_t> &a1, std::vector<uint8_t> &a2,
std::vector<uint8_t> &b1, std::vector<uint8_t> &b2) {
std::vector<uint8_t> &b1, std::vector<uint8_t> &b2,
uint16_t fullscale)
{
float step = 2.0F * std::numbers::pi / resolution;
const uint16_t FullScale = 1 << LEDC_TIMER_7_BIT;
const uint16_t Maximum = 1 << LEDC_TIMER_7_BIT;
for (size_t i = 0; i < resolution; i++) {
@@ -25,13 +27,13 @@ void populateLookUps(int resolution,
{
// 1 : 0 = Forward
// 1 : PWM = Forward PWM, slow decay
a1.push_back(FullScale);
a2.push_back(FullScale - (cos * FullScale));
a1.push_back(Maximum);
a2.push_back(Maximum - (cos * fullscale));
} else {
// 0 : 1 = Reverse
// PWM : 1 = Reverse PWM, slow decay
a1.push_back(FullScale - (-cos * FullScale));
a2.push_back(FullScale);
a1.push_back(Maximum - (-cos * fullscale));
a2.push_back(Maximum);
}
auto sin = std::sin(step * i);
@@ -39,29 +41,30 @@ void populateLookUps(int resolution,
{
// 1 : 0 = Forward
// 1 : PWM = Forward PWM, slow decay
b1.push_back(FullScale);
b2.push_back(FullScale - (sin * FullScale));
b1.push_back(Maximum);
b2.push_back(Maximum - (sin * fullscale));
} else {
// 0 : 1 = Reverse
// PWM : 1 = Reverse PWM, slow decay
b1.push_back(FullScale - ((-sin * FullScale)));
b2.push_back(FullScale);
b1.push_back(Maximum - ((-sin * fullscale)));
b2.push_back(Maximum);
}
};
}
StepperDriverDrv8833::StepperDriverDrv8833(Pins pins, int resolution)
StepperDriverDrv8833::StepperDriverDrv8833(Pins pins, int resolution, float fullscale)
: StepperDriver(resolution),
_pwmA1(pins.timer, pins.channels[0], pins.pins[0], LEDC_TIMER_7_BIT, 20000),
_pwmA2(pins.timer, pins.channels[1], pins.pins[1], LEDC_TIMER_7_BIT, 20000),
_pwmB1(pins.timer, pins.channels[2], pins.pins[2], LEDC_TIMER_7_BIT, 20000),
_pwmB2(pins.timer, pins.channels[3], pins.pins[3], LEDC_TIMER_7_BIT, 20000)
_pwmB2(pins.timer, pins.channels[3], pins.pins[3], LEDC_TIMER_7_BIT, 20000),
FullScale((1 << LEDC_TIMER_7_BIT) * std::max(0.0F, std::min(1.0F, fullscale)))
{
if ((Resolution % 4) != 0) {
ESP_LOGE("Stepper", "Resolution must be multiple of 4");
}
populateLookUps(resolution, _outA1, _outA2, _outB1, _outB2);
populateLookUps(resolution, _outA1, _outA2, _outB1, _outB2, FullScale);
ESP_LOGD("Stepper", "Output Data:");
ESP_LOG_BUFFER_HEXDUMP("Stepper", _outA1.data(), _outA1.size(), ESP_LOG_DEBUG);
components/movement/src/stepperMotor.cpp
+17 -262
View File
@@ -12,276 +12,31 @@
#include <functional>
#include <vector>
class ProfileGenerator {
public:
ProfileGenerator(StepperMotor::Config &config, float dT, float a, float w)
: _config(config), _dT(dT), _aMax(a), _wMax(w) {
}
Profile createSmoothStep(float delta) {
init();
// already solved in simple step
smoothSubStep(delta, _aMax, _wMax);
return transform();
}
Profile createBuildUpStep(float delta) {
init();
// t0..t1: increasing swing
buildUpSwing(_aMax / 4, _wMax, 3);
auto pnt = getStopDistance(_aMax, _wMax);
// t1..t2: movement to until almost destination
moveUntil(_wMax, delta - pnt.p);
// t2..r3: deceleration until destination
accelUntil(-_aMax, _wMax, [](Point pnt) { return pnt.w < 0; });
return transform();
}
Profile createRapidStartSwingInEnd(float delta) {
init();
auto wMax = _wMax;
// t0..t1: acceleration to max velocity
accelUntil(_aMax * 2, 0, [wMax](Point pnt) { return pnt.w > (wMax * 2); });
// t1..r2: deceleration until moving speed
accelUntil(-_aMax, _wMax * 2, [wMax](Point pnt) { return pnt.w < wMax; });
// t2..t3: movement to until destination (override)
moveUntil(_wMax, delta);
// t3..t4: deceleration
accelUntil(-_aMax, _wMax, [](Point pnt) { return pnt.w < 0; });
float pCen = delta + (_pVec.back() - delta) / 2;
accelUntil(-_aMax, 0, [pCen](Point pnt) { return pnt.p < pCen; });
accelUntil(_aMax, _wBack, [](Point pnt) { return pnt.w > 0; });
return transform();
}
Profile createJumpInTarget(float delta) {
init();
float jumpInRange = std::numbers::pi / 3;
float deltaPre = delta - jumpInRange;
smoothSubStep(deltaPre, _aMax, _wMax);
std::vector<float> jumpInSteps{jumpInRange / 2, jumpInRange / 3, jumpInRange / 6};
float nextPoint = deltaPre;
for (auto &&step : jumpInSteps) {
nextPoint = nextPoint + step;
smoothSubStep(nextPoint, _aMax * 10, _wMax * 2);
}
return transform();
}
private:
struct Point {
float p;
float w;
float t;
};
StepperMotor::Config &_config;
float _dT;
float _aMax;
float _wMax;
float _t;
std::vector<float> _pVec;
// std::vector<float> _wVec;
// std::vector<float> _tVec;
float _wBack;
float _tBack;
void init() {
_pVec = {0};
// _wVec = {0};
// _tVec = {0};
_wBack = 0;
_tBack = 0;
_t = 0;
}
void append(Point pnt) {
_pVec.push_back(pnt.p);
// _wVec.push_back(pnt.w);
// _tVec.push_back(pnt.t);
}
Point getStopDistance(float a, float w) {
float tDec = w / a;
float pDec = a * std::pow(tDec, 2) / 2;
return {pDec, w, tDec};
}
void accelUntil(float a, float wInit, std::function<bool(Point)> checkCbk) {
float t{};
auto pX = _pVec.back();
while (true) {
float w = wInit + a * t;
float p = pX + wInit * t + a * std::pow(t, 2) / 2;
if (checkCbk(Point{p, w, t})) {
break;
}
append(Point{p, w, _t + t});
t = t + _dT;
}
_t = _t + t;
}
void moveUntil(float wM, float pEnd) {
float t{};
auto pX = _pVec.back();
while (true) {
float p = pX + wM * t;
if (p > pEnd) {
break;
}
append(Point{p, wM, _t + t});
t = t + _dT;
}
_t = _t + t;
}
void buildUpSwing(float a, float vM, int swings) {
float t{};
auto pX = _pVec.back();
float TwMax = vM / a;
float tau = 2 * std::numbers::pi;
float sTau = swings * tau;
while (true) {
t = t + _dT;
float env = a * t;
float p = std::sin(sTau * t / TwMax) * env * TwMax / sTau;
float w = (p - pX) / _dT;
if (w > _wMax) {
break;
}
append(Point{p, w, _t + t});
}
_t = _t + t;
}
void smoothSubStep(float delta, float aM, float wM) {
auto pnt = getStopDistance(aM, wM);
if (pnt.p < delta / 2) {
// t0..t1: acceleration to max velocity
accelUntil(aM, 0, [wM](Point pnt) { return pnt.w > wM; });
// t1..t2: movement to until almost destination
moveUntil(wM, delta - pnt.p);
// t2..r3: deceleration until destination
accelUntil(-aM, wM, [](Point pnt) { return pnt.w < 0; });
} else {
float pCen = delta + (_pVec.back() - delta) / 2;
// t0..t1: acceleration until center is reached
accelUntil(-aM, 0, [pCen](Point pnt) { return pnt.p < pCen; });
// t2..r3: deceleration until destination
accelUntil(-aM, wM, [](Point pnt) { return pnt.w < 0; });
}
}
Profile transform() {
std::vector<uint8_t> steps;
steps.reserve(_pVec.size() + 1);
float stp = _config.stepRad * 4;
float res = stp / _config.resolution;
steps.push_back(0);
for (auto &&p : _pVec) {
auto f = p / stp;
f = f - std::floor(f);
uint8_t v = (uint8_t)(f * _config.resolution);
steps.push_back(v);
}
init(); // free memory for old values
return {steps};
}
};
void StepperMotor::createDemoProfile() {
long start = xTaskGetTickCount();
ProfileGenerator generator(_config, 0.001F, 1.0F, 1.0F);
std::function<Profile(float)> call{};
static int idx = 0;
switch (idx) {
case 0: {
call = [&](float f) { return generator.createSmoothStep(f); };
} break;
case 1: {
call = [&](float f) { return generator.createBuildUpStep(f); };
} break;
case 2: {
call = [&](float f) { return generator.createRapidStartSwingInEnd(f); };
} break;
default:
case 3: {
call = [&](float f) { return generator.createJumpInTarget(f); };
} break;
}
idx++;
idx = idx % 4;
auto profile = call(std::numbers::pi);
long ticks = xTaskGetTickCount() - start;
ESP_LOGD("Stepper", "TookTicks %lu ( %lu ms) for %d data points", ticks, ticks * portTICK_PERIOD_MS, profile.steps.size());
ESP_LOG_BUFFER_HEXDUMP("Stepper", profile.steps.data(), profile.steps.size(), ESP_LOG_DEBUG);
auto pp = std::make_unique<Profile>(std::move(profile));
runProfile(std::move(pp));
}
void StepperMotor::moveBy(float angle) {
ProfileGenerator generator(_config, 0.001F, 1.0F, 1.0F);
auto profile = generator.createSmoothStep(angle);
ESP_LOGD("Stepper", "TookTicks for %d data points", profile.steps.size());
ESP_LOG_BUFFER_HEXDUMP("Stepper", profile.steps.data(), profile.steps.size(), ESP_LOG_DEBUG);
auto pp = std::make_unique<Profile>(std::move(profile));
runProfile(std::move(pp));
_buffer.reset();
newProfile();
_generator.createProfile(ProfileGenerator::Profile::Smooth, angle);
// ESP_LOGD("Stepper", "TookTicks for %d data points", profile.steps.size());
// ESP_LOG_BUFFER_HEXDUMP("Stepper", profile.steps.data(), profile.steps.size(), ESP_LOG_DEBUG);
}
void StepperMotor::moveBy(float angle, uint8_t idx, float accel, float speed) {
long start = xTaskGetTickCount();
ESP_LOGD("Stepper", "Running %.1f with prof=%i a=%.2f w=%.2f",
angle *3 /std::numbers::pi, idx, accel, speed);
// long start = xTaskGetTickCount();
ESP_LOGI("Stepper", "Running %.1f with prof=%i a=%.2f w=%.2f",
angle * 3 / std::numbers::pi, idx, accel, speed);
ProfileGenerator generator(_config, 0.001F, accel, speed);
_buffer.reset();
newProfile();
_generator.setDynamic(accel, speed);
_generator.createProfile(static_cast<ProfileGenerator::Profile>(idx), angle);
std::function<Profile(float)> call{};
switch (idx) {
case 0: {
call = [&](float f) { return generator.createSmoothStep(f); };
} break;
case 1: {
call = [&](float f) { return generator.createBuildUpStep(f); };
} break;
case 2: {
call = [&](float f) { return generator.createRapidStartSwingInEnd(f); };
} break;
default:
case 3: {
call = [&](float f) { return generator.createJumpInTarget(f); };
} break;
while (!isIdle())
{
fos::Task::delayMs(200);
}
auto profile = call(angle);
ESP_LOGI("Profile", "processing finished ...");
// long ticks = xTaskGetTickCount() - start;
// ESP_LOGD("Stepper", "TookTicks %lu ( %lu ms) for %d data points", ticks, ticks * portTICK_PERIOD_MS, profile.steps.size());
// ESP_LOG_BUFFER_HEXDUMP("Stepper", profile.steps.data(), profile.steps.size(), ESP_LOG_DEBUG);
auto pp = std::make_unique<Profile>(std::move(profile));
runProfile(std::move(pp));
}