139 lines
4.0 KiB
C++
139 lines
4.0 KiB
C++
/**
|
|
* @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);
|
|
|
|
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]);
|
|
}
|