Update: replace board, add servo driver program

arduino_code/Test/src/mqtt_handler.cpp

@@ -64,10 +64,7 @@ static void reconnect() {
             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");
+            client.subscribe("servo/control/+/#");
         } else {
             vTaskDelay(pdMS_TO_TICKS(5000));
         }
@@ -112,20 +109,8 @@ 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();
-        }
+    } if (strncmp(topic, "servo/control", 12) == 0) {
+        handleServoMQTTCommand(topic, msg);
     }
 }
 
@@ -228,34 +213,85 @@ static void publishTelemetry() {
             }
         }
 
-        // Публикация угла (только при изменении)
-        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 checkAndPublishServoStatus(uint8_t channel) {
+    if (channel >= MAX_SERVOS) return;
+    
+    bool current_state = servo_enabled[channel];
+    if (current_state != last_published_status[channel]) {
+        last_published_status[channel] = current_state;
+        
+        char topic[64];
+        snprintf(topic, sizeof(topic), "servo/%d/feedback/status", channel);
+        
+        String status = current_state ? "on" : "off";
+        client.publish(topic, status.c_str());
+        
+        Serial.printf("Published servo %d status: %s\n", channel, status.c_str());
+    }
+}
+
+void checkAndPublishServoAngle(uint8_t channel) {
+    if (channel >= MAX_SERVOS) return;
+    
+    uint8_t current_angle = current_angles[channel];
+    if (current_angle != last_published_angles[channel]) {
+        last_published_angles[channel] = current_angle;
+        
+        char topic[64];
+        snprintf(topic, sizeof(topic), "servo/%d/feedback/angle", channel);
+        
+        char payload[8];
+        snprintf(payload, sizeof(payload), "%d", current_angle);
+        client.publish(topic, payload);
+        
+        Serial.printf("Published servo %d angle: %d\n", channel, current_angle);
+    }
+}
+
+void handleServoMQTTCommand(const char* topic, const char* payload) {
+    // Парсим топик: servo/control/{channel}/{command}
+    int channel = -1;
+    char command[32] = {0};
+    
+    if (sscanf(topic, "servo/control/%d/%s", &channel, command) != 2) {
+        Serial.printf("Invalid servo topic: %s\n", topic);
+        return;
+    }
+    
+    if (channel < 0 || channel >= MAX_SERVOS) {
+        Serial.printf("Invalid servo channel: %d\n", channel);
+        return;
+    }
+    
+    Serial.printf("Servo %d command: %s = %s\n", channel, command, payload);
+    
+    if (strcmp(command, "angle") == 0) {
+        int angle = atoi(payload);
+        if (angle >= 0 && angle <= 180) {
+            setServoAngle(channel, (uint8_t)angle);
+            checkAndPublishServoAngle(channel);
+            checkAndPublishServoStatus(channel);
+        }
+    }
+    else if (strcmp(command, "enable") == 0) {
+        bool enable = (strcmp(payload, "on") == 0 || strcmp(payload, "1") == 0 || strcmp(payload, "true") == 0);
+        if (enable) {
+            enableServo(channel);
+        } else {
+            disableServo(channel);
+        }
+        checkAndPublishServoStatus(channel);
+    }
+}
+
+/////
+
 void mqttTask(void *parameter) {
     static unsigned long last_stack_check = 0;
     setup_wifi();