New: adding control servo-drive

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));
     }
 }