PWM based motor driver added from other project

2024-11-16 20:04:38 +01:00
parent f6608075ff
commit 0181c72a9d
17 changed files with 1288 additions and 6 deletions
@@ -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]);
+}