PWM based motor driver added from other project
This commit is contained in:
@@ -0,0 +1,64 @@
|
||||
/**
|
||||
* @file gpTimer.cpp
|
||||
* @author Gustice
|
||||
* @brief HW-Timer class implementation
|
||||
*
|
||||
* @copyright Copyright (c) 2024
|
||||
*/
|
||||
|
||||
#include "gpTimer.hpp"
|
||||
#include "esp_log.h"
|
||||
|
||||
HwTimerBase::HwTimerBase(gptimer_alarm_cb_t callback) {
|
||||
ESP_LOGI("TMR", "Create timer handle");
|
||||
gptimer_config_t timer_config = {
|
||||
.clk_src = GPTIMER_CLK_SRC_DEFAULT,
|
||||
.direction = GPTIMER_COUNT_UP,
|
||||
.resolution_hz = 1000000, // 1MHz, 1 tick=1us
|
||||
};
|
||||
ESP_ERROR_CHECK(gptimer_new_timer(&timer_config, &gpTimer));
|
||||
|
||||
gptimer_event_callbacks_t cbs = {
|
||||
.on_alarm = callback,
|
||||
};
|
||||
ESP_ERROR_CHECK(gptimer_register_event_callbacks(gpTimer, &cbs, this));
|
||||
}
|
||||
|
||||
void HwTimerBase::setup(int thresh, Mode mode) {
|
||||
ESP_LOGI("TMR", "Setup timer");
|
||||
ESP_ERROR_CHECK(gptimer_enable(gpTimer));
|
||||
|
||||
gptimer_alarm_config_t cfg{};
|
||||
|
||||
switch (mode) {
|
||||
case Mode::OneShot:
|
||||
cfg.alarm_count = thresh;
|
||||
break;
|
||||
case Mode::Periodic:
|
||||
cfg.reload_count = 0,
|
||||
cfg.alarm_count = thresh;
|
||||
cfg.flags.auto_reload_on_alarm = true;
|
||||
break;
|
||||
default:
|
||||
ESP_LOGE("TMR", "Invalid mode %d", mode);
|
||||
break;
|
||||
}
|
||||
ESP_ERROR_CHECK(gptimer_set_alarm_action(gpTimer, &cfg));
|
||||
}
|
||||
|
||||
void HwTimerBase::start() {
|
||||
ESP_ERROR_CHECK(gptimer_start(gpTimer));
|
||||
}
|
||||
|
||||
void HwTimerBase::stop() {
|
||||
ESP_LOGI("TMR", "Stop timer");
|
||||
ESP_ERROR_CHECK(gptimer_stop(gpTimer));
|
||||
}
|
||||
|
||||
HwTimerBase::~HwTimerBase() {
|
||||
stop();
|
||||
ESP_LOGI("TMR", "Disable timer");
|
||||
ESP_ERROR_CHECK(gptimer_disable(gpTimer));
|
||||
ESP_LOGI("TMR", "Delete timer");
|
||||
ESP_ERROR_CHECK(gptimer_del_timer(gpTimer));
|
||||
}
|
||||
@@ -0,0 +1,48 @@
|
||||
/**
|
||||
* @file pwmPort.cpp
|
||||
* @author Gustice
|
||||
* @brief C++ Wrapper for PWM-output
|
||||
*
|
||||
* @copyright Copyright (c) 2024
|
||||
*/
|
||||
|
||||
#include "pwmPort.hpp"
|
||||
#include "esp_log.h"
|
||||
#include "esp_system.h"
|
||||
#include <cassert>
|
||||
#include <cmath>
|
||||
|
||||
PwmPort::PwmPort(ledc_timer_t timer, ledc_channel_t channel, gpio_num_t output, ledc_timer_bit_t resolution, uint32_t frequency)
|
||||
: FullScale((1 << resolution)), _timer(timer), _channel(channel), _port(output) {
|
||||
ledc_timer_config_t ledc_timer = {
|
||||
.speed_mode = LEDC_LOW_SPEED_MODE,
|
||||
.duty_resolution = resolution,
|
||||
.timer_num = timer,
|
||||
.freq_hz = frequency,
|
||||
.clk_cfg = LEDC_AUTO_CLK,
|
||||
};
|
||||
ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer));
|
||||
|
||||
// Prepare and then apply the LEDC PWM channel configuration
|
||||
ledc_channel_config_t ledc_channel = {
|
||||
.gpio_num = output,
|
||||
.speed_mode = LEDC_LOW_SPEED_MODE,
|
||||
.channel = channel,
|
||||
.intr_type = LEDC_INTR_DISABLE,
|
||||
.timer_sel = timer,
|
||||
.duty = 0,
|
||||
.hpoint = 0,
|
||||
};
|
||||
ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel));
|
||||
}
|
||||
|
||||
void PwmPort::write(float duty) {
|
||||
// duty <= 1.0F;
|
||||
ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, _channel, duty * FullScale));
|
||||
ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, _channel));
|
||||
}
|
||||
|
||||
void PwmPort::writeRaw(uint16_t duty) {
|
||||
ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, _channel, duty));
|
||||
ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, _channel));
|
||||
}
|
||||
@@ -0,0 +1,139 @@
|
||||
/**
|
||||
* @file stepperDrv8833.cpp
|
||||
* @author Gustice
|
||||
* @brief Software-driver for Stepper driver DRV8833
|
||||
*
|
||||
* @copyright Copyright (c) 2024
|
||||
*/
|
||||
|
||||
#include "stepperDrv8833.hpp"
|
||||
|
||||
#include "esp_log.h"
|
||||
#include <climits>
|
||||
#include <cmath>
|
||||
|
||||
void populateLookUps(int resolution,
|
||||
std::vector<uint8_t> &a1, std::vector<uint8_t> &a2,
|
||||
std::vector<uint8_t> &b1, std::vector<uint8_t> &b2) {
|
||||
float step = 2.0F * std::numbers::pi / resolution;
|
||||
const uint16_t FullScale = 1 << LEDC_TIMER_7_BIT;
|
||||
|
||||
for (size_t i = 0; i < resolution; i++) {
|
||||
|
||||
auto cos = std::cos(step * i);
|
||||
if (cos > 0)
|
||||
{
|
||||
// 1 : 0 = Forward
|
||||
// 1 : PWM = Forward PWM, slow decay
|
||||
a1.push_back(FullScale);
|
||||
a2.push_back(FullScale - (cos * FullScale));
|
||||
} else {
|
||||
// 0 : 1 = Reverse
|
||||
// PWM : 1 = Reverse PWM, slow decay
|
||||
a1.push_back(FullScale - (-cos * FullScale));
|
||||
a2.push_back(FullScale);
|
||||
}
|
||||
|
||||
auto sin = std::sin(step * i);
|
||||
if (sin > 0)
|
||||
{
|
||||
// 1 : 0 = Forward
|
||||
// 1 : PWM = Forward PWM, slow decay
|
||||
b1.push_back(FullScale);
|
||||
b2.push_back(FullScale - (sin * FullScale));
|
||||
} else {
|
||||
// 0 : 1 = Reverse
|
||||
// PWM : 1 = Reverse PWM, slow decay
|
||||
b1.push_back(FullScale - ((-sin * FullScale)));
|
||||
b2.push_back(FullScale);
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
StepperDriverDrv8833::StepperDriverDrv8833(Pins pins, int resolution)
|
||||
: 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)
|
||||
{
|
||||
if ((Resolution % 4) != 0) {
|
||||
ESP_LOGE("Stepper", "Resolution must be multiple of 4");
|
||||
}
|
||||
|
||||
populateLookUps(resolution, _outA1, _outA2, _outB1, _outB2);
|
||||
|
||||
char buffer[resolution * 2 + 10];
|
||||
ESP_LOGD("Stepper", "Output Data:");
|
||||
ESP_LOG_BUFFER_HEXDUMP("Stepper", _outA1.data(), _outA1.size(), ESP_LOG_DEBUG);
|
||||
ESP_LOG_BUFFER_HEXDUMP("Stepper", _outA2.data(), _outA2.size(), ESP_LOG_DEBUG);
|
||||
ESP_LOG_BUFFER_HEXDUMP("Stepper", _outB1.data(), _outB1.size(), ESP_LOG_DEBUG);
|
||||
ESP_LOG_BUFFER_HEXDUMP("Stepper", _outB2.data(), _outB2.size(), ESP_LOG_DEBUG);
|
||||
|
||||
step(0);
|
||||
}
|
||||
|
||||
StepperDriverDrv8833::StepCplx StepperDriverDrv8833::fullStep(int8_t cnt) {
|
||||
if (cnt > 0) {
|
||||
cnt = 1;
|
||||
} else {
|
||||
cnt = -1;
|
||||
}
|
||||
|
||||
int pos = (_position / Quad + cnt) % 4;
|
||||
pos = pos * Quad;
|
||||
return setPos(pos);
|
||||
}
|
||||
|
||||
StepperDriverDrv8833::StepCplx StepperDriverDrv8833::step(int8_t cnt) {
|
||||
if (cnt == 0) {
|
||||
return {0, (uint8_t)_position};
|
||||
}
|
||||
if (cnt > Resolution) {
|
||||
// ESP_LOGE("Stepper", "microstep exceeds four steps");
|
||||
}
|
||||
if (cnt > Quad) {
|
||||
// ESP_LOGW("Stepper", "microstep exceeds quadrature step");
|
||||
}
|
||||
|
||||
int8_t increment{};
|
||||
auto pos = _position + cnt;
|
||||
if (pos < 0) {
|
||||
increment--;
|
||||
pos += Resolution;
|
||||
} else if (pos >= Resolution) {
|
||||
increment++;
|
||||
pos -= Resolution;
|
||||
} else {
|
||||
// in that case ... nothing
|
||||
}
|
||||
setOutput(pos);
|
||||
return {increment, (uint8_t)_position};
|
||||
}
|
||||
|
||||
StepperDriverDrv8833::StepCplx StepperDriverDrv8833::setPos(uint8_t pos) {
|
||||
pos = (pos) % Resolution;
|
||||
|
||||
int8_t increment{};
|
||||
if (pos - _position < 0) {
|
||||
increment--;
|
||||
} else if (pos - _position > Resolution) {
|
||||
increment++;
|
||||
} else {
|
||||
// in that case ... nothing
|
||||
}
|
||||
|
||||
setOutput(pos);
|
||||
return {increment, (uint8_t)_position};
|
||||
}
|
||||
|
||||
void StepperDriverDrv8833::setOutput(uint8_t pos) {
|
||||
if (pos >= Resolution) {
|
||||
// ESP_LOGE("Stepper", "d exceeds micro-stepping size");
|
||||
}
|
||||
_position = pos;
|
||||
_pwmA1.writeRaw(_outA1[pos]);
|
||||
_pwmA2.writeRaw(_outA2[pos]);
|
||||
_pwmB1.writeRaw(_outB1[pos]);
|
||||
_pwmB2.writeRaw(_outB2[pos]);
|
||||
}
|
||||
@@ -0,0 +1,288 @@
|
||||
/**
|
||||
* @brief stepperMotor.cpp
|
||||
* @author Gustice
|
||||
* @brief Stepper Motor device class
|
||||
*
|
||||
* @copyright Copyright (c) 2024
|
||||
*/
|
||||
|
||||
#include "stepperMotor.hpp"
|
||||
#include "esp_log.h"
|
||||
#include <cmath>
|
||||
#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 pnt = getStopDistance(_aMax, _wMax);
|
||||
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 - _pVec.back()) / _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));
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
ProfileGenerator generator(_config, 0.001F, accel, speed);
|
||||
|
||||
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;
|
||||
}
|
||||
auto profile = call(angle);
|
||||
// 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));
|
||||
}
|
||||
Reference in New Issue
Block a user