New: adding control servo-drive

arduino_code/Test/src/main.cpp

@@ -14,7 +14,7 @@ void setup() {
     xTaskCreatePinnedToCore(
         mqttTask,
         "MQTT_Task",
-        8192,
+        20480,
         NULL,
         3,
         NULL,

arduino_code/Test/src/mqtt_handler.cpp

@@ -1,6 +1,7 @@
 #include "mqtt_handler.h"
 #include "config.h"
 #include "motor.h"
+#include "servo_control.h"
 #include <WiFi.h>
 #include <PubSubClient.h>
 
@@ -29,6 +30,11 @@ static int last_pub_driver_status = -1;
 static int last_pub_tmc_software_enable = -1;
 static uint8_t last_pub_current_scaling = 255;
 
+// Сервопривод
+static int last_pub_angle = -1;
+static int last_pub_enabled = -1;
+static int last_pub_attached = -1;
+
 static void setup_wifi() {
     Serial.print("Connecting to WiFi");
     WiFi.begin(WIFI_SSID, WIFI_PASS);
@@ -57,6 +63,11 @@ static void reconnect() {
             client.subscribe("motor/control/tmc/enable");
             client.subscribe("motor/control/tmc/stealthchop");
             client.subscribe("motor/control/tmc/coolstep");
+
+            client.subscribe("servo/control/angle");
+            client.subscribe("servo/control/pulse");
+            client.subscribe("servo/control/enable");
+            client.subscribe("servo/control/detach");
         } else {
             vTaskDelay(pdMS_TO_TICKS(5000));
         }
@@ -101,6 +112,20 @@ static void callback(char* topic, byte* payload, unsigned int length) {
     }
     else if (strcmp(topic, "motor/control/tmc/coolstep") == 0) {
         tmcSetCoolStep(strcmp(msg, "on") == 0);
+    } 
+    else if (strcmp(topic, "servo/control/angle") == 0) {
+        servoSetAngle(atoi(msg));
+    }
+    else if (strcmp(topic, "servo/control/pulse") == 0) {
+        servoSetPulse(atoi(msg));
+    }
+    else if (strcmp(topic, "servo/control/enable") == 0) {
+        servoEnable(strcmp(msg, "on") == 0);
+    }
+    else if (strcmp(topic, "servo/control/detach") == 0) {
+        if (strcmp(msg, "1") == 0 || strcmp(msg, "true") == 0) {
+            servoDetach();
+        }
     }
 }
 
@@ -202,19 +227,54 @@ static void publishTelemetry() {
                 last_pub_tmc_software_enable = tse;
             }
         }
+
+        // Публикация угла (только при изменении)
+        int current_angle = servoGetAngle();
+        if (current_angle != last_pub_angle)
+        {
+            client.publish("servo/feedback/angle", String(current_angle).c_str());
+            last_pub_angle = current_angle;
+        }
+
+        // Публикация статуса (только при изменении)
+        int enabled_int = servoIsEnabled() ? 1 : 0;
+        if (enabled_int != last_pub_enabled)
+        {
+            client.publish("servo/feedback/status", enabled_int ? "on" : "off");
+            last_pub_enabled = enabled_int;
+        }
+
+        // Публикация статуса (только при изменении)
+        int attached_int = servoIsAttached() ? 1 : 0;
+        if (attached_int != last_pub_attached)
+        {
+            client.publish("servo/feedback/status", attached_int ? "on" : "off");
+            last_pub_attached = attached_int;
+        }
+
         last_feedback_time = now;
     }
 }
 
 void mqttTask(void *parameter) {
+    static unsigned long last_stack_check = 0;
     setup_wifi();
     client.setServer(MQTT_SERVER, MQTT_PORT);
     client.setCallback(callback);
 
+    servoInit();
+
     for (;;) {
         if (!client.connected()) reconnect();
         client.loop();
         publishTelemetry();
+
+        // Вывод свободного стека каждые 10 секунд
+        if (millis() - last_stack_check > 10000) {
+            Serial.printf("[STACK] Free: %u bytes\n", uxTaskGetStackHighWaterMark(NULL));
+            last_stack_check = millis();
+        }
+
         vTaskDelay(pdMS_TO_TICKS(10));
     }
 }

arduino_code/Test/src/servo_control.cpp

@@ -0,0 +1,93 @@
+#include "servo_control.h"
+
+static Servo servo;
+static int current_angle = 90;          // Стартовое положение — центр
+static bool servo_enabled = false;
+static bool servo_attached = false;
+
+// ======================================================
+// Инициализация
+// ======================================================
+void servoInit() {
+    // Разрешаем таймеры ESP32 для работы с серво
+    // ESP32PWM::allocateTimer(0);
+    // ESP32PWM::allocateTimer(1);
+    
+    servo.setPeriodHertz(50);                   // Стандартные 50 Гц для серво
+    servo.attach(SERVO_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
+    servo_attached = true;
+    
+    // Устанавливаем стартовое положение
+    servo.write(current_angle);
+    servo_enabled = true;
+    
+    Serial.printf("[SERVO] Initialized on pin %d, angle=%d°\n", SERVO_PIN, current_angle);
+}
+
+// ======================================================
+// Управление
+// ======================================================
+void servoSetAngle(int angle) {
+    // Ограничиваем диапазон 0..180
+    if (angle < 0) angle = 0;
+    if (angle > 180) angle = 180;
+    
+    current_angle = angle;
+    
+    if (servo_attached && servo_enabled) {
+        servo.write(current_angle);
+    }
+    Serial.printf("[SERVO] Angle set to %d°\n", current_angle);
+}
+
+void servoSetPulse(uint16_t pulse_us) {
+    if (!servo_attached) return;
+    
+    // Ограничиваем диапазон импульса
+    if (pulse_us < SERVO_MIN_PULSE) pulse_us = SERVO_MIN_PULSE;
+    if (pulse_us > SERVO_MAX_PULSE) pulse_us = SERVO_MAX_PULSE;
+    
+    servo.writeMicroseconds(pulse_us);
+    
+    // Пересчитываем угол для телеметрии (линейная аппроксимация)
+    current_angle = map(pulse_us, SERVO_MIN_PULSE, SERVO_MAX_PULSE, 0, 180);
+    Serial.printf("[SERVO] Pulse set to %d µs (~%d°)\n", pulse_us, current_angle);
+}
+
+void servoEnable(bool enable) {
+    servo_enabled = enable;
+    
+    if (servo_attached) {
+        if (servo_enabled) {
+            servo.attach(SERVO_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
+            servo.write(current_angle);
+            Serial.println("[SERVO] Enabled");
+        } else {
+            servo.detach();
+            Serial.println("[SERVO] Disabled (detached)");
+        }
+    }
+}
+
+void servoDetach() {
+    if (servo_attached) {
+        servo.detach();
+        servo_attached = false;
+        Serial.println("[SERVO] Detached");
+    }
+}
+
+// ======================================================
+// Получение состояния
+// ======================================================
+int servoGetAngle() {
+    return current_angle;
+}
+
+bool servoIsEnabled() {
+    return servo_enabled;
+}
+
+bool servoIsAttached() {
+    return servo_attached;
+}

arduino_code/Test/src/servo_control.h

@@ -0,0 +1,21 @@
+#include <Arduino.h>
+#include <ESP32Servo.h>
+
+// --- Конфигурация ---
+#define SERVO_PIN           19
+#define SERVO_MIN_PULSE     600     // мкс (соответствует 0°)
+#define SERVO_MAX_PULSE     2400    // мкс (соответствует 180°)
+
+// --- Инициализация ---
+void servoInit();
+
+// --- Управление ---
+void servoSetAngle(int angle);          // 0..180
+void servoSetPulse(uint16_t pulse_us);  // 500..2500 мкс
+void servoEnable(bool enable);
+void servoDetach();
+
+// --- Получение состояния ---
+int  servoGetAngle();
+bool servoIsEnabled();
+bool servoIsAttached();