fix: Debugging motor movements

2025-04-02 15:06:25 +02:00
parent 4106bcc67c
commit 5ca595eddc
11 changed files with 2584 additions and 3 deletions
@@ -0,0 +1,4 @@
+idf_component_register(
+    INCLUDE_DIRS "."
+    REQUIRES devHal
+)
@@ -0,0 +1,47 @@
+#pragma once
+
+#include "StepperDrv8833.hpp"
+#include "stepperMotor.hpp"
+#include <cmath>
+
+class WatchMotor {
+    static constexpr uint8_t Positions = 6;
+
+  public:
+    WatchMotor(StepperMotor &stepper, OutputPort &sleep)
+        : _stepper(stepper),
+          _motorSleep(sleep) {
+        _motorSleep.write(false);
+    }
+
+    void moveTo(uint8_t sign, uint8_t profile = 0) {
+        sign = sign % 6;
+
+        float delta = 2.0F * std::numbers::pi;
+        float corr = 0.0F;
+        float currentPos = _stepper.getScaledPos() * 6;
+        if ((_currentIndex > 4) && (currentPos < 2)) {
+            corr = 6 - _currentIndex - currentPos;
+        } else if ((_currentIndex < 2) && (currentPos > 2)) {
+            corr = _currentIndex + 6 - currentPos;
+        } else {
+            corr = _currentIndex - currentPos;
+        }
+
+        // stepper.getScaledPos() * 6
+        if (_currentIndex < sign) {
+            delta = delta / 6 * (sign - _currentIndex);
+        } else if (_currentIndex > sign) {
+            delta = delta / 6 * (sign + 6 - _currentIndex);
+        }
+        ESP_LOGD("MV", "Moving to %d by %.2f + %.2f", sign, delta, corr);
+
+        _stepper.moveBy(delta + corr, profile);
+        _currentIndex = sign;
+    }
+
+  private:
+    StepperMotor &_stepper;
+    OutputPort &_motorSleep;
+    uint8_t _currentIndex{};
+};