PWM based motor driver added from other project
This commit is contained in:
@@ -0,0 +1,11 @@
|
||||
idf_component_register(
|
||||
SRC_DIRS "src"
|
||||
INCLUDE_DIRS "include"
|
||||
REQUIRES freeRtosApi
|
||||
project_bases
|
||||
board_io
|
||||
esp_common
|
||||
esp_driver_gpio
|
||||
esp_driver_ledc
|
||||
esp_driver_gptimer
|
||||
)
|
||||
@@ -0,0 +1,42 @@
|
||||
/**
|
||||
* @file StepperDrv8833.hpp
|
||||
* @author Gustice
|
||||
* @brief Software-driver for Stepper driver DRV8833
|
||||
* @details https://www.ti.com/lit/ds/symlink/drv8833.pdf?ts=1726872460958&ref_url=https%253A%252F%252Fwww.ti.com%252Fproduct%252Fde-de%252FDRV8833
|
||||
*
|
||||
* @copyright Copyright (c) 2024
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "stepperDrv.hpp"
|
||||
#include <array>
|
||||
#include <vector>
|
||||
|
||||
class StepperDriverDrv8833 : public StepperDriver {
|
||||
public:
|
||||
struct Pins {
|
||||
ledc_timer_t timer;
|
||||
std::array<ledc_channel_t, 4> channels;
|
||||
std::array<gpio_num_t, 4> pins;
|
||||
};
|
||||
|
||||
StepperDriverDrv8833(Pins pins, int resolution);
|
||||
StepCplx fullStep(int8_t cnt) override;
|
||||
StepCplx step(int8_t cnt) override;
|
||||
StepCplx setPos(uint8_t pos) override;
|
||||
uint8_t getSubPos() {
|
||||
return _position;
|
||||
}
|
||||
|
||||
private:
|
||||
PwmPort _pwmA1;
|
||||
PwmPort _pwmA2;
|
||||
PwmPort _pwmB1;
|
||||
PwmPort _pwmB2;
|
||||
|
||||
std::vector<uint8_t> _outA1;
|
||||
std::vector<uint8_t> _outA2;
|
||||
std::vector<uint8_t> _outB1;
|
||||
std::vector<uint8_t> _outB2;
|
||||
void setOutput(uint8_t pos) override;
|
||||
};
|
||||
@@ -0,0 +1,82 @@
|
||||
/**
|
||||
* @file gpTimer.hpp
|
||||
* @author Gustice
|
||||
* @brief ESP-hardware-timer class
|
||||
*
|
||||
* @copyright Copyright (c) 2024
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "driver/gptimer.h"
|
||||
#include "esp_attr.h"
|
||||
#include "esp_log.h"
|
||||
#include "portBase.hpp"
|
||||
|
||||
/**
|
||||
* @brief Base class for tasks
|
||||
*/
|
||||
class HwTimerBase : public PortBase {
|
||||
public:
|
||||
enum Mode {
|
||||
OneShot = 0,
|
||||
Periodic
|
||||
};
|
||||
|
||||
void setup(int thresh, Mode mode);
|
||||
void start();
|
||||
void stop();
|
||||
|
||||
protected:
|
||||
gptimer_handle_t gpTimer{};
|
||||
|
||||
HwTimerBase(gptimer_alarm_cb_t callback);
|
||||
~HwTimerBase();
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Simple Timer without callback
|
||||
*/
|
||||
class HwTimer : public HwTimerBase {
|
||||
public:
|
||||
using CallSignature = bool (*)(HwTimer &);
|
||||
|
||||
HwTimer(CallSignature call)
|
||||
: HwTimerBase(CallFrame), _call{call} {
|
||||
}
|
||||
|
||||
private:
|
||||
CallSignature _call;
|
||||
|
||||
static bool IRAM_ATTR CallFrame(gptimer_handle_t timer, const gptimer_alarm_event_data_t *edata, void *_this) {
|
||||
auto &tmr = *reinterpret_cast<HwTimer *>(_this);
|
||||
return tmr._call(tmr);
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Timer with callback parameter
|
||||
* @tparam T Type of task parameter on call
|
||||
*/
|
||||
template <typename T>
|
||||
class HwTimerT : public HwTimerBase {
|
||||
using CallSignatureT = bool (*)(HwTimerT<T> &, T);
|
||||
|
||||
public:
|
||||
HwTimerT(CallSignatureT call, const T ¶m)
|
||||
: HwTimerBase(CallFrame), _param(call, param) {
|
||||
}
|
||||
|
||||
private:
|
||||
template <typename TS>
|
||||
struct Param {
|
||||
CallSignatureT call;
|
||||
const TS &arg;
|
||||
};
|
||||
static bool IRAM_ATTR CallFrame(gptimer_handle_t timer, const gptimer_alarm_event_data_t *edata, void *_this) {
|
||||
auto &tmr = *reinterpret_cast<HwTimerT *>(_this);
|
||||
auto ¶m = tmr._param;
|
||||
return param.call(tmr, param.arg);
|
||||
}
|
||||
Param<T> _param;
|
||||
};
|
||||
@@ -0,0 +1,34 @@
|
||||
/**
|
||||
* @file pwmPort.hpp
|
||||
* @author Gustice
|
||||
* @brief C++ Wrapper for PWM-output
|
||||
*
|
||||
* @copyright Copyright (c) 2024
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "driver/gpio.h"
|
||||
#include "driver/ledc.h"
|
||||
#include "portBase.hpp"
|
||||
#include <cstdint>
|
||||
|
||||
/**
|
||||
* @brief PWM-Class
|
||||
* @details Provides Write to PWM-Port
|
||||
*/
|
||||
class PwmPort : public PortBase {
|
||||
public:
|
||||
PwmPort(ledc_timer_t timer, ledc_channel_t channel, gpio_num_t output, ledc_timer_bit_t resolution, uint32_t frequency);
|
||||
~PwmPort() = default;
|
||||
|
||||
void write(float duty);
|
||||
void writeRaw(uint16_t duty);
|
||||
|
||||
protected:
|
||||
const uint32_t FullScale;
|
||||
const ledc_timer_t _timer;
|
||||
const ledc_channel_t _channel;
|
||||
const gpio_num_t _port;
|
||||
|
||||
private:
|
||||
};
|
||||
@@ -0,0 +1,36 @@
|
||||
/**
|
||||
* @file stepperDrv.hpp
|
||||
* @author Gustice
|
||||
* @brief Abstraction for Stepper driver
|
||||
*
|
||||
* @copyright Copyright (c) 2024
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "gpioPort.hpp"
|
||||
#include "pwmPort.hpp"
|
||||
#include <array>
|
||||
#include <vector>
|
||||
|
||||
class StepperDriver {
|
||||
public:
|
||||
struct StepCplx {
|
||||
int8_t increment{};
|
||||
uint8_t subPos{};
|
||||
};
|
||||
|
||||
StepperDriver(int resolution)
|
||||
: Resolution(resolution), Quad(Resolution / 4) {}
|
||||
virtual StepCplx fullStep(int8_t cnt) = 0;
|
||||
virtual StepCplx step(int8_t cnt) = 0;
|
||||
virtual StepCplx setPos(uint8_t pos) = 0;
|
||||
uint8_t getSubPos() {
|
||||
return _position;
|
||||
}
|
||||
|
||||
protected:
|
||||
const uint8_t Resolution;
|
||||
const uint8_t Quad;
|
||||
int8_t _position{};
|
||||
virtual void setOutput(uint8_t pos) = 0;
|
||||
};
|
||||
@@ -0,0 +1,311 @@
|
||||
/**
|
||||
* @file stepperMotor.hpp
|
||||
* @author Gustice
|
||||
* @brief Stepper Motor device class
|
||||
*
|
||||
* @copyright Copyright (c) 2024
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "gpTimer.hpp"
|
||||
#include "gpioPort.hpp"
|
||||
#include "stepperDrv.hpp"
|
||||
#include <Task.hpp>
|
||||
#include <cmath>
|
||||
#include <memory>
|
||||
|
||||
struct Profile {
|
||||
std::vector<uint8_t> steps;
|
||||
};
|
||||
|
||||
class StepperMotor {
|
||||
enum Mode {
|
||||
Idle = 0,
|
||||
HomingFromSensor, // if home sensor already active
|
||||
HomingToSensorBegin,
|
||||
HomingToSensorEnd,
|
||||
Homing,
|
||||
Run,
|
||||
CrawlToEnd,
|
||||
};
|
||||
|
||||
public:
|
||||
template <int MAX, int PREC>
|
||||
struct Position {
|
||||
uint16_t position{};
|
||||
uint16_t subPos{};
|
||||
Position &operator+(Position &lhs) {
|
||||
auto v1 = getFixPoint();
|
||||
auto v2 = lhs.getFixPoint();
|
||||
auto v = v1 + v2;
|
||||
if (v > (MAX << 16)) {
|
||||
v -= (MAX << 16);
|
||||
}
|
||||
position = getPosition(v);
|
||||
subPos = getSubPos(v);
|
||||
return *this;
|
||||
}
|
||||
|
||||
Position &operator-(Position &lhs) {
|
||||
auto v1 = getFixPoint();
|
||||
auto v2 = lhs.getFixPoint();
|
||||
auto v = v1 - v2;
|
||||
if (v < (MAX << 16)) {
|
||||
v += (MAX << 16);
|
||||
}
|
||||
position = getPosition(v);
|
||||
subPos = getSubPos(v);
|
||||
return *this;
|
||||
}
|
||||
|
||||
Position &operator+(StepperDriver::StepCplx &pos) {
|
||||
position += pos.increment;
|
||||
if (position > MAX) {
|
||||
position -= MAX;
|
||||
} else if (position < 0) {
|
||||
position += MAX;
|
||||
} else {
|
||||
// nothing to do here
|
||||
}
|
||||
subPos += pos.subPos;
|
||||
return *this;
|
||||
}
|
||||
|
||||
Position &halv() {
|
||||
auto v1 = getFixPoint();
|
||||
auto v = v1 / 2;
|
||||
position = getPosition(v);
|
||||
subPos = getSubPos(v);
|
||||
return *this;
|
||||
}
|
||||
|
||||
float getScaled() {
|
||||
auto scaled = (float)position / MAX;
|
||||
// scaled += (float)subPos / (1 << PREC);
|
||||
return scaled;
|
||||
}
|
||||
|
||||
private:
|
||||
int32_t getFixPoint() {
|
||||
int32_t value = (((int32_t)position) << 16) | (subPos << (16 - PREC));
|
||||
return value;
|
||||
}
|
||||
uint16_t getPosition(int32_t value) {
|
||||
auto pos = (value >> 16) & 0xFFFF;
|
||||
return pos;
|
||||
}
|
||||
uint16_t getSubPos(int32_t value) {
|
||||
auto pos = ((value & 0xFFFF) >> (16 - PREC));
|
||||
return pos;
|
||||
}
|
||||
};
|
||||
|
||||
using Trajectory = std::unique_ptr<Profile>;
|
||||
|
||||
struct Config {
|
||||
Config(float stepDeg, int microsteps)
|
||||
: step(stepDeg), resolution(microsteps), resolutionQuad(resolution/4) {}
|
||||
|
||||
/// @brief Step-size in degree (e.g. 1.8 deg)
|
||||
float step;
|
||||
/// @brief Step-size in radiant (e.g. 1.8 deg)
|
||||
float stepRad{(float)(std::numbers::pi * step / 180)};
|
||||
/// @brief Number of microsteps for four steps
|
||||
int resolution;
|
||||
/// @brief Number of microsteps for four steps
|
||||
int resolutionQuad;
|
||||
/// @brief Number of Steps for full revolution
|
||||
int steps{std::lround(360.0F / step)};
|
||||
};
|
||||
|
||||
StepperMotor(Config &config, StepperDriver &driver, InputPort &initSensor)
|
||||
: _config(config), _driver(driver), _initSensor(initSensor),
|
||||
_timer(TimerCallParam, *this) {
|
||||
_timer.setup(1'000, HwTimerBase::Mode::Periodic);
|
||||
_timer.start();
|
||||
}
|
||||
~StepperMotor() {};
|
||||
|
||||
void findRef() {
|
||||
ESP_LOGI("Stepper", "Homing ...");
|
||||
// if (_initSensor.read()) {
|
||||
// _mode = Mode::HomingFromSensor;
|
||||
// ESP_LOGI("Stepper", "First leave HomePos");
|
||||
// } else {
|
||||
// _mode = Mode::HomingToSensorBegin;
|
||||
// ESP_LOGI("Stepper", "Find start of HomePos");
|
||||
// }
|
||||
|
||||
// while (_mode != Mode::Idle) {
|
||||
// fos::Task::delayTicks(1);
|
||||
// }
|
||||
|
||||
int fullStep = _config.resolution / 4;
|
||||
_driver.setPos(0);
|
||||
|
||||
if (_initSensor.read()) {
|
||||
ESP_LOGI("Stepper", "Need to move away from home");
|
||||
// MoveAway
|
||||
while (_initSensor.read()) {
|
||||
(void)_driver.step(-fullStep);
|
||||
fos::Task::delayTicks(1);
|
||||
}
|
||||
for (size_t i = 0; i < 8; i++) // Drive another 4 Steps away
|
||||
{
|
||||
(void)_driver.step(-fullStep);
|
||||
fos::Task::delayTicks(1);
|
||||
}
|
||||
}
|
||||
|
||||
ESP_LOGI("Stepper", "Fast find right side of home sensor");
|
||||
// Find right Endpoint
|
||||
while (not _initSensor.read()) {
|
||||
(void)_driver.step(fullStep);
|
||||
fos::Task::delayTicks(1);
|
||||
}
|
||||
while (_initSensor.read()) { // move away
|
||||
(void)_driver.step(-fullStep);
|
||||
fos::Task::delayTicks(1);
|
||||
}
|
||||
ESP_LOGI("Stepper", "Find right side of home sensor");
|
||||
while (not _initSensor.read()) {
|
||||
(void)_driver.step(1);
|
||||
fos::Task::delayTicks(1);
|
||||
}
|
||||
int pos{};
|
||||
resetPosition();
|
||||
ESP_LOGI("Stepper", "Find end of sensor");
|
||||
while (_initSensor.read()) {
|
||||
(void)_driver.step(1);
|
||||
pos++;
|
||||
fos::Task::delayTicks(1);
|
||||
}
|
||||
for (size_t i = 0; i < 32; i++) // Drive another 4 Steps away
|
||||
{
|
||||
(void)_driver.step(1);
|
||||
pos++;
|
||||
fos::Task::delayTicks(1);
|
||||
}
|
||||
|
||||
ESP_LOGI("Stepper", "Find left side of home sensor");
|
||||
while (not _initSensor.read()) {
|
||||
(void)_driver.step(-1);
|
||||
pos--;
|
||||
fos::Task::delayTicks(1);
|
||||
}
|
||||
|
||||
ESP_LOGI("Stepper", "Move back to mid position %d", pos / 2);
|
||||
for (size_t i = 0; i < pos / 2; i++) {
|
||||
(void)_driver.step(-1);
|
||||
fos::Task::delayTicks(1);
|
||||
}
|
||||
ESP_LOGI("Stepper", "Reset position");
|
||||
resetPosition();
|
||||
}
|
||||
|
||||
void demoFullTurn() {
|
||||
auto steps = _config.resolution * _config.steps;
|
||||
ESP_LOGI("Stepper", "Running %i * %i steps for full turn", _config.resolution, _config.steps);
|
||||
|
||||
ESP_LOGI("Stepper", "Start is %f.2", getScaledPos());
|
||||
for (size_t i = 0; i < _config.resolutionQuad * _config.steps; i++) {
|
||||
auto delta = _driver.step(1);
|
||||
_indexPos = _indexPos + delta;
|
||||
fos::Task::delayTicks(1);
|
||||
}
|
||||
ESP_LOGI("Stepper", "End is %f.2", getScaledPos());
|
||||
}
|
||||
|
||||
void createDemoProfile();
|
||||
void moveBy(float angle);
|
||||
void moveBy(float angle, uint8_t profile, float accel, float speed);
|
||||
|
||||
void runProfile(Trajectory profile) {
|
||||
if (profile == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
_trajectory = std::move(profile);
|
||||
_step = 0;
|
||||
_mode = Run;
|
||||
}
|
||||
|
||||
bool isIdle() {
|
||||
return _mode == Mode::Idle;
|
||||
}
|
||||
|
||||
float getScaledPos() {
|
||||
return _indexPos.getScaled();
|
||||
}
|
||||
|
||||
void resetPosition() {
|
||||
_indexPos = Position<50, 5>();
|
||||
}
|
||||
|
||||
private:
|
||||
Config &_config;
|
||||
StepperDriver &_driver;
|
||||
InputPort &_initSensor;
|
||||
HwTimerT<StepperMotor &> _timer;
|
||||
Mode _mode{};
|
||||
int _step{};
|
||||
Trajectory _trajectory{};
|
||||
Position<50, 5> _indexPos{};
|
||||
|
||||
static bool IRAM_ATTR TimerCallParam(HwTimerT<StepperMotor &> &, StepperMotor &stepper) {
|
||||
switch (stepper._mode) {
|
||||
case Mode::HomingFromSensor: {
|
||||
if (stepper._initSensor.read() == false) {
|
||||
stepper._mode = HomingToSensorBegin;
|
||||
break;
|
||||
}
|
||||
(void)stepper._driver.step(1);
|
||||
} break;
|
||||
|
||||
case Mode::HomingToSensorBegin: {
|
||||
if (stepper._initSensor.read()) {
|
||||
stepper._mode = HomingToSensorEnd;
|
||||
stepper._indexPos = {0, stepper._driver.getSubPos()};
|
||||
break;
|
||||
}
|
||||
(void)stepper._driver.step(1);
|
||||
} break;
|
||||
|
||||
case Mode::HomingToSensorEnd: {
|
||||
if (stepper._initSensor.read() == false) {
|
||||
stepper._mode = Homing;
|
||||
stepper._indexPos = stepper._indexPos.halv();
|
||||
break;
|
||||
}
|
||||
auto delta = stepper._driver.step(1);
|
||||
stepper._indexPos = stepper._indexPos + delta;
|
||||
} break;
|
||||
|
||||
case Mode::Homing: {
|
||||
if (stepper._indexPos.position == 0) {
|
||||
stepper._mode = Idle;
|
||||
break;
|
||||
}
|
||||
auto delta = stepper._driver.step(-1);
|
||||
stepper._indexPos = stepper._indexPos + delta;
|
||||
} break;
|
||||
|
||||
case Mode::Run: {
|
||||
auto stp = stepper._trajectory->steps[stepper._step];
|
||||
auto delta = stepper._driver.setPos(stp);
|
||||
stepper._indexPos = stepper._indexPos + delta;
|
||||
|
||||
stepper._step++;
|
||||
if (stepper._step >= stepper._trajectory->steps.size()) {
|
||||
stepper._mode = Idle;
|
||||
}
|
||||
} break;
|
||||
|
||||
case Mode::Idle:
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
};
|
||||
@@ -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